Numerical instability in `trimesh.geometry.align_vectors`

Hi!

I came across this edge case where align_vectors returned a non rigid transform, it might be that this tolerance should be relaxed a bit?

>>> import trimesh
>>> import numpy as np
>>> vector_1 = np.array([ 7.12106798e-07, -7.43194705e-08,  1.00000000e+00])
>>> vector_2 = np.array([0, 0, -1])
>>> transform = trimesh.geometry.align_vectors(vector_1, vector_2)
>>> transform
array([[-9.78992868e-01,  2.06538826e-01,  7.12106798e-07,
         0.00000000e+00],
       [ 2.06538826e-01,  9.78444446e-01, -7.43194705e-08,
         0.00000000e+00],
       [-7.12106798e-07,  7.43194705e-08, -1.00054842e+00,
         0.00000000e+00],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])
>>> np.linalg.det(transform)
1.0010971441512102

[wannesvanloock]

To give some context regarding the method proposed by @lejafar. The SVD is used here to construct a basis for the null space of the vector (the last two columns of the svd). The first column is the normalized vector itself. The result will always be rigid, i.e. abs(determinant) == 1. It is however is not guaranteed to be right handed, and this is where @lejafar proposal fails (cfr #608). This is simple to fix by reversing one of the axes:

def right_handed_basis(a):
    '''Constructs a right handed basis from a and the null space of a.'''
    u, *_ = np.linalg.svd(a[:, np.newaxis])
    if np.linalg.det(u) < 0:
        u[:, -1] *= -1
    return u

def align_vectors(a, b):
    a_R_o = right_handed_basis(a)
    b_R_o = right_handed_basis(b)
    return a_R_o.dot(b_R_o.T)

Note that this should work in all cases.

[mikedh]

Good catch! I’m able to reproduce, and yeah I definitely think align_vectors should always return a rigid transform. I’ll add an assert is_rigid(T) to unit tests, and try to generate more examples of failing values before working on a fix.

Thanks for the report!