Euler angle to quaternion

 

Then (suppose the coordinates before rotation: ( x0, y0, z0 ) , the coordinates after rotation: ( x3, y3, z3 )):

1. # Quaternion converted to Euler angles
2. def quat2eular (w, x, y, z):
3.     # Cartesian coordinate system
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, the X axis of the Cartesian coordinates becomes the Z axis , the Y axis becomes the X axis , and the Z axis becomes the Y axis
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.     # Converted into an angle
14.     return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)
15.  
16. # Euler angles is converted to quaternions , rotation sequence is ZYX ( yaw angle Yaw, pitch angle Pitch, roll angle of roll)
17. def eular2quat(yaw, pitch, roll):
18.     #Note that this must be converted to radians first , because all the triangle calculations here use radians .
19.     yaw = math.radians(yaw)
20.     pitch = math.radians(pitch)
21.     roll = math.radians(roll)
22.  
23.     en , 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.     # Cartesian coordinate system
28.     w = c * ep * cs + r * sp * sy
29.     x = r * ep * cy - er * sp * sy
30.     y = cr * sp * cy + sr * cp * sy
31.     z = c * ep * sy - r * sp * cs
32.  
33.     # Direct3D, the X axis of the Cartesian coordinates becomes the Z axis , the Y axis becomes the X axis , and the Z axis becomes the Y axis
34.     # W 'cp = CR + SR * Cy * sp * sy
35.     # X = c * sp * cs + SR 'cp * sy
36.     # y = cr * cp * sy - sr * sp * cy
37.     # Z = r * CP * cy - er 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))