找到旋转矩阵来对齐两个向量

Question

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

我有一个向量 A = [ax, ay, az]，我想将其与向量 B = [1, 0, 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, 0, -v1],
                      [-v2, v1, 0]])

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

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

x_axis = np.array([1, 0, 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, 1.004, -0.02], dtype=np.float64) # Point in the direction of the unit vector
result = Ralign.dot(point)

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

Answer 1

如果您只想旋转一个向量

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)
    if np.isclose(c, -1.0):
        return -np.eye(3, dtype=np.float64)

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

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

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

该算法还修复了另一个边缘情况： 如果使用两个方向相反的平行向量调用该函数，

h

将计算为

inf

，并且 R 将仅包含

nan

。

align_vectors(np.array([1,0,0]), np.array([-1,0,0]))

这是通过将

c

与 -1 进行比较并返回负单位矩阵来捕获的。 测试：

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), angle(direction,result2))) # True
print('Length:', np.isclose(np.linalg.norm(direction), np.linalg.norm(result2))) # True