from math import acos, cos, sin, sqrt, floor, pi
from typing import NamedTuple, Sequence, List
[docs]class Vector(NamedTuple):
""" Vector of the form:
x <i>, y <j>, z <k>
"""
x: float
y: float
z: float
def interpolate(self, other, alpha):
x = (1.0 - alpha) * self.x + alpha * other.x
y = (1.0 - alpha) * self.y + alpha * other.y
z = (1.0 - alpha) * self.z + alpha * other.z
return Vector(x, y, z)
[docs]class ECI(Vector):
""" Vector class denoting Earth Centered Inertial (fixed to the stars) reference frame. """
x: float
y: float
z: float
[docs] def to_geo(self, mjd: float) -> Vector:
""" Code to calculate theta copied from the ECIToGeo C++ L1 Services function. """
ut = mjd - 51544.5
ut0 = floor(ut - 0.5) + 0.5
frac = (ut - ut0)
tu = ut0 / 36525.0
gmst = (24110.54841 + (8640184.812866 + (0.093104 - 0.0000062 * tu) * tu) * tu) / 86400.0 + frac * 1.00273790935
gmst = gmst - floor(gmst)
theta = gmst * 2.0 * pi # Angle to rotate by in radians
rotation_quart = TaitBryanZYX(yaw=-theta, pitch=0.0, roll=0.0).to_quaternion()
return rotation_quart.rot([self])[0]
[docs]class ECEF(Vector):
""" Vector class denoting Earth Centered, Earth Fixed reference frame. """
x: float
y: float
z: float
[docs]class Geodetic(NamedTuple):
""" Vector class denoting Earth Centered, Earth Fixed reference frame in latitude, longitude, height. """
latitude: float
longitude: float
height: float
[docs]class UnitQuaternion(NamedTuple):
""" Normalized Quaternion with multiply and vector rotate methods of the form:
a + bi + cj + dk
"""
a: float
b: float
c: float
d: float
def __neg__(self):
return UnitQuaternion(-self.a, -self.b, -self.c, -self.d)
def __add__(self, other):
a = self.a + other.a
b = self.b + other.b
c = self.c + other.c
d = self.d + other.d
return UnitQuaternion(a, b, c, d)
def __sub__(self, other):
return self + (-other)
def __mul__(self, other):
if type(other) is UnitQuaternion:
a = self.a * other.a - self.b * other.b - self.c * other.c - self.d * other.d
b = self.a * other.b + self.b * other.a + self.c * other.d - self.d * other.c
c = self.a * other.c - self.b * other.d + self.c * other.a + self.d * other.b
d = self.a * other.d + self.b * other.c - self.c * other.b + self.d * other.a
return UnitQuaternion(a, b, c, d)
else:
try:
return UnitQuaternion(other * self.a, other * self.b, other * self.c, other * self.d)
except Exception:
raise TypeError(f'{self.__class__.__name__} does not support __mul__ with {other.__class__.__name__})')
def normalize(self):
norm = sqrt(self.a ** 2.0 + self.b ** 2.0 + self.c ** 2.0 + self.d ** 2.0)
return UnitQuaternion(self.a / norm, self.b / norm, self.c / norm, self.d / norm)
def dot(self, other) -> float:
return self.a * other.a + self.b * other.b + self.c * other.c + self.d * other.d
[docs] def rot(self, other: Sequence[Vector]) -> List[Vector]:
""" Rotate a list of vectors by a Quaternion.
q * v * q'
"""
a, b, c, d = self
# Define the 3x3 rotation matrix (A)
a00 = a ** 2.0 + b ** 2.0 - c ** 2.0 - d ** 2.0
a01 = 2.0 * b * c - 2.0 * a * d
a02 = 2.0 * b * d + 2.0 * a * c
a10 = 2.0 * b * c + 2.0 * a * d
a11 = a ** 2.0 - b ** 2.0 + c ** 2.0 - d ** 2.0
a12 = 2.0 * c * d - 2.0 * a * b
a20 = 2.0 * b * d - 2.0 * a * c
a21 = 2.0 * c * d + 2.0 * a * b
a22 = a ** 2.0 - b ** 2.0 - c ** 2.0 + d ** 2.0
rotated = []
for x, y, z in other:
xx = a00 * x + a01 * y + a02 * z
yy = a10 * x + a11 * y + a12 * z
zz = a20 * x + a21 * y + a22 * z
rotated.append(Vector(xx, yy, zz))
return rotated
[docs] def rot_no_z(self, other: Sequence[Vector]) -> List[Vector]:
""" Rotate a list of vectors with z=0.0 by a Quaternion.
q * v * q'
"""
a, b, c, d = self
# Define the 3x2 rotation matrix (A)
a00 = a ** 2.0 + b ** 2.0 - c ** 2.0 - d ** 2.0
a01 = 2.0 * b * c - 2.0 * a * d
a10 = 2.0 * b * c + 2.0 * a * d
a11 = a ** 2.0 - b ** 2.0 + c ** 2.0 - d ** 2.0
a20 = 2.0 * b * d - 2.0 * a * c
a21 = 2.0 * c * d + 2.0 * a * b
rotated = []
for x, y, z in other:
xx = a00 * x + a01 * y
yy = a10 * x + a11 * y
zz = a20 * x + a21 * y
rotated.append(Vector(xx, yy, zz))
return rotated
[docs] def rot_unit_x(self) -> Vector:
""" Rotate the x-unit vector <1, 0, 0> by a Quaternion.
q * v * q'
"""
a, b, c, d = self
# Define the 3x3 rotation matrix (A)
x = a ** 2.0 + b ** 2.0 - c ** 2.0 - d ** 2.0
y = 2.0 * b * c + 2.0 * a * d
z = 2.0 * b * d - 2.0 * a * c
return Vector(x, y, z)
[docs] def slerp(self, other, alpha):
""" Quaternion slerp adapted from c++ reference on wikipedia article.
Only unit quaternions are valid rotations.
"""
dot = self.dot(other)
# If the dot product is negative, the quaternions
# have opposite handed-ness and slerp won't take
# the shorter path. Fix by reversing one quaternion.
if dot < 0.0:
other = -other
dot = -dot
dot_threshold = 0.9995
if dot > dot_threshold:
# If the inputs are too close for comfort, linearly interpolate and normalize the result.
# return self - alpha * (other - self)
return (self + (other - self) * alpha).normalize()
theta_0 = acos(dot) # theta_0 = angle between input vectors
theta = theta_0 * alpha # theta = angle between self and result
perpendicular = (other - self * dot).normalize() # { self, perpendicular } is now an orthonormal basis
return self * cos(theta) + perpendicular * sin(theta)
[docs]class TaitBryanZYX(NamedTuple):
yaw: float # heading
pitch: float # attitude
roll: float # bank
def to_quaternion(self) -> UnitQuaternion:
cr = cos(self.roll * 0.5)
sr = sin(self.roll * 0.5)
cp = cos(self.pitch * 0.5)
sp = sin(self.pitch * 0.5)
cy = cos(self.yaw * 0.5)
sy = sin(self.yaw * 0.5)
a = cy * cr * cp + sy * sr * sp
b = cy * sr * cp - sy * cr * sp
c = cy * cr * sp + sy * sr * cp
d = sy * cr * cp - cy * sr * sp
return UnitQuaternion(a, b, c, d)