new method to specify rotation in terms of directions of new frame wrt old

petercorke · petercorke · commit 53fedf96d003 · 2021-11-02T21:25:14.000+10:00
diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
@@ -536,6 +536,69 @@ def OA(cls, o, a):
         """
         return cls(base.oa2r(o, a), check=False)
 
+    @classmethod
+    def TwoVectors(cls, x=None, y=None, z=None):
+        """
+        Construct a new SO(3) from any two vectors
+
+        :param x: new x-axis, defaults to None
+        :type x: str, array_like(3), optional
+        :param y: new y-axis, defaults to None
+        :type y: str, array_like(3), optional
+        :param z: new z-axis, defaults to None
+        :type z: str, array_like(3), optional
+
+        Create a rotation by defining the direction of two of the new
+        axes in terms of the old axes.  Axes are denoted by strings ``"x"``, 
+        ``"y"``, ``"z"``, ``"-x"``, ``"-y"``, ``"-z"``.
+
+        The directions can also be specified by 3-element vectors, but these
+        must be orthogonal.
+
+        To create a rotation where the new frame has its x-axis in -z-direction
+        of the previous frame, and its z-axis in the x-direction of the previous
+        frame is::
+        
+            >>> SO3.TwoVectors(x='-z', z='x')
+        """
+        def vval(v):
+            if isinstance(v, str):
+                sign = 1
+                if v[0] == '-':
+                    sign = -1
+                    v = v[1:] # skip sign char
+                elif v[0] == '+':
+                    v = v[1:] # skip sign char
+                if v[0] == 'x':
+                    v = [sign, 0, 0]
+                elif v[0] == 'y':
+                    v = [0, sign, 0]
+                elif v[0] == 'z':
+                    v = [0, 0, sign]
+                return np.r_[v]
+            else:
+                return base.unitvec(base.getvector(v, 3))
+
+        if x is not None and y is not None and z is None:
+            # z = x x y
+            x = vval(x)
+            y = vval(y)
+            z = np.cross(x, y)
+
+        elif x is None and y is not None and z is not None:
+            # x = y x z
+            y = vval(y)
+            z = vval(z)
+            x = np.cross(y, z)
+
+        elif x is not None and y is None and z is not None:
+            # y = z x x
+            z = vval(z)
+            x = vval(x)
+            y = np.cross(z, x)
+
+        return cls(np.c_[x, y, z], check=False)
+
     @classmethod
     def AngleAxis(cls, theta, v, *, unit='rad'):
         r"""
