@@ -725,6 +725,48 @@ def vval(v):
725725
726726 return cls (np .c_ [x , y , z ], check = True )
727727
728+ @classmethod
729+ def RotatedVector (cls , v1 : ArrayLike3 , v2 : ArrayLike3 , tol = 20 ) -> Self :
730+ """
731+ Construct a new SO(3) from a vector and its rotated image
732+
733+ :param v1: initial vector
734+ :type v1: array_like(3)
735+ :param v2: vector after rotation
736+ :type v2: array_like(3)
737+ :param tol: tolerance for singularity in units of eps, defaults to 20
738+ :type tol: float
739+ :return: SO(3) rotation
740+ :rtype: :class:`SO3` instance
741+
742+ ``SO3.RotatedVector(v1, v2)`` is an SO(3) rotation defined in terms of
743+ two vectors. The rotation takes vector ``v1`` to ``v2``.
744+
745+ .. runblock:: pycon
746+
747+ >>> from spatialmath import SO3
748+ >>> v1 = [1, 2, 3]
749+ >>> v2 = SO3.Eul(0.3, 0.4, 0.5) * v1
750+ >>> print(v2)
751+ >>> R = SO3.RotatedVector(v1, v2)
752+ >>> print(R)
753+ >>> print(R * v1)
754+
755+ .. note:: The vectors do not have to be unit-length.
756+ """
757+ # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
758+ v1 = smb .unitvec (v1 )
759+ v2 = smb .unitvec (v2 )
760+ v = smb .cross (v1 , v2 )
761+ s = smb .norm (v )
762+ if abs (s ) < tol * np .finfo (float ).eps :
763+ return cls (np .eye (3 ), check = False )
764+ else :
765+ c = np .dot (v1 , v2 )
766+ V = smb .skew (v )
767+ R = np .eye (3 ) + V + V @ V * (1 - c ) / (s ** 2 )
768+ return cls (R , check = False )
769+ 728770 @classmethod
729771 def AngleAxis (cls , theta : float , v : ArrayLike3 , * , unit : str = "rad" ) -> Self :
730772 r"""
0 commit comments