Pascal Serrarens
208b9056dc
git-subtree-dir: LinearAlgebra git-subtree-mainline: b87c4df0e3c2ee2aaf58bb8446d4b970d6fcb433 git-subtree-split: b60b248f7529a56244681bce0b204443abceacb2
44 lines
1.3 KiB
Python
44 lines
1.3 KiB
Python
import math
|
|
|
|
Deg2Rad = (math.pi * 2) / 360
|
|
|
|
class Quaternion:
|
|
|
|
def __init__(self):
|
|
self.x = 0
|
|
self.y = 0
|
|
self.z = 0
|
|
self.w = 1
|
|
|
|
@staticmethod
|
|
def Euler(x, y, z):
|
|
yaw = x * Deg2Rad
|
|
pitch = y * Deg2Rad
|
|
roll = z * Deg2Rad
|
|
|
|
roll_over_2 = roll * 0.5
|
|
sin_roll_over_2 = math.sin(roll_over_2)
|
|
cos_roll_over_2 = math.cos(roll_over_2)
|
|
|
|
pitch_over_2 = pitch * 0.5
|
|
sin_pitch_over_2 = math.sin(pitch_over_2)
|
|
cos_pitch_over_2 = math.cos(pitch_over_2)
|
|
|
|
yaw_over_2 = yaw * 0.5
|
|
sin_yaw_over_2 = math.sin(yaw_over_2)
|
|
cos_yaw_over_2 = math.cos(yaw_over_2)
|
|
|
|
result = Quaternion()
|
|
result.w = (cos_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
|
|
sin_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
|
|
result.x = (sin_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
|
|
cos_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
|
|
result.y = (cos_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2 -
|
|
sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2)
|
|
result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 -
|
|
sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2)
|
|
|
|
return result
|
|
|
|
|
|
Quaternion.identity = Quaternion() |