fix: SO3.RotatedVector returned identity for antipodal vectors (#201)

## Summary

`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 (`abs(s) < tol * eps`) treats both cases
identically -- returning identity regardless:

```python
>>> from spatialmath import SO3
>>> SO3.RotatedVector([0, 0, 1], [0, 0, -1])  # should flip, not be identity
   1         0         0
   0         1         0
   0         0         1
```

That's correct when `v1` and `v2` point the same way, but wrong when
they're antipodal (`v2 == -v1`) -- the correct answer there is a 180°
rotation about any axis perpendicular to `v1`, not identity.

- [x] Distinguish the two cases via the sign of `v1 . v2` at the
  singularity.
- [x] The antipodal case uses the closed form for a 180° 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 (so the cross
  product used to find `u` is never itself degenerate).
- [x] Existing test coverage
(`tests/test_pose3d.py::test_rotatedvector`)
  only covered the general and exact-same-direction cases -- added the
  antipodal case there, parametrised over several different `v1` values.

Found this while building a `FromTo(start, end)` constructor for an
`Arrow` shape in a downstream package, which needs exactly this "rotate
a
reference axis to point along an arbitrary direction" operation and hit
the bug immediately for a direction pointing straight backward relative
to the reference axis.

## Test plan

- [x] Full suite: `pytest tests/` -- 342 passed
- [x] New antipodal-case test passes; confirmed it fails against the old
  code (returns identity instead of a real rotation)
- [x] Confirmed the general-rotation and exact-same-direction cases are
  unaffected (existing test assertions unchanged)

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

@@ -731,6 +731,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