import math from Vector import Vector3 from Angle import Angle from Direction import Direction Deg2Rad = (math.pi * 2) / 360 class Quaternion: def __init__(self, x: float = 0, y: float = 0, z: float = 0, w: float = 1): self.x = x self.y = y self.z = z self.w = w magnitude: float = self.Magnitude() self.x /= magnitude self.y /= magnitude self.z /= magnitude self.w /= magnitude @staticmethod def FromAngles(yaw: Angle, pitch: Angle, roll: Angle): roll_over_2 = roll.InRadians() * 0.5 sin_roll_over_2 = math.sin(roll_over_2) cos_roll_over_2 = math.cos(roll_over_2) pitch_over_2 = pitch.InRadians() * 0.5 sin_pitch_over_2 = math.sin(pitch_over_2) cos_pitch_over_2 = math.cos(pitch_over_2) yaw_over_2 = yaw.InRadians() * 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 def ToAngles(self): test: float = self.x * self.y + self.z * self.w; if test > 0.499: # singularity at north pole return ( Angle.zero, Angle.Radians(2 * math.atan2(self.x, self.w)), Angle.Degrees(90) ) elif test < -0.499: # singularity at south pole return ( Angle.zero, Angle.Radians(-2 * math.atan2(self.x, self.w)), Angle.Degrees(-90) ) else: sqx: float = self.x * self.x sqy: float = self.y * self.y sqz: float = self.z * self.z yaw = Angle.Radians(math.atan2(2 * self.y * self.w - 2 * self.x * self.z, 1 - 2 * sqy - 2 * sqz)) pitch = Angle.Radians(math.atan2(2 * self.x * self.w - 2 * self.y * self.z, 1 - 2 * sqx - 2 * sqz)) roll = Angle.Radians(math.asin(2 * test)) return (yaw, pitch, roll) def Degrees(yaw: float, pitch: float, roll: float): return Quaternion.FromAngles( Angle.Degrees(yaw), Angle.Degrees(pitch), Angle.Degrees(roll) ) def Radians(yaw: float, pitch: float, roll: float): return Quaternion.FromAngles( Angle.Radians(yaw), Angle.Radians(pitch), Angle.Radians(roll) ) def FromAngleAxis(angle: Angle, axis: Direction): if axis.SqrMagnitude() == 0: return Quaternion.identity result: Quaternion = Quaternion.identity radians = angle.InRadians() radians *= 0.5 axis2: Vector3 = axis * math.sin(radians); q = Quaternion( axis2.right, axis2.up, axis2.forward, math.cos(radians) ) return q def ToAngleAxis(self) -> tuple[Angle, Direction]: angle: Angle = Angle.Radians(2 * math.acos(self.w)) den: float = math.sqrt(1 - self.w * self.w) if den > 0.0001: axis = Direction.FromVector3(self.Axis() / den) else: # This occurs when the angle is zero. # Not a problem: just set an arbitrary normalized axis. axis = Direction.right return (angle, axis) def __eq__(self, other) -> bool: return ( self.x == other.x and self.y == other.y and self.z == other.z and self.w == other.w ) def SqrMagnitude(self) -> float: return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w def Magnitude(self) -> float: """! The vector length @return The vector length """ return math.sqrt(self.SqrMagnitude()) def __mul__(self, other): if isinstance(other, Quaternion): return self.MultQuaternion(other) elif isinstance(other, Vector3): return self.MultVector(other) def MultQuaternion(self, q): return Quaternion( self.x * q.w + self.y * q.z - self.z * q.y + self.w * q.x, -self.x * q.z + self.y * q.w + self.z * q.x + self.w * q.y, self.x * q.y - self.y * q.x + self.z * q.w + self.w * q.z, -self.x * q.x - self.y * q.y - self.z * q.z + self.w * q.w ) def MultVector(self, v: Vector3): num = self.x * 2 num2 = self.y * 2 num3 = self.z * 2 num4 = self.x * num num5 = self.y * num2 num6 = self.z * num3 num7 = self.x * num2 num8 = self.x * num3 num9 = self.y * num3 num10 = self.w * num num11 = self.w * num2 num12 = self.w * num3 px = v.right py = v.up pz = v.forward rx = (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz ry = (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz rz = (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz result = Vector3(rx, ry, rz) return result def Dot(q1, q2) -> float: return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w def Angle(q1, q2): f: float = Quaternion.Dot(q1, q2) angle = Angle.Radians(math.acos(min(math.fabs(f), 1)) * 2) return angle # return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg; def AngleAround(self, axis: Direction): secondaryRotation: Quaternion = self.RotationAround(axis); (rotationAngle, rotationAxis) = secondaryRotation.ToAngleAxis(); # Do the axis point in opposite directions? if Vector3.Dot(axis.ToVector3(), rotationAxis.ToVector3()) < 0: return -rotationAngle; return rotationAngle; def RotationAround(self, axis: Direction): ra = Vector3(self.x, self.y, self.z) # rotation axis p: Vector3 = Vector3.Project(ra, axis.ToVector3()) # return projection ra on to axis (parallel component) twist: Quaternion = Quaternion(p.right, p.up, p.forward, self.w) return twist; def Axis(self) -> Vector3: return Vector3(self.x, self.y, self.z) Quaternion.identity = Quaternion()