欧拉角和旋转矩阵转换

欧拉角和旋转矩阵是描述三维空间中物体旋转的两种常见方法。欧拉角是指通过固定顺序的三个旋转角度来表示旋转的方法，而旋转矩阵则是表示旋转的线性变换矩阵。下面介绍欧拉角和旋转矩阵之间的转换方法。

欧拉角到旋转矩阵的转换：

设三个旋转角度分别为α、β、γ，按照顺序先绕z轴旋转α角度，然后绕y轴旋转β角度，最后绕z轴旋转γ角度，得到的旋转矩阵为：

R = Rz(γ) * Ry(β) * Rz(α)

其中Rz(θ)和Ry(θ)分别表示绕z轴和y轴旋转θ角度的旋转矩阵，具体表达式为：

Rz(θ) = [cosθ -sinθ 0; sinθ cosθ 0; 0 0 1]

Ry(θ) = [cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]

旋转矩阵到欧拉角的转换：

设旋转矩阵为R，根据欧拉角的定义，有：

R = Rz(γ) * Ry(β) * Rz(α)

将上式两边同时左乘Rz(-α)得：

R * Rz(-α) = Rz(γ) * Ry(β)

再将上式两边同时右乘Ry(-β)得：

R * Rz(-α) * Ry(-β) = Rz(γ)

因此，可以从旋转矩阵中求出欧拉角：

γ = arctan2(R(2,1), R(1,1))

β = arctan2(-R(3,1), sqrt(R(3,2)^2 + R(3,3)^2))

α = arctan2(R(3,2), R(3,3))

其中arctan2函数是求反正切值的函数，可以避免由于正切函数的周期性带来的计算错误