Euler-Winkel zur Quaternion

 

Dann (nehmen wir die Koordinaten vor der Drehung an: ( x0, y0, z0 ) , die Koordinaten nach der Drehung: ( x3, y3, z3 )):

1. # Quaternion in Euler-Winkel konvertiert
2. def quat2eular (w, x, y, z):
3.     # Kartesisches Koordinatensystem
4.     roll = math.atan2 (2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
5.     Tonhöhe = 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, die X- Achse der kartesischen Koordinaten wird zur Z- Achse , die Y- Achse wird zur X- Achse und die Z- Achse wird zur Y- Achse
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.     # In einen Winkel umgewandelt
14.     Rückgabe math.degrees (Gieren), math.degrees (Tonhöhe), math.degrees (Rolle)
15.  
16. # Eulerwinkel werden in Quaternionen umgewandelt , Rotationssequenz ist ZYX ( Gierwinkel Gieren , Nickwinkel Pitch, Rollwinkel der Rolle)
17. def eular2quat (Gieren, Neigen , Rollen):
18.     #Beachten Sie, dass dies zuerst in Bogenmaß konvertiert werden muss , da alle Dreiecksberechnungen hier Bogenmaß verwenden .
19.     yaw = math.radians (yaw)
20.     Tonhöhe = math.radians (Tonhöhe)
21.     roll = math.radians (roll)
22.  
23.     en , sy = math.cos (Gieren * 0,5 ), math.sin (Gieren * 0,5 )
24.     cp, sp = math.cos (Tonhöhe * 0,5 ), math.sin (Tonhöhe * 0,5 )
25.     cr, sr = math.cos (roll * 0,5 ), math.sin (roll * 0,5 )
26.  
27.     # Kartesisches Koordinatensystem
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, die X- Achse der kartesischen Koordinaten wird zur Z- Achse , die Y- Achse wird zur X- Achse und die Z- Achse wird zur Y- Achse
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.     Drucken (q0, q1, q2, q3)
46.     y, p, r = quat2eular (q0, q1, q2, q3)
47.     q0, q1, q2, q3 = eular2quat (y, p, r)
48.     Drucken (q0, q1, q2, q3)
49.     print (quat2eular (q0, q1, q2, q3))