From 0e771c3fae06b73ca7b11bc87448423a43991162 Mon Sep 17 00:00:00 2001
From: Peter Corke <peter.i.corke@gmail.com>
Date: Tue, 21 Jan 2025 07:10:14 +1000
Subject: [PATCH] added method to construct an SO3 object from a vector and its
 rotated image.

---
 spatialmath/pose3d.py | 42 ++++++++++++++++++++++++++++++++++++++++++
 tests/test_pose3d.py  | 10 ++++++++++
 2 files changed, 52 insertions(+)

diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
index b4301d93..103def47 100644
--- a/spatialmath/pose3d.py
+++ b/spatialmath/pose3d.py
@@ -725,6 +725,48 @@ def vval(v):
 
         return cls(np.c_[x, y, z], check=True)
 
+    @classmethod
+    def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self:
+        """
+        Construct a new SO(3) from a vector and its rotated image
+
+        :param v1: initial vector
+        :type v1: array_like(3)
+        :param v2: vector after rotation
+        :type v2: array_like(3)
+        :param tol: tolerance for singularity in units of eps, defaults to 20
+        :type tol: float
+        :return: SO(3) rotation
+        :rtype: :class:`SO3` instance
+
+        ``SO3.RotatedVector(v1, v2)`` is an SO(3) rotation defined in terms of
+        two vectors. The rotation takes vector ``v1`` to ``v2``.
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import SO3
+            >>> v1 = [1, 2, 3]
+            >>> v2 = SO3.Eul(0.3, 0.4, 0.5) * v1
+            >>> print(v2)
+            >>> R = SO3.RotatedVector(v1, v2)
+            >>> print(R)
+            >>> print(R * v1)
+
+        .. note:: The vectors do not have to be unit-length.
+        """
+        # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
+        v1 = smb.unitvec(v1)
+        v2 = smb.unitvec(v2)
+        v = smb.cross(v1, v2)
+        s = smb.norm(v)
+        if abs(s) < tol * np.finfo(float).eps:
+            return cls(np.eye(3), check=False)
+        else:
+            c = np.dot(v1, v2)
+            V = smb.skew(v)
+            R = np.eye(3) + V + V @ V * (1 - c) / (s**2)
+            return cls(R, check=False)
+
     @classmethod
     def AngleAxis(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> Self:
         r"""
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
index d6a941c3..f5fe286a 100755
--- a/tests/test_pose3d.py
+++ b/tests/test_pose3d.py
@@ -717,6 +717,16 @@ def test_functions_lie(self):
         nt.assert_equal(R, SO3.EulerVec(R.eulervec()))
         np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3))
 
+    def test_rotatedvector(self):
+        v1 = [1, 2, 3]
+        R = SO3.Eul(0.3, 0.4, 0.5)
+        v2 = R * v1
+        Re = SO3.RotatedVector(v1, v2)
+        np.testing.assert_almost_equal(v2, Re * v1)
+
+        Re = SO3.RotatedVector(v1, v1)
+        np.testing.assert_almost_equal(Re, np.eye(3))
+
 
 # ============================== SE3 =====================================#