1
方法一：
2
#使用轴角法求取向量之间的变换矩阵，和上一个函数是一样的方法
3
def rotation_matrix_from_vectors2(vec2):
4
    vec1 = [0, 0, 1]
5
    #规范化两个向量
6
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
7
    #求两个向量变换的旋转轴
8
    v = np.cross(a, b)
9
    s = np.linalg.norm(v)
10
    v = v / s
11
    #求旋转角度
12
    c = np.dot(a, b)
13
    theta = math.acos(c)
14
    #使用mr库求取轴角法的旋转矩阵，用的是罗德里格斯公式
15
    omegaHat = mr.VecToso3(v)
16
    so3mat = omegaHat * theta
17
    matrix = mr.MatrixExp3(so3mat)
18
    #求矩阵对应的欧拉角
19
    r = R.from_matrix(matrix)
20
    euler = r.as_euler('ZYX',degrees=True)
21
    matrix = r.as_matrix()
22
    return matrix
23

24
方法二：本质上都是一样的
25
def rotation_matrix_from_vectors(vec1,vec2):
26
    """ Find the rotation matrix that aligns vec1 to vec2
27
    :param vec1: A 3d "source" vector
28
    :param vec2: A 3d "destination" vector
29
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
30
    """
31
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
32
    v = np.cross(a, b)
33
    c = np.dot(a, b)
34
    s = np.linalg.norm(v)
35
    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
36
    rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
37
    return rotation_matrix