Repository: mikedh/trimesh

Issue:
trimesh.geometry.align_vectors returns invalid rigid transform
Related to #540, it seems like there is still an issue where the method returns an invalid rigid transform when passed input that is close to the reverse vector. An example:
```python
In [1]: import trimesh
In [2]: import numpy as np
In [3]: a = trimesh.geometry.align_vectors([0,0,-1], [0,0,1])
In [4]: b = trimesh.geometry.align_vectors([0,0,-1], [-1e-17, 1e-17, 1])
In [5]: np.linalg.det(a)
Out[5]: 1.0
In [6]: np.linalg.det(b)
Out[6]: -1.0
```
Note that this still occurs with inputs that aren't quite as small (like 1e-4):
```python
In [7]: b = trimesh.geometry.align_vectors([0,0,-1], [-1e-4, 1e-4, 1])
In [8]: np.linalg.det(b)
Out[8]: -0.9999999999999999
```
It does work for the case in #540 though (the same one in the unit tests):
```python
In [9]: v1 = np.array([ 7.12106798e-07, -7.43194705e-08,  1.00000000e+00])
In [10]: c = trimesh.geometry.align_vectors([0,0,-1], v1)
In [11]: np.linalg.det(c)
Out[11]: 1.0000000000000002
```
Any thoughts on this? I'm not sure that I 100% understand the SVD trick.

Discussion:

Patch:
diff --git a/trimesh/geometry.py b/trimesh/geometry.py
index 10d10d29..6db8533d 100644
--- a/trimesh/geometry.py
+++ b/trimesh/geometry.py
@@ -1,7 +1,7 @@
 import numpy as np
 
 from . import util
-from .constants import tol, log
+from .constants import log
 
 try:
     import scipy.sparse
@@ -72,34 +72,32 @@ def align_vectors(a, b, return_angle=False):
 
     # projection of a onto b
     dot = np.dot(a, b)
-
-    # are vectors just reversed
-    if dot < (tol.merge - 1):
+    # resolution to compare floating point numbers
+    epsilon = 1e-12
+    if dot < (epsilon - 1):
         # a reversed vector is 180 degrees
         angle = np.pi
-
-        # https://github.com/mikedh/trimesh/issues/540
-        svd_a = np.linalg.svd(a[:, np.newaxis])[0]
-        svd_b = np.linalg.svd(b[:, np.newaxis])[0]
-        rotation = svd_b.dot(svd_a.T)
-
-    # are vectors already the same
-    elif dot > (1 - tol.merge):
+        # get an arbitrary perpendicular vector
+        # note that we are using both a and b
+        # so small values will be halved
+        perp = util.generate_basis(a - b)[0]
+        # compose the rotation matrix around our
+        # perpendicular vector with a simplification since
+        # cos(pi)=-1 and sin(pi)=0
+        rotation = np.outer(perp, perp) * 2.0 - np.eye(3)
+    elif dot > (1 - epsilon):
+        # are vectors already the same
         angle = 0.0
         # no rotation
         rotation = np.eye(3)
-
     # vectors are at some angle to each other
     else:
         # 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 float between 0.5 and 1.0
         c = 1.0 / (1.0 + dot)
-
         # (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]],
diff --git a/trimesh/util.py b/trimesh/util.py
index eedbe178..48ddaf05 100644
--- a/trimesh/util.py
+++ b/trimesh/util.py
@@ -60,6 +60,8 @@ log = logging.getLogger('trimesh')
 TOL_ZERO = np.finfo(np.float64).resolution * 100
 # how close to merge vertices
 TOL_MERGE = 1e-8
