kopia lustrzana https://github.com/peterhinch/micropython-samples
244 wiersze
7.4 KiB
Python
244 wiersze
7.4 KiB
Python
![]() |
# quat.py "Micro" Quaternion class
|
||
|
|
||
|
# Released under the MIT License (MIT). See LICENSE.
|
||
|
# Copyright (c) 2020 Peter Hinch
|
||
|
|
||
|
from math import sqrt, sin, cos, acos, isclose, asin, atan2, pi
|
||
|
from array import array
|
||
|
mdelta = 0.001 # 0.1% Minimum difference considered significant for graphics
|
||
|
adelta = 0.001 # Absolute tolerance for components near 0
|
||
|
|
||
|
def _arglen(arg):
|
||
|
length = 0
|
||
|
try:
|
||
|
length = len(arg)
|
||
|
except TypeError:
|
||
|
pass
|
||
|
if length not in (0, 3, 4):
|
||
|
raise ValueError('Sequence length must be 3 or 4')
|
||
|
return length
|
||
|
|
||
|
# Convert a rotation quaternion to Euler angles. Beware:
|
||
|
# https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible
|
||
|
def euler(q): # Return (heading, pitch, roll)
|
||
|
if not q.isrot():
|
||
|
raise ValueError('Must be a rotation quaternion.')
|
||
|
w, x, y, z = q
|
||
|
pitch = asin(2*(w*y - x*z))
|
||
|
if isclose(pitch, pi/2, rel_tol = mdelta):
|
||
|
return -2 * atan2(x, w), pitch, 0
|
||
|
if isclose(pitch, -pi/2, rel_tol = mdelta):
|
||
|
return 2 * atan2(x, w), pitch, 0
|
||
|
roll = atan2(2*(w*x + y*z), w*w - x*x - y*y + z*z)
|
||
|
#roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
|
||
|
hdg = atan2(2*(w*z + x*y), w*w + x*x - y*y - z*z)
|
||
|
#hdg = atan2(2*(w*z + x*y), 1 - 2 *(y*y + z*z))
|
||
|
return hdg, pitch, roll
|
||
|
|
||
|
|
||
|
class Quaternion:
|
||
|
|
||
|
def __init__(self, w=1, x=0, y=0, z=0): # Default: the identity quaternion
|
||
|
self.d = array('f', (w, x, y, z))
|
||
|
|
||
|
@property
|
||
|
def w(self):
|
||
|
return self[0]
|
||
|
|
||
|
@w.setter
|
||
|
def w(self, v):
|
||
|
self[0] = v
|
||
|
|
||
|
@property
|
||
|
def x(self):
|
||
|
return self[1]
|
||
|
|
||
|
@x.setter
|
||
|
def x(self, v):
|
||
|
self[1] = v
|
||
|
|
||
|
@property
|
||
|
def y(self):
|
||
|
return self[2]
|
||
|
|
||
|
@y.setter
|
||
|
def y(self, v):
|
||
|
self[2] = v
|
||
|
|
||
|
@property
|
||
|
def z(self):
|
||
|
return self[3]
|
||
|
|
||
|
@z.setter
|
||
|
def z(self, v):
|
||
|
self[3] = v
|
||
|
|
||
|
def normalise(self):
|
||
|
if self[0] == 1: # acos(1) == 0. Identity quaternion: no rotation
|
||
|
return Quaternion(1, 0, 0, 0)
|
||
|
m = abs(self) # Magnitude
|
||
|
assert m > 0.1 # rotation quaternion should have magnitude ~= 1
|
||
|
if isclose(m, 1.0, rel_tol=mdelta):
|
||
|
return self # No normalisation necessary
|
||
|
return Quaternion(*(a/m for a in self))
|
||
|
|
||
|
def __getitem__(self, key):
|
||
|
return self.d[key]
|
||
|
|
||
|
def __setitem__(self, key, v):
|
||
|
try:
|
||
|
v1 = array('f', v)
|
||
|
except TypeError: # Scalar
|
||
|
v1 = v
|
||
|
self.d[key] = v1
|
||
|
|
||
|
def copy(self):
|
||
|
return Quaternion(*self)
|
||
|
|
||
|
def __abs__(self): # Return magnitude
|
||
|
return sqrt(sum((d*d for d in self)))
|
||
|
|
||
|
def __len__(self):
|
||
|
return 4
|
||
|
# Comparison: == and != perform equality test of all elements
|
||
|
def __eq__(self, other):
|
||
|
return all((isclose(a, b, rel_tol=mdelta, abs_tol=adelta) for a, b in zip(self, other)))
|
||
|
|
||
|
def __ne__(self, other):
|
||
|
return not self == other
|
||
|
|
||
|
# < and > comparisons compare magnitudes.
|
||
|
def __gt__(self, other):
|
||
|
return abs(self) > abs(other)
|
||
|
|
||
|
def __lt__(self, other):
|
||
|
return abs(self) < abs(other)
|
||
|
|
||
|
# <= and >= return True for complete equality otherwise magnitudes are compared.
|
||
|
def __ge__(self, other):
|
||
|
return True if self == other else abs(self) > abs(other)
|
||
|
|
||
|
def __le__(self, other):
|
||
|
return True if self == other else abs(self) < abs(other)
|
||
|
|
||
|
def to_angle_axis(self):
|
||
|
q = self.normalise()
|
||
|
if isclose(q[0], 1.0, rel_tol = mdelta):
|
||
|
return 0, 1, 0, 0
|
||
|
theta = 2*acos(q[0])
|
||
|
s = sin(theta/2)
|
||
|
return [theta] + [a/s for a in q[1:]]
|
||
|
|
||
|
def conjugate(self):
|
||
|
return Quaternion(self[0], *(-a for a in self[1:]))
|
||
|
|
||
|
def inverse(self): # Reciprocal
|
||
|
return self.conjugate()/sum((d*d for d in self))
|
||
|
|
||
|
def __str__(self):
|
||
|
return 'w = {:4.2f} x = {:4.2f} y = {:4.2f} z = {:4.2f}'.format(*self)
|
||
|
|
||
|
def __pos__(self):
|
||
|
return Quaternion(*self)
|
||
|
|
||
|
def __neg__(self):
|
||
|
return Quaternion(*(-a for a in self))
|
||
|
|
||
|
def __truediv__(self, scalar):
|
||
|
if isinstance(scalar, Quaternion): # See docs for reason
|
||
|
raise ValueError('Cannot divide by Quaternion')
|
||
|
return Quaternion(*(a/scalar for a in self))
|
||
|
|
||
|
def __rtruediv__(self, other):
|
||
|
return self.inverse() * other
|
||
|
|
||
|
# Multiply by quaternion, list, tuple, or scalar: result = self * other
|
||
|
def __mul__(self, other):
|
||
|
if isinstance(other, Quaternion):
|
||
|
w1, x1, y1, z1 = self
|
||
|
w2, x2, y2, z2 = other
|
||
|
w = w1*w2 - x1*x2 - y1*y2 - z1*z2
|
||
|
x = w1*x2 + x1*w2 + y1*z2 - z1*y2
|
||
|
y = w1*y2 - x1*z2 + y1*w2 + z1*x2
|
||
|
z = w1*z2 + x1*y2 - y1*x2 + z1*w2
|
||
|
return Quaternion(w, x, y, z)
|
||
|
length = _arglen(other)
|
||
|
if length == 0: # Assume other is scalar
|
||
|
return Quaternion(*(a * other for a in self))
|
||
|
elif length == 3:
|
||
|
return Quaternion(0, *(a * b for a, b in zip(self[1:], other)))
|
||
|
# length == 4:
|
||
|
return Quaternion(*(a * b for a, b in zip(self, other)))
|
||
|
|
||
|
def __rmul__(self, other):
|
||
|
return self * other # Multiplication by scalars and tuples is commutative
|
||
|
|
||
|
def __add__(self, other):
|
||
|
if isinstance(other, Quaternion):
|
||
|
return Quaternion(*(a + b for a, b in zip(self, other)))
|
||
|
length = _arglen(other)
|
||
|
if length == 0: # Assume other is scalar
|
||
|
return Quaternion(self[0] + other, *self[1:]) # ? Is adding a scalar meaningful?
|
||
|
elif length == 3:
|
||
|
return Quaternion(0, *(a + b for a, b in zip(self[1:], other)))
|
||
|
# length == 4:
|
||
|
return Quaternion(*(a + b for a, b in zip(self, other)))
|
||
|
|
||
|
def __radd__(self, other):
|
||
|
return self.__add__(other)
|
||
|
|
||
|
def __sub__(self, other):
|
||
|
if isinstance(other, Quaternion):
|
||
|
return Quaternion(*(a - b for a, b in zip(self, other)))
|
||
|
length = _arglen(other)
|
||
|
if length == 0: # Assume other is scalar
|
||
|
return Quaternion(self[0] - other, *self[1:]) # ? Is this meaningful?
|
||
|
elif length == 3:
|
||
|
return Quaternion(0, *(a - b for a, b in zip(self[1:], other)))
|
||
|
# length == 4:
|
||
|
return Quaternion(*(a - b for a, b in zip(self, other)))
|
||
|
|
||
|
def __rsub__(self, other):
|
||
|
return other + self.__neg__() # via __radd__
|
||
|
|
||
|
def isrot(self):
|
||
|
return isclose(abs(self), 1.0, rel_tol = mdelta)
|
||
|
|
||
|
def isvec(self):
|
||
|
return isclose(self[0], 0, abs_tol = adelta)
|
||
|
|
||
|
def __matmul__(self, rot):
|
||
|
return rot * self * rot.conjugate()
|
||
|
|
||
|
def rrot(self, rot):
|
||
|
return rot.conjugate() * self * rot
|
||
|
|
||
|
# A vector quaternion has real part 0. It can represent a point in space.
|
||
|
def Vector(x, y, z):
|
||
|
return Quaternion(0, x, y, z)
|
||
|
|
||
|
Point = Vector
|
||
|
|
||
|
# A rotation quaternion is a unit quaternion i.e. magnitude == 1
|
||
|
def Rotator(theta=0, x=0, y=0, z=0):
|
||
|
s = sin(theta/2)
|
||
|
m = sqrt(x*x + y*y + z*z) # Convert to unit vector
|
||
|
if m > 0:
|
||
|
return Quaternion(cos(theta/2), s*x/m, s*y/m, s*z/m)
|
||
|
else:
|
||
|
return Quaternion(1, 0, 0, 0) # Identity quaternion
|
||
|
|
||
|
def Euler(heading, pitch, roll):
|
||
|
cy = cos(heading * 0.5);
|
||
|
sy = sin(heading * 0.5);
|
||
|
cp = cos(pitch * 0.5);
|
||
|
sp = sin(pitch * 0.5);
|
||
|
cr = cos(roll * 0.5);
|
||
|
sr = sin(roll * 0.5);
|
||
|
|
||
|
w = cr * cp * cy + sr * sp * sy;
|
||
|
x = sr * cp * cy - cr * sp * sy;
|
||
|
y = cr * sp * cy + sr * cp * sy;
|
||
|
z = cr * cp * sy - sr * sp * cy;
|
||
|
return Quaternion(w, x, y, z) # Tait-Bryan angles but z == towards sky
|