三维空间中旋转两个面使其平行python代码

三维空间中旋转两个面使其平行: 平面的法向量代表这个面的方向，只需要求解两个法向量的旋转矩阵即可。

如果输出的两个面的法向量的夹角为0 或 3.14（近似为）则成功。

#归一化
def normal(vertex):
    _range = np.max(vertex) - np.min(vertex)
    vertex = (vertex - np.min(vertex)) / _range
    return vertex

#罗德里格斯公式
def rodrigues_rotation(r, theta):
    # n旋转轴[3x1]
    # theta为旋转角度
    # 旋转是过原点的，n是旋转轴
    r = np.array(r).reshape(3, 1)
    rx, ry, rz = r[:, 0]
    M = np.array([
        [0, -rz, ry],
        [rz, 0, -rx],
        [-ry, rx, 0]
    ])
    R = np.eye(3)
    R[:3, :3] = np.cos(theta) * np.eye(3) +        \
                (1 - np.cos(theta)) * r @ r.T +    \
                np.sin(theta) * M
    return R

if __name__ == '__main__':
    inject_path = 'test1.obj'  # basic_gem.obj
    obj_path = 'test2.obj' # T5.obj
    inject_target_face_index = 0 #6
    obj_target_face_index = 0
    inject = trimesh.load(inject_path)
    # 将inject归一化
    inject.vertices = normal(inject.vertices)
    obj = trimesh.load(obj_path)
    inject_faces_list = inject.faces.tolist()
    obj_faces_list = obj.faces.tolist()

    # 计算两个面间的旋转矩阵
    # 得到两个物体中一个平面的法向量
    inject_normal = inject.face_normals[inject_target_face_index]
    inject_face_points = inject.vertices[inject.faces[inject_target_face_index]]

    obj_normal = obj.face_normals[obj_target_face_index]
    obj_face_points = obj.vertices[obj.faces[obj_target_face_index]]

    # 计算的是以pi为单位的两个平面法线的角度,
    deta = math.acos(np.dot(inject_normal, obj_normal) / (np.linalg.norm(inject_normal) * np.linalg.norm(obj_normal)))
    print('original_deta: {}'.format(deta))
    # 得到两个法线组成的平面的法线
    np.cross([0,0,1], [0,1,0])
    plane_normal = np.cross(inject_normal, obj_normal)
    # 此时得到的平面的法线指的是垂直于 obj_normal 和 inject_normal 组成的平面的法线， 过原点
    # 使用罗德里格斯公式计算两个平面的旋转
    R = rodrigues_rotation(plane_normal, deta)
    np.transpose(np.matrix(R)) * np.matrix(R)

    for i in range(len(inject.vertices)):
        inject.vertices[i] = np.matrix(inject.vertices[i]) * np.transpose(np.matrix(R))

    # 计算两个面间的平移向量
    inject_point = inject.vertices[inject_faces_list[inject_target_face_index][0]]
    obj_point = obj.vertices[obj_faces_list[obj_target_face_index][0]]
    v = inject_point - obj_point

    inject_face_points = inject.vertices[inject.faces[inject_target_face_index]]
    inject_normal = np.cross(inject_face_points[0]-inject_face_points[2], inject_face_points[1]-inject_face_points[2])
    deta = math.acos(np.dot(inject_normal, obj_normal) / (np.linalg.norm(inject_normal) * np.linalg.norm(obj_normal)))
    print('after_deta: {}'.format(deta))```