align_vectors to more robust method and tests

tests/test_vector.py

@@ -45,6 +45,57 @@ def test_hemisphere(self):
             assert g.np.allclose(v, a * s.reshape((-1, 1)))
 
 
+class AlignTests(g.unittest.TestCase):
+
+    def test_align(self):
+        """
+        Test aligning two 3D vectors
+        """
+
+        # function we're testing
+        align = g.trimesh.geometry.align_vectors
+
+        # start with some edge cases and make sure the transform works
+        target = g.np.array([0, 0, 1], dtype=g.np.float64)
+        vectors = g.np.vstack((g.trimesh.unitize(g.np.random.random((1000, 3)) - .5),
+                               [-target, target],
+                               g.trimesh.util.generate_basis(target)))
+        for vector in vectors:
+            T, a = align(vector, target, return_angle=True)
+            result = g.np.dot(T, g.np.append(vector, 1))[:3]
+            aligned = g.np.linalg.norm(result - target) < 1e8
+            assert aligned
+
+        # these vectors should be perpendicular and zero
+        angles = [align(i, target, return_angle=True)[1]
+                  for i in g.trimesh.util.generate_basis(target)]
+        assert g.np.allclose(angles, [g.np.pi / 2, g.np.pi / 2, 0.0])
+
+        # generate angles from 0 to 180 degrees
+        angles = g.np.linspace(0.0, g.np.pi, 1000)
+        # generate on- plane vectors
+        vectors = g.np.column_stack((g.np.cos(angles),
+                                     g.np.sin(angles),
+                                     g.np.zeros(len(angles))))
+
+        # rotate them arbitrarily off the plane just for funsies
+        vectors = g.trimesh.transform_points(vectors,
+                                             g.transforms[20])
+
+        for angle, vector in zip(angles, vectors):
+            # check alignment to first vector
+            # which was created with zero angle
+            T, a = align(vector, vectors[0], return_angle=True)
+
+            # check to make sure returned angle corresponds with truth
+            assert g.np.isclose(a, angle)
+
+            # check to make sure returned transform is correct
+            check = g.np.dot(T, g.np.append(vector, 1))[:3]
+            aligned = g.np.linalg.norm(check - vector[0]) < 1e8
+            assert aligned
+
+
 if __name__ == '__main__':
     g.trimesh.util.attach_to_log()
     g.unittest.main()

trimesh/geometry.py

@@ -34,62 +34,90 @@ def plane_transform(origin, normal):
     return transform
 
 
-def align_vectors(vector_start, vector_end, return_angle=False):
+def align_vectors(a, b, return_angle=False):
     """
-    Returns the 4x4 transformation matrix which will rotate from
-    vector_start to vector_end, eg:
-
-    vector_end == np.dot(T, np.append(vector_start, 1))[0:3]
+    Find a transform between two 3D vectors.
 
+    Implements the method described here:
+    http://ethaneade.com/rot_between_vectors.pdf
 
     Parameters
-    -----------
-    vector_start: (3,) float, vector in space
-    vector_end:   (3,) float, vector in space
-    return_angle: bool, return angle between vectors or not
+    --------------
+    a : (3,) float
+      Source vector
+    b : (3,) float
+      Target vector
+    return_angle : bool
+      If True return the angle between the two vectors
 
     Returns
-    -----------
-    transform: (4,4) float, transformation matrix
-    angle:     float, angle in radians (only returned if flag set)
-
+    -------------
+    transform : (4, 4) float
+      Homogenous transform from a to b
+    angle : float
+      Angle between vectors in radians
+      Only returned if return_angle
     """
