perf(base): speed up isskewa and cache identity matrix in trexp/trlog/rodrigues

trexp/trlog don't have one dominant wasteful generic call like the
isR/trnorm/tr2adjoint/qqmul/qvmul fixes did (#213/#214/#215) - they're
deep call chains (trexp -> isskewa -> vexa -> iszerovec ->
unittwist_norm -> rodrigues -> skew -> rt2tr -> ishom -> isR) where
cost is spread thin across many small layers. Two targeted, verified
wins pulled out of that chain:

- isskewa (validity check trexp runs on se(3) input) had the same bug
  ishom had pre-#213: np.linalg.norm on a small fixed matrix, plus an
  all(S[-1,:] == 0) array-allocation-and-compare for the bottom row.
  Fixed the same way, ~1.9x faster standalone.
- np.eye(3) was rebuilt from scratch on every call in rodrigues (1x),
  trexp's V-matrix construction (1x), and trlog (2x), despite only
  ever being used as a read-only operand in an addition/multiplication
  that produces a new array. Replaced with a module-level constant
  _EYE3, used only at call sites where it's provably never mutated or
  returned directly (an aliasing hazard if it were) - the four
  zero-motion/zero-rotation early-return `np.eye(N)` calls in
  rodrigues/trexp are untouched, left as fresh arrays, since they hand
  the object directly to the caller.

isskew (the plain, non-augmented so(n) check) was also tried with the
same explicit-arithmetic treatment as isskewa, but did NOT show a
reliable win under min-of-repeats benchmarking - the only cost it
avoids is np.linalg.norm on an already-cheap `S + S.T`, not enough
margin to reliably beat by hand-unrolling. Reverted to the original
implementation rather than keep an unproven "fix".

Net effect, measured old-vs-new in the same process (min of 9 repeats
each, to suppress system jitter after an earlier round of misleading
single-run numbers): isskewa ~1.9x, rodrigues ~1.13x, trexp ~1.05x,
trlog ~1.10x. Real but modest - see the PR description's "further
speedup opportunities" section for what a bigger win here would
actually require.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

Author: petercorke

spatialmath/base/transforms3d.py

@@ -52,6 +52,11 @@
 
 _eps = np.finfo(np.float64).eps
 
+# read-only constant: safe to use directly as an operand in expressions that
+# produce a new array (e.g. `_EYE3 + ...`), never in a context that could
+# mutate it in place or return it directly to a caller
+_EYE3 = np.eye(3)
+
 # ---------------------------------------------------------------------------------------#
 
 
@@ -1355,7 +1360,7 @@ def trlog(
         else:
             # general case
             Ginv = (
-                np.eye(3)
+                _EYE3
                 - S / 2
                 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S
             )
@@ -1374,8 +1379,7 @@ def trlog(
             diagonal = R.diagonal()
             k = diagonal.argmax()
             mx = diagonal[k]
-            I = np.eye(3)
-            col = R[:, k] + I[:, k]
+            col = R[:, k] + _EYE3[:, k]
             w = col / np.sqrt(2 * (1 + mx))
             theta = math.pi
             if twist:
@@ -1514,7 +1518,7 @@ def trexp(S, theta=None, check=True):
 
         skw = skew(w)
         V = (
-            np.eye(3) * theta
+            _EYE3 * theta
             + (1.0 - math.cos(theta)) * skw
             + (theta - math.sin(theta)) * skw @ skw
         )
@@ -2774,7 +2778,7 @@ def rodrigues(w: ArrayLike3, theta: Optional[float] = None) -> SO3Array:
 
     skw = skew(cast(ArrayLike3, w))
     return (
-        np.eye(skw.shape[0])
+        _EYE3
         + math.sin(theta) * skw
         + (1.0 - math.cos(theta)) * skw @ skw
     )

spatialmath/base/transformsNd.py

@@ -408,6 +408,11 @@ def isskew(S: NDArray, tol: float = 20) -> bool:  # -> TypeGuard[sonArray]:
 
     :seealso: isskewa
     """
+    # NB: unlike isR/isskewa, an explicit-arithmetic fast path here did not
+    # show a reliable win under careful (min-of-repeats) benchmarking - the
+    # only overhead being avoided is np.linalg.norm on an already-cheap
+    # `S + S.T`, not enough to reliably beat the scalar-indexing cost of
+    # unrolling it by hand. Left as the original implementation.
     return bool(np.linalg.norm(S + S.T) < tol * _eps)
 
 
@@ -436,9 +441,42 @@ def isskewa(S: NDArray, tol: float = 20) -> bool:  # -> TypeGuard[senArray]:
 
     :seealso: isskew
     """
-    return bool(np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps) and all(
-        S[-1, :] == 0
-    )
+    n = S.shape[0]
+    if n == 4:
+        # explicit sum-of-squares + scalar bottom-row check avoids the
+        # generic-dispatch overhead of np.linalg.norm and the array
+        # allocation + all() of the bottom-row comparison, same as isR/ishom
+        r00 = S[0, 0] + S[0, 0]
+        r01 = S[0, 1] + S[1, 0]
+        r02 = S[0, 2] + S[2, 0]
+        r11 = S[1, 1] + S[1, 1]
+        r12 = S[1, 2] + S[2, 1]
+        r22 = S[2, 2] + S[2, 2]
+        resid = r00 * r00 + r11 * r11 + r22 * r22 + 2.0 * (
+            r01 * r01 + r02 * r02 + r12 * r12
+        )
+        return bool(
+            resid < (tol * _eps) ** 2
+            and S[3, 0] == 0
+            and S[3, 1] == 0
+            and S[3, 2] == 0
+            and S[3, 3] == 0
+        )
+    elif n == 3:
+        r00 = S[0, 0] + S[0, 0]
+        r01 = S[0, 1] + S[1, 0]
+        r11 = S[1, 1] + S[1, 1]
+        resid = r00 * r00 + r11 * r11 + 2.0 * r01 * r01
+        return bool(
+            resid < (tol * _eps) ** 2
+            and S[2, 0] == 0
+            and S[2, 1] == 0
+            and S[2, 2] == 0
+        )
+    else:
+        return bool(
+            np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps
+        ) and all(S[-1, :] == 0)
 
 
 def iseye(S: NDArray, tol: float = 20) -> bool: