查找旋转矩阵以对齐两个向量

问题描述

我尝试找到旋转矩阵来对齐两个向量。

我有一个向量 A = [ax,ay,az]，我想将它与向量 B = [1,0]（x 轴单位向量）对齐。

我找到了以下解释并尝试实现它：https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/897677#897677

def align_vectors(a,b):
     v = np.cross(a,b)
     s = np.linalg.norm(v)
     c = np.dot(a,b)

     v1,v2,v3 = v
     h = 1 / (1 + c)

     Vmat = np.array([[0,-v3,v2],[v3,-v1],[-v2,v1,0]])

     R = np.eye(3,dtype=np.float64) + Vmat + (Vmat.dot(Vmat) * h)
     return R

当我应用它来找到一个点的旋转时，这就是我所拥有的：

x_axis = np.array([1,0],dtype=np.float64)
direction = np.array([-0.02,1.004,-0.02],dtype=np.float64)
Ralign = align_vectors(x_axis,direction)
point = 1000 * np.array([-0.02,dtype=np.float64) # Point in the direction of the unit vector
result = Ralign.dot(point)

结果点未与单位向量对齐。

解决方法

如果您只想旋转一个向量 a 以与 b 对齐，而不是整个坐标都包含该向量，请使用简单的向量投影和 a 的长度：

a_norm = np.linalg.norm(a)
b_norm = np.linalg.norm(b)
result = b * a_norm / b_norm

以下解决了输入不是向量归一化单位向量的问题。

def align_vectors(a,b):
    b = b / np.linalg.norm(b) # normalize a
    a = a / np.linalg.norm(a) # normalize b
    v = np.cross(a,b)
    # s = np.linalg.norm(v)
    c = np.dot(a,b)

    v1,v2,v3 = v
    h = 1 / (1 + c)

    Vmat = np.array([[0,-v3,v2],[v3,-v1],[-v2,v1,0]])

    R = np.eye(3,dtype=np.float64) + Vmat + (Vmat.dot(Vmat) * h)
    return R

测试：


def angle(a,b):
    """Angle between vectors"""
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)
    return np.arccos(a.dot(b))

point = np.array([-0.02,1.004,-0.02])
direction = np.array([1.,0.,0.])
rotation = align_vectors(point,direction)

# Rotate point in align with direction. The result vector is aligned with direction
result = rotation.dot(point)
print(result)
print('Angle:',angle(direction,point)) # 0.0
print('Length:',np.isclose(np.linalg.norm(point),np.linalg.norm(result))) # True


# Rotate direction by the matrix,result does not align with direction but the angle between the original vector (direction) and the result2 are the same.
result2 = rotation.dot(direction)
print(result2)
print('Same Angle:',np.isclose(angle(point,result),result2))) # True
print('Length:',np.isclose(np.linalg.norm(direction),np.linalg.norm(result2))) # True

python rotation