python 欧拉角快速转旋转矩阵

在Python中，欧拉角（Euler Angles）通常用于描述三维空间中的旋转，它们包括三个角度：滚（Pitch）、俯（Roll）和yaw（Yaw）。将这三个欧拉角转换为旋转矩阵是一个常见的操作，因为旋转矩阵更便于数学计算和计算机图形学中的旋转应用。

快速将欧拉角转换成旋转矩阵可以采用各种算法，如Tait-Bryan（也叫ZYX、XYZ或ZXZ顺序），或者Cardan angles。其中最常用的是绕x轴、y轴和z轴的旋转顺序，例如：

1. 首先，计算绕x轴旋转的旋转矩阵Rx(θx)。
2. 然后，根据欧拉角顺序，对已经旋转的坐标系应用下一个旋转：如果是绕y轴，则计算Ry(θy)(Rx(θx) * [x, y, z])；如果是绕z轴，则计算Rz(θz)(Ry(θy)(Rx(θx) * [x, y, z]))。

这里[x, y, z]代表未经旋转前的原点坐标向量。

以下是伪代码示例：

import numpy as np

def euler_to_matrix(euler_angles):
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(euler_angles), -np.sin(euler_angles)],
                   [0, np.sin(euler_angles), np.cos(euler_angles)]])
    
    Ry = np.array([[np.cos(euler_angles), 0, np.sin(euler_angles)],
                   [0, 1, 0],
                   [-np.sin(euler_angles), 0, np.cos(euler_angles)]])

    Rz = np.array([[np.cos(euler_angles), -np.sin(euler_angles), 0],
                   [np.sin(euler_angles), np.cos(euler_angles), 0],
                   [0, 0, 1]])

    # 根据欧拉角顺序确定最终矩阵
    if 'zyx' in euler_angles.order:
        return Rz @ Ry @ Rx
    elif 'xyz' in euler_angles.order:
        return Rx @ Ry @ Rz
    else:
        raise ValueError("Invalid Euler angle sequence")

# 示例
euler_angles = (np.radians(45), np.radians(30), np.radians(60))
rot_mat = euler_to_matrix(Eulerangles(euler_angles))