perf(base): speed up trnorm and tr2adjoint

spatialmath/base/transforms3d.py

@@ -1591,17 +1591,36 @@ def trnorm(T: SE3Array) -> SE3Array:
     if not ishom(T) and not isrot(T):
         raise ValueError("expecting SO(3) or SE(3)")
 
-    o = T[:3, 1]
-    a = T[:3, 2]
+    # explicit scalar arithmetic avoids the generic-dispatch overhead of
+    # np.cross/unitvec/np.stack (built for arbitrary shapes/broadcasting),
+    # which dominates cost for a fixed 3x3 rotation submatrix
+    o0, o1, o2 = T[0, 1], T[1, 1], T[2, 1]
+    a0, a1, a2 = T[0, 2], T[1, 2], T[2, 2]
 
-    n = np.cross(o, a)  # N = O x A
-    o = np.cross(a, n)  # (a)];
-    R = np.stack((unitvec(n), unitvec(o), unitvec(a)), axis=1)
+    # n = o x a
+    n0 = o1 * a2 - o2 * a1
+    n1 = o2 * a0 - o0 * a2
+    n2 = o0 * a1 - o1 * a0
 
-    if ishom(T):
-        return rt2tr(cast(SO3Array, R), T[:3, 3])
-    else:
-        return R
+    # o = a x n, to re-orthogonalize
+    o0, o1, o2 = a1 * n2 - a2 * n1, a2 * n0 - a0 * n2, a0 * n1 - a1 * n0
+
+    n_norm = (n0 * n0 + n1 * n1 + n2 * n2) ** 0.5
+    o_norm = (o0 * o0 + o1 * o1 + o2 * o2) ** 0.5
+    a_norm = (a0 * a0 + a1 * a1 + a2 * a2) ** 0.5
+
+    is_hom = ishom(T)
+    R = np.empty((4, 4) if is_hom else (3, 3))
+    R[0, 0], R[1, 0], R[2, 0] = n0 / n_norm, n1 / n_norm, n2 / n_norm
+    R[0, 1], R[1, 1], R[2, 1] = o0 / o_norm, o1 / o_norm, o2 / o_norm
+    R[0, 2], R[1, 2], R[2, 2] = a0 / a_norm, a1 / a_norm, a2 / a_norm
+
+    if is_hom:
+        R[3, :3] = 0.0
+        R[3, 3] = 1.0
+        R[:3, 3] = T[:3, 3]
+
+    return cast(SE3Array, R)
 
 
 @overload
@@ -2718,20 +2737,21 @@ def tr2adjoint(T):
     :SymPy: supported
     """
 
-    Z = np.zeros((3, 3), dtype=T.dtype)
     if T.shape == (3, 3):
         # SO(3) adjoint
         R = T
         return R
     elif T.shape == (4, 4):
         # SE(3) adjoint
         (R, t) = tr2rt(T)
-        # fmt: off
-        return np.block([
-                    [R, skew(t) @ R],
-                    [Z, R]
-                ])
-        # fmt: on
+        # direct block assignment avoids np.block's generic nested-list
+        # assembly overhead, which dominates cost for this fixed 6x6-from-
+        # four-3x3 layout; dtype preserved for SymPy support
+        A = np.zeros((6, 6), dtype=T.dtype)
+        A[:3, :3] = R
+        A[:3, 3:] = skew(t) @ R
+        A[3:, 3:] = R
+        return A
     else:
         raise ValueError("bad argument")