python 3d旋转 父子旋转

python 3d旋转 父子旋转

如果父子旋转的原点相同，那么旋转矩阵是相同的。

import numpy as np
np.set_printoptions(precision=2, suppress=True)
def calculate_rotated_child_position(parent_position, child_position, rotation_angles):
    # 将旋转角度（ZYX顺序）转换为旋转矩阵
    rotation_matrix = np.linalg.inv(np.array([
        [np.cos(rotation_angles[1]) * np.cos(rotation_angles[0]), np.sin(rotation_angles[2]) * np.sin(rotation_angles[1]) * np.cos(rotation_angles[0]) - np.cos(rotation_angles[2]) * np.sin(rotation_angles[0]), np.cos(rotation_angles[2]) * np.sin(rotation_angles[1]) * np.cos(rotation_angles[0]) + np.sin(rotation_angles[2]) * np.sin(rotation_angles[0])],
        [np.cos(rotation_angles[1]) * np.sin(rotation_angles[0]), np.sin(rotation_angles[2]) * np.sin(rotation_angles[1]) * np.sin(rotation_angles[0]) + np.cos(rotation_angles[2]) * np.cos(rotation_angles[0]), np.cos(rotation_angles[2]) * np.sin(rotation_angles[1]) * np.sin(rotation_angles[0]) - np.sin(rotation_angles[2]) * np.cos(rotation_angles[0])],
        [-np.sin(rotation_angles[1]), np.sin(rotation_angles[2]) * np.cos(rotation_angles[1]), np.cos(rotation_angles[2]) * np.cos(rotation_angles[1])]
    ]))

    rotated_parent_position = np.dot(rotation_matrix, np.array(parent_position))
    rotated_child_position = np.dot(rotation_matrix, np.array(child_position))

    return rotated_parent_position, rotated_child_position

# 示例：父节点位置、子节点位置和旋转角度（ZYX顺序，弧度）
parent_position = [1, 0, 0]
child_position = [2, 0,0]
rotation_angles = [np.pi/4,np.pi/4,np.pi/4]  # 45度旋转


# 计算旋转后的子节点位置
parent_position, child_position = calculate_rotated_child_position(parent_position, child_position, rotation_angles)
print(f"旋转后的子节点位置: {parent_position} {child_position}")