-    # convert start and end to (3, ) float unit vectors
-    start = np.asanyarray(vector_start,
-                          dtype=np.float64).reshape(3)
-    start /= np.linalg.norm(start)
-    end = np.asanyarray(vector_end,
-                        dtype=np.float64).reshape(3)
-    end /= np.linalg.norm(end)
-
-    # get a unit vector perpendicular to both vectors
-    # this will be the axis we are rotating around
-    cross = np.cross(start, end)
-    # we clip the norm to 1, as otherwise floating point bs
-    # can cause the arcsin to error
-    norm = np.linalg.norm(cross)
-    norm = np.clip(norm, -1.0, 1.0)
-    direction = np.sign(np.dot(start, end))
-
-    if norm < tol.zero:
-        # if the norm is zero, the vectors are the same
-        # and no rotation is needed
-        T = np.eye(4)
-        T[0:3] *= direction
+    # copy of input vectors
+    a = np.array(a, dtype=np.float64, copy=True)
+    b = np.array(b, dtype=np.float64, copy=True)
+
+    # make sure vectors are 3D
+    if a.shape != (3,) or b.shape != (3,):
+        raise ValueError('only works for (3,) vectors')
+
+    # unitize input vectors
+    a /= np.linalg.norm(a)
+    b /= np.linalg.norm(b)
+
+    # projection of a onto b
+    dot = np.dot(a, b)
+
+    # are vectors just reversed
+    if dot < (tol.zero - 1):
+        # a reversed vector is 180 degrees
+        angle = np.pi
+
+        # get an arbitrary perpendicular vector to a
+        perp = util.generate_basis(a)[0] * np.eye(3)
+
+        # (3, 3) rotation from a to b
+        rotation = (2 * np.dot(perp, perp.T)) - np.eye(3)
+
+    # are vectors already the same
+    elif dot > (1 - tol.zero):
         angle = 0.0
+        # no rotation
+        rotation = np.eye(3)
+
+    # vectors are at some angle to each other
     else:
-        angle = np.arcsin(norm)
-        if direction < 0:
-            angle = np.pi - angle
-        T = rotation_matrix(angle, cross)
+        # we already handled values out of the range [-1.0, 1.0]
+        angle = np.arccos(dot)
+
+        # (3,) vector perpendicular to both a and b
+        w = np.cross(a, b)
+
+        # a scalar
+        c = 1.0 / (1.0 + dot)
 
-    check = np.abs(np.dot(T[:3, :3], start) - end)
-    if not (check < 1e-5).all():
-        raise ValueError('aligning vectors failed!')
+        # (3, 3) skew- symmetric matrix from the (3,) vector w
+        # the matrix has the property: wx == -wx.T
+        wx = np.array([[0, -w[2], w[1]],
+                       [w[2], 0, -w[0]],
+                       [-w[1], w[0], 0]])
+
+        # (3, 3) rotation from a to b
+        rotation = np.eye(3) + wx + (np.dot(wx, wx) * c)
+
+    # put rotation into homogenous transformation matrix
+    transform = np.eye(4)
+    transform[:3, :3] = rotation
 
     if return_angle:
-        return T, angle
-    return T
+        return transform, angle
+
+    return transform
 
 
 def faces_to_edges(faces, return_index=False):

trimesh/util.py

@@ -1816,19 +1816,24 @@ def generate_basis(z):
 
     Returns
     -------
-    x: (3,) float, the x axis
-    y: (3,) float, the y axis
-    z: (3,) float, the z axis
+    x : (3,) float
+      Vector along x axis
+    y : (3,) float
+      Vector along y axis
+    z : (3,) float
+      Vector along z axis
     """
     z = z / np.linalg.norm(z)
+
     x = np.array([-z[1], z[0], 0.0])
-    if np.linalg.norm(x) == 0.0:
+    if np.isclose(np.linalg.norm(x), 0.0):
         x = np.array([1.0, 0.0, 0.0])
     x = x / np.linalg.norm(x)
     y = np.cross(z, x)
     result = np.array([x, y, z])
     return result
 
+
 def unique_bincount(values,
                     minlength,
                     return_inverse=True):

trimesh/version.py

@@ -1 +1 @@
-__version__ = '2.35.41'
+__version__ = '2.35.42'