196 lines
6.6 KiB
Python
196 lines
6.6 KiB
Python
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() |