+# enable additional potentially slow checks
+_STRICT = False
 
 
 def unitize(vectors,
@@ -2000,18 +2002,28 @@ def generate_basis(z):
     # X as arbitrary perpendicular vector
     x = np.array([-z[1], z[0], 0.0])
     # avoid degenerate case
-    if np.isclose(np.linalg.norm(x), 0.0):
-        # Z is already along Z [0, 0, 1]
+    x_norm = np.linalg.norm(x)
+    if x_norm < 1e-12:
+        # this means that
         # so a perpendicular X is just X
-        x = np.array([1.0, 0.0, 0.0])
+        x = np.array([-z[2], z[1], 0.0])
+        x /= np.linalg.norm(x)
     else:
         # otherwise normalize X in- place
-        x /= np.linalg.norm(x)
+        x /= x_norm
     # get perpendicular Y with cross product
     y = np.cross(z, x)
-    # append result values into vector
+    # append result values into (3, 3) vector
     result = np.array([x, y, z], dtype=np.float64)
 
+    if _STRICT:
+        # run checks to make sure axis are perpendicular
+        assert np.abs(np.dot(x, z)) < 1e-8
+        assert np.abs(np.dot(y, z)) < 1e-8
+        assert np.abs(np.dot(x, y)) < 1e-8
+        # all vectors should be unit vector
+        assert np.allclose(np.linalg.norm(result, axis=1), 1.0)
+
     return result
 
 
@@ -2039,6 +2051,7 @@ def isclose(a, b, atol):
     """
     diff = a - b
     close = np.logical_and(diff > -atol, diff < atol)
+
     return close
 
 
diff --git a/trimesh/version.py b/trimesh/version.py
index f6310076..da4564dd 100644
--- a/trimesh/version.py
+++ b/trimesh/version.py
@@ -1,1 +1,1 @@
-__version__ = '3.4.0'
+__version__ = '3.4.1'


Test patch:
diff --git a/tests/generic.py b/tests/generic.py
index b5d79d2b..091e0f2c 100644
--- a/tests/generic.py
+++ b/tests/generic.py
@@ -45,8 +45,9 @@ from collections import deque
 from trimesh.constants import tol, tol_path
 from trimesh.base import Trimesh
 
-# make sure everyone knows they should run additional
-# validation checks and raise exceptions
+# make sure functions know they should run additional
+# potentially slow validation checks and raise exceptions
+trimesh.util._STRICT = True
 trimesh.constants.tol.strict = True
 trimesh.constants.tol_path.strict = True
 
diff --git a/tests/test_align.py b/tests/test_align.py
new file mode 100644
index 00000000..0b0e3639
--- /dev/null
+++ b/tests/test_align.py
@@ -0,0 +1,95 @@
+try:
+    from . import generic as g
+except BaseException:
+    import generic as g
+
+tol_norm = 1e-6
+
+
+class AlignTests(g.unittest.TestCase):
+
+    def test_align(self):
+        """
+        Test aligning two 3D vectors
+        """
+
+        # function we're testing
+        align = g.trimesh.geometry.align_vectors
+        is_rigid = g.trimesh.transformations.is_rigid
+
+        # 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),
+            [[7.12106798e-07, -7.43194705e-08, 1.00000000e+00],
+             [0, 0, -1],
+             [1e-4, 1e-4, -1]]))
+
+        for vector in vectors:
+            T, a = align(vector, target, return_angle=True)
+            assert is_rigid(T)
+            assert g.np.isclose(g.np.linalg.det(T), 1.0)
+            # rotate vector with transform
+            check = g.np.dot(T[:3, :3], vector)
+            # compare to target vector
+            norm = g.np.linalg.norm(check - target)
+            assert norm < tol_norm
+
+        # 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])
+
+    def test_range(self):
+        # function we're testing
+        align = g.trimesh.geometry.align_vectors
+        is_rigid = g.trimesh.transformations.is_rigid
+
+        # generate angles from 0 to 180 degrees
+        angles = g.np.linspace(0.0, g.np.pi / 1e7, 10000)
+        # 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):
+            g.trimesh.util.generate_basis(vector)
+            # check alignment to first vector
+            # which was created with zero angle
+            T, a = align(vector, vectors[0], return_angle=True)
+            assert is_rigid(T)
+            # check to make sure returned angle corresponds with truth
+
+            assert g.np.isclose(a, angle, atol=1e-6)
+
+            # check to make sure returned transform is correct
+            check = g.np.dot(T[:3, :3], vector)
+            norm = g.np.linalg.norm(check - vectors[0])
+
+            assert norm < tol_norm
+
+    def test_rigid(self):
+        # check issues with near-reversed vectors not returning rigid
+        align = g.trimesh.geometry.align_vectors
+        T = align([0, 0, -1], [-1e-17, 1e-17, 1])
+        assert g.np.isclose(g.np.linalg.det(T), 1.0)
+
+        T = align([0, 0, -1], [-1e-4, 1e-4, 1])
+        assert g.np.isclose(g.np.linalg.det(T), 1.0)
+
+        vector_1 = g.np.array([7.12106798e-07, -7.43194705e-08, 1.00000000e+00])
+        vector_2 = g.np.array([0, 0, -1])
+        T, angle = align(vector_1, vector_2, return_angle=True)
+        assert g.np.isclose(g.np.linalg.det(T), 1.0)
+
+
+if __name__ == '__main__':
+    g.trimesh.util.attach_to_log()
+    g.unittest.main()
diff --git a/tests/test_vector.py b/tests/test_vector.py
index 87594b33..ff356adc 100644
--- a/tests/test_vector.py
+++ b/tests/test_vector.py
@@ -46,64 +46,6 @@ class HemisphereTests(g.unittest.TestCase):
             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
-        is_rigid = g.trimesh.transformations.is_rigid
-
-        # 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),
-                               [[7.12106798e-07, -7.43194705e-08, 1.00000000e+00],
-                                [0, 0, -1]]))
-        for vector in vectors:
-            T, a = align(vector, target, return_angle=True)
-            assert is_rigid(T)
-            # rotate vector with transform
-            check = g.np.dot(T[:3, :3], vector)
-            # compare to target vector
-            norm = g.np.linalg.norm(check - target)
-            assert norm < 1e-8
-
-        # 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)
-            assert is_rigid(T)
-            # 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]
-            norm = g.np.linalg.norm(check - vectors[0])
-
-            assert norm < 1e-8
-
-
 if __name__ == '__main__':
     g.trimesh.util.attach_to_log()
     g.unittest.main()