围绕给定轴旋转任意平面会导致结果不一致

问题描述

我正在尝试绕任意轴旋转和平移任意平面。 为了进行测试，我编写了一个简单的python程序，该程序绕着X轴旋转一个随机平面 度。

不幸的是，当检查平面之间的角度时，我得到的结果不一致。 这是代码：

def angle_between_planes(plane1, plane2):
    plane1 = (plane1 / np.linalg.norm(plane1))[:3]
    plane2 = (plane2/ np.linalg.norm(plane2))[:3]
    cos_a = np.dot(plane1.T, plane2) / (np.linalg.norm(plane1) * np.linalg.norm(plane2))
    print(np.arccos(cos_a)[0, 0])


def test():
    axis = np.array([1, 0, 0])
    theta = np.pi / 2
    translation = np.array([0, 0, 0])
    T = get_transformation(translation, axis * theta)
    for i in range(1, 10):
        source = np.append(np.random.randint(1, 20, size=3), 0).reshape(4, 1)
        target = np.dot(T, source)
        angle_between_planes(source, target)

它打印：

1.21297144225
1.1614420953
1.48042948278
1.10098697889
0.992418096794
1.16954303911
1.04180591409
1.08015300394
1.51949177153

调试该代码时，我看到转换矩阵是正确的，因为它表明

我不确定发生了什么问题，希望在此提供任何帮助。

*生成转换矩阵的代码为：

def get_transformation(translation_vec, rotation_vec):
    r_4 = np.array([0, 0, 0, 1]).reshape(1, 4)
    rotation_vec= rotation_vec.reshape(3, 1)
    theta = np.linalg.norm(rotation_vec)
    axis = rotation_vec/ theta
    R = get_rotation_mat_from_axis_and_angle(axis, theta)
    T = translation_vec.reshape(3, 1)
    R_T = np.append(R, T, axis = 1)
    return np.append(R_T, r_4, axis=0)



def get_rotation_mat_from_axis_and_angle(axis, theta):
    axis = axis / np.linalg.norm(axis)
    a, b, c = axis
    omct = 1 - np.cos(theta)
    ct = np.cos(theta)
    st = np.sin(theta)
    rotation_matrix =  np.array([a * a * omct + ct,  a * b * omct - c * st,  a * c * omct + b * st,
                                 a * b * omct + c * st, b * b * omct + ct,  b * c * omct - a * st,
                                 a * c * omct - b * st, b * c * omct + a * st, c * c * omct + ct]).reshape(3, 3)
    rotation_matrix[abs(rotation_matrix) < 1e-8] = 0
    return rotation_matrix