欧拉角 转四元数

则（设旋转前坐标：（x0, y0, z0）, 旋转后坐标：（x3, y3, z3））：

1. # 四元数转换为欧拉角
2. def quat2eular(w, x, y, z):
3.     # 笛卡尔坐标系
4.     roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
5.     pitch = math.asin(2 * (w * y - x * z))
6.     yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
7.  
8.     # Direct3D, 笛卡尔坐标的X轴变为Z轴, Y轴变为X轴, Z轴变为Y轴
9.     # roll = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + x * x))
10.     # pitch = math.asin(2 * (w * x - y * z))
11.     # yaw = math.atan2(2 * (w * y + z * x), 1 - 2 * (x * x + y * y))
12.  
13.     # 转换为角度
14.     return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)
15.  
16. # 欧拉角转换为四元数, 旋转顺序为ZYX(偏航角yaw, 俯仰角pitch, 横滚角roll)
17. def eular2quat(yaw, pitch, roll):
18.     # 注意这里必须先转换为弧度, 因为这里的三角计算均使用的是弧度.
19.     yaw = math.radians(yaw)
20.     pitch = math.radians(pitch)
21.     roll = math.radians(roll)
22.  
23.     cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
24.     cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
25.     cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
26.  
27.     # 笛卡尔坐标系
28.     w = cr * cp * cy + sr * sp * sy
29.     x = sr * cp * cy - cr * sp * sy
30.     y = cr * sp * cy + sr * cp * sy
31.     z = cr * cp * sy - sr * sp * cy
32.  
33.     # Direct3D, 笛卡尔坐标的X轴变为Z轴, Y轴变为X轴, Z轴变为Y轴
34.     # w = cr * cp * cy + sr * sp * sy
35.     # x = cr * sp * cy + sr * cp * sy
36.     # y = cr * cp * sy - sr * sp * cy
37.     # z = sr * cp * cy - cr * sp * sy
38.  
39.     return w, x, y, z
40.  
41.  
42. if __name__ == "__main__":
43.     y, p, r = (10, 20, 30)
44.     q0, q1, q2, q3 = eular2quat(y, p, r)
45.     print(q0, q1, q2, q3)
46.     y, p, r = quat2eular(q0, q1, q2, q3)
47.     q0, q1, q2, q3 = eular2quat(y, p, r)
48.     print(q0, q1, q2, q3)
49.     print(quat2eular(q0, q1, q2, q3))