* Added Flightgear multiplayer output interface to uhd_modes.py. This allows flight with live traffic in fgfs. * Quaternion library borrowed from PyPi and modified to generate angle/axis representation and construct rotation quat from lat/lon to ECEF. * Miscellaneous enhancements and cleanup to fix timestamps, add aircraft type field (seems to be unused anyway), turnrate infopull/14/head
Nick Foster
12 years ago
parent
c16886c4c1
commit
17f7cb9a53
@ -0,0 +1,441 @@
|
||||
"""
|
||||
Quaternion provides a class for manipulating quaternion objects. This class provides:
|
||||
|
||||
- a convenient constructor to convert to/from Euler Angles (RA,Dec,Roll)
|
||||
to/from quaternions
|
||||
- class methods to multiply and divide quaternions
|
||||
"""
|
||||
|
||||
"""Copyright 2009 Smithsonian Astrophysical Observatory
|
||||
Released under New BSD / 3-Clause BSD License
|
||||
All rights reserved
|
||||
"""
|
||||
"""
|
||||
Modified 2012 by Nick Foster
|
||||
Modified from version 0.3.1
|
||||
http://pypi.python.org/pypi/Quaternion/0.03
|
||||
Added _get_angle_axis to get the angle-axis representation
|
||||
Added _latlontoquat to get a rotation quat to ECEF from lat/lon
|
||||
"""
|
||||
|
||||
|
||||
|
||||
import numpy as np
|
||||
from math import cos, sin, radians, degrees, atan2, sqrt, acos, pi
|
||||
|
||||
class Quat(object):
|
||||
"""
|
||||
Quaternion class
|
||||
|
||||
Example usage::
|
||||
|
||||
>>> from Quaternion import Quat
|
||||
>>> quat = Quat((12,45,45))
|
||||
>>> quat.ra, quat.dec, quat.roll
|
||||
(12, 45, 45)
|
||||
>>> quat.q
|
||||
array([ 0.38857298, -0.3146602 , 0.23486498, 0.8335697 ])
|
||||
>>> q2 = Quat([ 0.38857298, -0.3146602 , 0.23486498, 0.8335697])
|
||||
>>> q2.ra
|
||||
11.999999315925008
|
||||
|
||||
|
||||
Multiplication and division operators are overloaded for the class to
|
||||
perform appropriate quaternion multiplication and division.
|
||||
|
||||
Example usage::
|
||||
|
||||
>>> q1 = Quat((20,30,40))
|
||||
>>> q2 = Quat((30,40,50))
|
||||
>>> q = q1 / q2
|
||||
|
||||
Performs the operation as q1 * inverse q2
|
||||
|
||||
Example usage::
|
||||
|
||||
>>> q1 = Quat((20,30,40))
|
||||
>>> q2 = Quat((30,40,50))
|
||||
>>> q = q1 * q2
|
||||
|
||||
|
||||
:param attitude: initialization attitude for quat
|
||||
|
||||
``attitude`` may be:
|
||||
* another Quat
|
||||
* a 4 element array (expects x,y,z,w quat form)
|
||||
* a 3 element array (expects ra,dec,roll in degrees)
|
||||
* a 3x3 transform/rotation matrix
|
||||
|
||||
"""
|
||||
def __init__(self, attitude):
|
||||
self._q = None
|
||||
self._equatorial = None
|
||||
self._T = None
|
||||
# checks to see if we've been passed a Quat
|
||||
if isinstance(attitude, Quat):
|
||||
self._set_q(attitude.q)
|
||||
else:
|
||||
# make it an array and check to see if it is a supported shape
|
||||
attitude = np.array(attitude)
|
||||
if len(attitude) == 4:
|
||||
self._set_q(attitude)
|
||||
elif attitude.shape == (3,3):
|
||||
self._set_transform(attitude)
|
||||
elif attitude.shape == (3,):
|
||||
self._set_equatorial(attitude)
|
||||
elif attitude.shape == (2,):
|
||||
self._set_latlon(attitude)
|
||||
else:
|
||||
raise TypeError("attitude is not one of possible types (2, 3 or 4 elements, Quat, or 3x3 matrix)")
|
||||
|
||||
|
||||
def _set_q(self, q):
|
||||
"""
|
||||
Set the value of the 4 element quaternion vector
|
||||
|
||||
:param q: list or array of normalized quaternion elements
|
||||
"""
|
||||
q = np.array(q)
|
||||
if abs(np.sum(q**2) - 1.0) > 1e-6:
|
||||
raise ValueError('Quaternion must be normalized so sum(q**2) == 1; use Quaternion.normalize')
|
||||
self._q = (q if q[3] > 0 else -q)
|
||||
# Erase internal values of other representations
|
||||
self._equatorial = None
|
||||
self._T = None
|
||||
|
||||
def _get_q(self):
|
||||
"""
|
||||
Retrieve 4-vector of quaternion elements in [x, y, z, w] form
|
||||
|
||||
:rtype: numpy array
|
||||
|
||||
"""
|
||||
if self._q is None:
|
||||
# Figure out q from available values, doing nothing others are not defined
|
||||
if self._equatorial is not None:
|
||||
self._q = self._equatorial2quat()
|
||||
elif self._T is not None:
|
||||
self._q = self._transform2quat()
|
||||
return self._q
|
||||
|
||||
# use property to make this get/set automatic
|
||||
q = property(_get_q, _set_q)
|
||||
|
||||
def _set_equatorial(self, equatorial):
|
||||
"""Set the value of the 3 element equatorial coordinate list [RA,Dec,Roll]
|
||||
expects values in degrees
|
||||
bounds are not checked
|
||||
|
||||
:param equatorial: list or array [ RA, Dec, Roll] in degrees
|
||||
|
||||
"""
|
||||
att = np.array(equatorial)
|
||||
ra, dec, roll = att
|
||||
self._ra0 = ra
|
||||
if ( ra > 180 ):
|
||||
self._ra0 = ra - 360
|
||||
self._roll0 = roll
|
||||
if ( roll > 180):
|
||||
self._roll0 = roll - 360
|
||||
self._equatorial = att
|
||||
|
||||
def _set_latlon(self, latlon):
|
||||
self._q = self._latlontoquat(latlon)
|
||||
|
||||
def _get_equatorial(self):
|
||||
"""Retrieve [RA, Dec, Roll]
|
||||
|
||||
:rtype: numpy array
|
||||
"""
|
||||
if self._equatorial is None:
|
||||
if self._q is not None:
|
||||
self._equatorial = self._quat2equatorial()
|
||||
elif self._T is not None:
|
||||
self._q = self._transform2quat()
|
||||
self._equatorial = self._quat2equatorial()
|
||||
return self._equatorial
|
||||
|
||||
equatorial = property(_get_equatorial,_set_equatorial)
|
||||
|
||||
def _get_ra(self):
|
||||
"""Retrieve RA term from equatorial system in degrees"""
|
||||
return self.equatorial[0]
|
||||
|
||||
def _get_dec(self):
|
||||
"""Retrieve Dec term from equatorial system in degrees"""
|
||||
return self.equatorial[1]
|
||||
|
||||
def _get_roll(self):
|
||||
"""Retrieve Roll term from equatorial system in degrees"""
|
||||
return self.equatorial[2]
|
||||
|
||||
ra = property(_get_ra)
|
||||
dec = property(_get_dec)
|
||||
roll = property(_get_roll)
|
||||
|
||||
def _set_transform(self, T):
|
||||
"""
|
||||
Set the value of the 3x3 rotation/transform matrix
|
||||
|
||||
:param T: 3x3 array/numpy array
|
||||
"""
|
||||
transform = np.array(T)
|
||||
self._T = transform
|
||||
|
||||
def _get_transform(self):
|
||||
"""
|
||||
Retrieve the value of the 3x3 rotation/transform matrix
|
||||
|
||||
:returns: 3x3 rotation/transform matrix
|
||||
:rtype: numpy array
|
||||
|
||||
"""
|
||||
if self._T is None:
|
||||
if self._q is not None:
|
||||
self._T = self._quat2transform()
|
||||
elif self._equatorial is not None:
|
||||
self._T = self._equatorial2transform()
|
||||
return self._T
|
||||
|
||||
transform = property(_get_transform, _set_transform)
|
||||
|
||||
def _quat2equatorial(self):
|
||||
"""
|
||||
Determine Right Ascension, Declination, and Roll for the object quaternion
|
||||
|
||||
:returns: RA, Dec, Roll
|
||||
:rtype: numpy array [ra,dec,roll]
|
||||
"""
|
||||
|
||||
q = self.q
|
||||
q2 = self.q**2
|
||||
|
||||
## calculate direction cosine matrix elements from $quaternions
|
||||
xa = q2[0] - q2[1] - q2[2] + q2[3]
|
||||
xb = 2 * (q[0] * q[1] + q[2] * q[3])
|
||||
xn = 2 * (q[0] * q[2] - q[1] * q[3])
|
||||
yn = 2 * (q[1] * q[2] + q[0] * q[3])
|
||||
zn = q2[3] + q2[2] - q2[0] - q2[1]
|
||||
|
||||
##; calculate RA, Dec, Roll from cosine matrix elements
|
||||
ra = degrees(atan2(xb , xa)) ;
|
||||
dec = degrees(atan2(xn , sqrt(1 - xn**2)));
|
||||
roll = degrees(atan2(yn , zn)) ;
|
||||
if ( ra < 0 ):
|
||||
ra += 360
|
||||
if ( roll < 0 ):
|
||||
roll += 360
|
||||
|
||||
return np.array([ra, dec, roll])
|
||||
|
||||
|
||||
def _quat2transform(self):
|
||||
"""
|
||||
Transform a unit quaternion into its corresponding rotation matrix (to
|
||||
be applied on the right side).
|
||||
|
||||
:returns: transform matrix
|
||||
:rtype: numpy array
|
||||
|
||||
"""
|
||||
x, y, z, w = self.q
|
||||
xx2 = 2 * x * x
|
||||
yy2 = 2 * y * y
|
||||
zz2 = 2 * z * z
|
||||
xy2 = 2 * x * y
|
||||
wz2 = 2 * w * z
|
||||
zx2 = 2 * z * x
|
||||
wy2 = 2 * w * y
|
||||
yz2 = 2 * y * z
|
||||
wx2 = 2 * w * x
|
||||
|
||||
rmat = np.empty((3, 3), float)
|
||||
rmat[0,0] = 1. - yy2 - zz2
|
||||
rmat[0,1] = xy2 - wz2
|
||||
rmat[0,2] = zx2 + wy2
|
||||
rmat[1,0] = xy2 + wz2
|
||||
rmat[1,1] = 1. - xx2 - zz2
|
||||
rmat[1,2] = yz2 - wx2
|
||||
rmat[2,0] = zx2 - wy2
|
||||
rmat[2,1] = yz2 + wx2
|
||||
rmat[2,2] = 1. - xx2 - yy2
|
||||
|
||||
return rmat
|
||||
|
||||
def _equatorial2quat( self ):
|
||||
"""Dummy method to return return quat.
|
||||
|
||||
:returns: quaternion
|
||||
:rtype: Quat
|
||||
|
||||
"""
|
||||
return self._transform2quat()
|
||||
|
||||
def _equatorial2transform( self ):
|
||||
"""Construct the transform/rotation matrix from RA,Dec,Roll
|
||||
|
||||
:returns: transform matrix
|
||||
:rtype: 3x3 numpy array
|
||||
|
||||
"""
|
||||
ra = radians(self._get_ra())
|
||||
dec = radians(self._get_dec())
|
||||
roll = radians(self._get_roll())
|
||||
ca = cos(ra)
|
||||
sa = sin(ra)
|
||||
cd = cos(dec)
|
||||
sd = sin(dec)
|
||||
cr = cos(roll)
|
||||
sr = sin(roll)
|
||||
|
||||
# This is the transpose of the transformation matrix (related to translation
|
||||
# of original perl code
|
||||
rmat = np.array([[ca * cd, sa * cd, sd ],
|
||||
[-ca * sd * sr - sa * cr, -sa * sd * sr + ca * cr, cd * sr],
|
||||
[-ca * sd * cr + sa * sr, -sa * sd * cr - ca * sr, cd * cr]])
|
||||
|
||||
return rmat.transpose()
|
||||
|
||||
def _transform2quat( self ):
|
||||
"""Construct quaternion from the transform/rotation matrix
|
||||
|
||||
:returns: quaternion formed from transform matrix
|
||||
:rtype: numpy array
|
||||
"""
|
||||
|
||||
# Code was copied from perl PDL code that uses backwards index ordering
|
||||
T = self.transform.transpose()
|
||||
den = np.array([ 1.0 + T[0,0] - T[1,1] - T[2,2],
|
||||
1.0 - T[0,0] + T[1,1] - T[2,2],
|
||||
1.0 - T[0,0] - T[1,1] + T[2,2],
|
||||
1.0 + T[0,0] + T[1,1] + T[2,2]])
|
||||
|
||||
max_idx = np.flatnonzero(den == max(den))[0]
|
||||
|
||||
q = np.zeros(4)
|
||||
q[max_idx] = 0.5 * sqrt(max(den))
|
||||
denom = 4.0 * q[max_idx]
|
||||
if (max_idx == 0):
|
||||
q[1] = (T[1,0] + T[0,1]) / denom
|
||||
q[2] = (T[2,0] + T[0,2]) / denom
|
||||
q[3] = -(T[2,1] - T[1,2]) / denom
|
||||
if (max_idx == 1):
|
||||
q[0] = (T[1,0] + T[0,1]) / denom
|
||||
q[2] = (T[2,1] + T[1,2]) / denom
|
||||
q[3] = -(T[0,2] - T[2,0]) / denom
|
||||
if (max_idx == 2):
|
||||
q[0] = (T[2,0] + T[0,2]) / denom
|
||||
q[1] = (T[2,1] + T[1,2]) / denom
|
||||
q[3] = -(T[1,0] - T[0,1]) / denom
|
||||
if (max_idx == 3):
|
||||
q[0] = -(T[2,1] - T[1,2]) / denom
|
||||
q[1] = -(T[0,2] - T[2,0]) / denom
|
||||
q[2] = -(T[1,0] - T[0,1]) / denom
|
||||
|
||||
return q
|
||||
|
||||
def _get_angle_axis(self):
|
||||
lim = 1e-12
|
||||
norm = np.linalg.norm(self.q)
|
||||
if norm < lim:
|
||||
angle = 0
|
||||
axis = [0,0,0]
|
||||
else:
|
||||
rnorm = 1.0 / norm
|
||||
angle = acos(max(-1, min(1, rnorm*self.q[3])));
|
||||
sangle = sin(angle)
|
||||
if sangle < lim:
|
||||
axis = [0,0,0]
|
||||
else:
|
||||
axis = (rnorm / sangle) * np.array(self.q[0:3])
|
||||
|
||||
angle *= 2
|
||||
|
||||
return (angle, axis)
|
||||
|
||||
def _latlontoquat ( self, latlon ):
|
||||
q = np.zeros(4)
|
||||
|
||||
lon = latlon[1]*(pi/180.)
|
||||
lat = latlon[0]*(pi/180.)
|
||||
zd2 = 0.5*lon
|
||||
yd2 = -0.25*pi - 0.5*lat
|
||||
Szd2 = sin(zd2)
|
||||
Syd2 = sin(yd2)
|
||||
Czd2 = cos(zd2)
|
||||
Cyd2 = cos(yd2)
|
||||
q[0] = -Szd2*Syd2
|
||||
q[1] = Czd2*Syd2
|
||||
q[2] = Szd2*Cyd2
|
||||
q[3] = Czd2*Cyd2
|
||||
|
||||
return q
|
||||
|
||||
def __div__(self, quat2):
|
||||
"""
|
||||
Divide one quaternion by another.
|
||||
|
||||
Example usage::
|
||||
|
||||
>>> q1 = Quat((20,30,40))
|
||||
>>> q2 = Quat((30,40,50))
|
||||
>>> q = q1 / q2
|
||||
|
||||
Performs the operation as q1 * inverse q2
|
||||
|
||||
:returns: product q1 * inverse q2
|
||||
:rtype: Quat
|
||||
|
||||
"""
|
||||
return self * quat2.inv()
|
||||
|
||||
|
||||
def __mul__(self, quat2):
|
||||
"""
|
||||
Multiply quaternion by another.
|
||||
|
||||
Example usage::
|
||||
|
||||
>>> q1 = Quat((20,30,40))
|
||||
>>> q2 = Quat((30,40,50))
|
||||
>>> (q1 * q2).equatorial
|
||||
array([ 349.73395729, 76.25393056, 127.61636727])
|
||||
|
||||
:returns: product q1 * q2
|
||||
:rtype: Quat
|
||||
|
||||
"""
|
||||
q1 = self.q
|
||||
q2 = quat2.q
|
||||
mult = np.zeros(4)
|
||||
mult[0] = q1[3]*q2[0] - q1[2]*q2[1] + q1[1]*q2[2] + q1[0]*q2[3]
|
||||
mult[1] = q1[2]*q2[0] + q1[3]*q2[1] - q1[0]*q2[2] + q1[1]*q2[3]
|
||||
mult[2] = -q1[1]*q2[0] + q1[0]*q2[1] + q1[3]*q2[2] + q1[2]*q2[3]
|
||||
mult[3] = -q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] + q1[3]*q2[3]
|
||||
return Quat(mult)
|
||||
|
||||
def inv(self):
|
||||
"""
|
||||
Invert the quaternion
|
||||
|
||||
:returns: inverted quaternion
|
||||
:rtype: Quat
|
||||
"""
|
||||
return Quat([self.q[0], self.q[1], self.q[2], -self.q[3]])
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def normalize(array):
|
||||
"""
|
||||
Normalize a 4 element array/list/numpy.array for use as a quaternion
|
||||
|
||||
:param quat_array: 4 element list/array
|
||||
:returns: normalized array
|
||||
:rtype: numpy array
|
||||
|
||||
"""
|
||||
quat = np.array(array)
|
||||
return quat / np.sqrt(np.dot(quat, quat))
|
||||
|
||||
Loading…
Reference in new issue