Calculate rotation matrix to align two vectors in 3D space?

前端 未结 3 1400
灰色年华
灰色年华 2020-12-10 20:10

I have two separate vectors of 3D data points that represent curves and I\'m plotting these as scatter data in a 3D plot with matplotlib.

Both the vectors start at t

3条回答
  •  旧时难觅i
    2020-12-10 21:09

    Based on Daniel F's correction, here is a function that does what you want:

    import numpy as np
    
    def rotation_matrix_from_vectors(vec1, vec2):
        """ Find the rotation matrix that aligns vec1 to vec2
        :param vec1: A 3d "source" vector
        :param vec2: A 3d "destination" vector
        :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
        """
        a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
        v = np.cross(a, b)
        c = np.dot(a, b)
        s = np.linalg.norm(v)
        kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
        rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
        return rotation_matrix
    

    Test:

    vec1 = [2, 3, 2.5]
    vec2 = [-3, 1, -3.4]
    
    mat = rotation_matrix_from_vectors(vec1, vec2)
    vec1_rot = mat.dot(vec1)
    assert np.allclose(vec1_rot/np.linalg.norm(vec1_rot), vec2/np.linalg.norm(vec2))
    

提交回复
热议问题