fix: SO3.RotatedVector returned identity for antipodal vectors

SO3.RotatedVector(v1, v2) computes the rotation axis as v1 x v2. That
cross product is zero whenever v1 and v2 are (anti)parallel, and the
existing singularity check treated both cases identically -- returning
identity regardless. Correct only when v1 and v2 point the same way;
when they're antipodal (v2 == -v1), the correct answer is a 180 degree
rotation about any axis perpendicular to v1, not identity.

Distinguish the two cases via the sign of v1.v2 at the singularity.
The antipodal case uses the closed form for a 180 degree rotation
about a unit axis u (R = 2*u*u^T - I), with u found by crossing v1
against whichever world axis it's least aligned with.

Found while building spatialgeometry's Arrow.FromTo(start, end), which
needs exactly this "rotate a reference axis to point along an
arbitrary direction" operation and hit the bug immediately for an
arrow pointing straight backward relative to its own rest orientation.

Author: petercorke

spatialmath/pose3d.py

@@ -769,7 +769,24 @@ def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self:
         v = smb.cross(v1, v2)
         s = smb.norm(v)
         if abs(s) < tol * np.finfo(float).eps:
-            return cls(np.eye(3), check=False)
+            c = np.dot(v1, v2)
+            if c > 0:
+                # v1 and v2 already (anti)parallel in the same direction
+                return cls(np.eye(3), check=False)
+            # v1 and v2 point in opposite directions -- the formula below
+            # is singular here too (it divides by s**2), but unlike the
+            # c > 0 case the answer isn't identity: any 180 degree
+            # rotation about an axis perpendicular to v1 takes v1 to v2.
+            # Closed form for a 180 degree rotation about unit axis u:
+            # R = 2*u*u^T - I (Rodrigues at theta=pi, sin=0, cos=-1).
+            # Pick u by crossing v1 with whichever world axis it's least
+            # aligned with, so the cross product is never itself
+            # degenerate.
+            axis = np.zeros(3)
+            axis[np.argmin(np.abs(v1))] = 1.0
+            u = smb.unitvec(smb.cross(v1, axis))
+            R = 2 * np.outer(u, u) - np.eye(3)
+            return cls(R, check=False)
         else:
             c = np.dot(v1, v2)
             V = smb.skew(v)

tests/test_pose3d.py

@@ -726,6 +726,19 @@ def test_rotatedvector(self):
         Re = SO3.RotatedVector(v1, v1)
         np.testing.assert_almost_equal(Re, np.eye(3))
 
+        # Antipodal case: v1 and v2 point in exactly opposite directions.
+        # The cross product used to find the rotation axis is zero here
+        # too, same as the parallel case above, but the correct answer is
+        # a 180 degree flip, not identity -- regression test for a bug
+        # where this silently returned identity instead.
+        for v1 in ([1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 2, 3], [-3, 1, 2]):
+            v1 = unitvec(v1)
+            v2 = [-x for x in v1]
+            Re = SO3.RotatedVector(v1, v2)
+            np.testing.assert_almost_equal(np.asarray(Re * v1).flatten(), v2)
+            # must actually be a 180 degree rotation, not identity
+            assert not np.allclose(Re.A, np.eye(3))
+
         R = SO3()  # identity matrix case
 
         # Check log and exponential map