1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Jon Berndt, Mathis Froehlich
7 ------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------------
8 ------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
10 This program is free software; you can redistribute it and/or modify it under
11 the terms of the GNU General Public License as published by the Free Software
12 Foundation; either version 2 of the License, or (at your option) any later
15 This program is distributed in the hope that it will be useful, but WITHOUT
16 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
20 You should have received a copy of the GNU General Public License along with
21 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
22 Place - Suite 330, Boston, MA 02111-1307, USA.
24 Further information about the GNU General Public License can also be found on
25 the world wide web at http://www.gnu.org.
28 -------------------------------------------------------------------------------
30 15/01/04 MF Quaternion class from old FGColumnVector4
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
36 #ifndef FGQUATERNION_H
37 #define FGQUATERNION_H
39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 #include "FGJSBBase.h"
44 #include "FGMatrix33.h"
45 #include "FGColumnVector3.h"
46 #include "FGPropertyManager.h"
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 #define ID_QUATERNION "$Id$"
56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 /** Models the Quaternion representation of rotations.
61 FGQuaternion is a representation of an arbitrary rotation through a
62 quaternion. It has vector properties. This class also contains access
63 functions to the euler angle representation of rotations and access to
64 transformation matrices for 3D vectors. Transformations and euler angles are
65 therefore computed once they are requested for the first time. Then they are
66 cached for later usage as long as the class is not accessed trough
67 a nonconst member function.
69 Note: The order of rotations used in this class corresponds to a 3-2-1 sequence,
70 or Y-P-R, or Z-Y-X, if you prefer.
72 @see Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
73 Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
75 @see D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
77 @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
78 NASA-Ames", NASA CR-2497, January 1975
79 @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
80 Wiley & Sons, 1979 ISBN 0-471-03032-5
81 @see Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
82 1982 ISBN 0-471-08936-2
83 @author Mathias Froehlich, extended FGColumnVector4 originally by Tony Peden
87 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
89 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
94 /** Default initializer.
95 Default initializer, initializes the class with the identity rotation. */
96 FGQuaternion() : mCacheValid(false) {
98 Entry(2) = Entry(3) = Entry(4) = 0.0;
101 /** Copy constructor.
102 Copy constructor, initializes the quaternion.
103 @param q a constant reference to another FGQuaternion instance */
104 FGQuaternion(const FGQuaternion& q);
106 /** Initializer by euler angles.
107 Initialize the quaternion with the euler angles.
108 @param phi The euler X axis (roll) angle in radians
109 @param tht The euler Y axis (attitude) angle in radians
110 @param psi The euler Z axis (heading) angle in radians */
111 FGQuaternion(double phi, double tht, double psi);
113 /** Initializer by one euler angle.
114 Initialize the quaternion with the single euler angle where its index
115 is given in the first argument.
116 @param idx Index of the euler angle to initialize
117 @param angle The euler angle in radians */
118 FGQuaternion(int idx, double angle)
119 : mCacheValid(false) {
120 double angle2 = 0.5*angle;
122 double Sangle2 = sin(angle2);
123 double Cangle2 = cos(angle2);
131 } else if (idx == eTht) {
149 /** Quaternion 'velocity' for given angular rates.
150 Computes the quaternion derivative which results from the given
152 @param PQR a constant reference to the body rate vector
153 @return the quaternion derivative */
154 FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
156 /** Transformation matrix.
157 @return a reference to the transformation/rotation matrix
158 corresponding to this quaternion rotation. */
159 const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
161 /** Backward transformation matrix.
162 @return a reference to the inverse transformation/rotation matrix
163 corresponding to this quaternion rotation. */
164 const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
166 /** Retrieves the Euler angles.
167 @return a reference to the triad of euler angles corresponding
168 to this quaternion rotation.
170 const FGColumnVector3& GetEuler(void) const {
175 /** Retrieves the Euler angles.
176 @param i the euler angle index.
177 @return a reference to the i-th euler angles corresponding
178 to this quaternion rotation.
180 double GetEuler(int i) const {
182 return mEulerAngles(i);
185 /** Retrieves the Euler angles.
186 @param i the euler angle index.
187 @return a reference to the i-th euler angles corresponding
188 to this quaternion rotation.
190 double GetEulerDeg(int i) const {
192 return radtodeg*mEulerAngles(i);
195 /** Retrieves sine of the given euler angle.
196 @return the sine of the Euler angle theta (pitch attitude) corresponding
197 to this quaternion rotation. */
198 double GetSinEuler(int i) const {
200 return mEulerSines(i);
203 /** Retrieves cosine of the given euler angle.
204 @return the sine of the Euler angle theta (pitch attitude) corresponding
205 to this quaternion rotation. */
206 double GetCosEuler(int i) const {
208 return mEulerCosines(i);
211 /** Read access the entries of the vector.
213 @param idx the component index.
215 Return the value of the matrix entry at the given index.
216 Indices are counted starting with 1.
218 Note that the index given in the argument is unchecked.
220 double operator()(unsigned int idx) const { return Entry(idx); }
222 /** Write access the entries of the vector.
224 @param idx the component index.
226 Return a reference to the vector entry at the given index.
227 Indices are counted starting with 1.
229 Note that the index given in the argument is unchecked.
231 double& operator()(unsigned int idx) { return Entry(idx); }
233 /** Read access the entries of the vector.
235 @param idx the component index.
237 Return the value of the matrix entry at the given index.
238 Indices are counted starting with 1.
240 This function is just a shortcut for the @ref double
241 operator()(unsigned int idx) const function. It is
242 used internally to access the elements in a more convenient way.
244 Note that the index given in the argument is unchecked.
246 double Entry(unsigned int idx) const { return mData[idx-1]; }
248 /** Write access the entries of the vector.
250 @param idx the component index.
252 Return a reference to the vector entry at the given index.
253 Indices are counted starting with 1.
255 This function is just a shortcut for the @ref double&
256 operator()(unsigned int idx) function. It is
257 used internally to access the elements in a more convenient way.
259 Note that the index given in the argument is unchecked.
261 double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
263 /** Assignment operator "=".
264 Assign the value of q to the current object. Cached values are
266 @param q reference to an FGQuaternion instance
267 @return reference to a quaternion object */
268 const FGQuaternion& operator=(const FGQuaternion& q) {
269 // Copy the master values ...
274 // .. and copy the derived values if they are valid
275 mCacheValid = q.mCacheValid;
279 mEulerAngles = q.mEulerAngles;
280 mEulerSines = q.mEulerSines;
281 mEulerCosines = q.mEulerCosines;
286 /** Comparison operator "==".
287 @param q a quaternion reference
288 @return true if both quaternions represent the same rotation. */
289 bool operator==(const FGQuaternion& q) const {
290 return Entry(1) == q(1) && Entry(2) == q(2)
291 && Entry(3) == q(3) && Entry(4) == q(4);
294 /** Comparison operator "!=".
295 @param q a quaternion reference
296 @return true if both quaternions do not represent the same rotation. */
297 bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
298 const FGQuaternion& operator+=(const FGQuaternion& q) {
299 // Copy the master values ...
308 /** Arithmetic operator "-=".
309 @param q a quaternion reference.
310 @return a quaternion reference representing Q, where Q = Q - q. */
311 const FGQuaternion& operator-=(const FGQuaternion& q) {
312 // Copy the master values ...
321 /** Arithmetic operator "*=".
322 @param scalar a multiplicative value.
323 @return a quaternion reference representing Q, where Q = Q * scalar. */
324 const FGQuaternion& operator*=(double scalar) {
333 /** Arithmetic operator "/=".
334 @param scalar a divisor value.
335 @return a quaternion reference representing Q, where Q = Q / scalar. */
336 const FGQuaternion& operator/=(double scalar) {
337 return operator*=(1.0/scalar);
340 /** Arithmetic operator "+".
341 @param q a quaternion to be summed.
342 @return a quaternion representing Q, where Q = Q + q. */
343 FGQuaternion operator+(const FGQuaternion& q) const {
344 return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
345 Entry(3)+q(3), Entry(4)+q(4));
348 /** Arithmetic operator "-".
349 @param q a quaternion to be subtracted.
350 @return a quaternion representing Q, where Q = Q - q. */
351 FGQuaternion operator-(const FGQuaternion& q) const {
352 return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
353 Entry(3)-q(3), Entry(4)-q(4));
356 /** Arithmetic operator "*".
357 Multiplication of two quaternions is like performing successive rotations.
358 @param q a quaternion to be multiplied.
359 @return a quaternion representing Q, where Q = Q * q. */
360 FGQuaternion operator*(const FGQuaternion& q) const {
361 return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
362 Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
363 Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
364 Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
367 /** Arithmetic operator "*=".
368 Multiplication of two quaternions is like performing successive rotations.
369 @param q a quaternion to be multiplied.
370 @return a quaternion reference representing Q, where Q = Q * q. */
371 const FGQuaternion& operator*=(const FGQuaternion& q) {
372 double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
373 double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
374 double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
375 double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
384 /** Inverse of the quaternion.
386 Compute and return the inverse of the quaternion so that the orientation
387 represented with *this multiplied with the returned value is equal to
388 the identity orientation.
390 FGQuaternion Inverse(void) const {
391 double norm = Magnitude();
394 double rNorm = 1.0/norm;
395 return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
396 -Entry(3)*rNorm, -Entry(4)*rNorm );
399 /** Conjugate of the quaternion.
401 Compute and return the conjugate of the quaternion. This one is equal
402 to the inverse iff the quaternion is normalized.
404 FGQuaternion Conjugate(void) const {
405 return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
408 friend FGQuaternion operator*(double, const FGQuaternion&);
410 /** Length of the vector.
412 Compute and return the euclidean norm of this vector.
414 double Magnitude(void) const { return sqrt(SqrMagnitude()); }
416 /** Square of the length of the vector.
418 Compute and return the square of the euclidean norm of this vector.
420 double SqrMagnitude(void) const {
421 return Entry(1)*Entry(1)+Entry(2)*Entry(2)
422 +Entry(3)*Entry(3)+Entry(4)*Entry(4);
427 Normalize the vector to have the Magnitude() == 1.0. If the vector
428 is equal to zero it is left untouched.
430 void Normalize(void);
432 /** Zero quaternion vector. Does not represent any orientation.
433 Useful for initialization of increments */
434 static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
437 /** Copying by assigning the vector valued components. */
438 FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
439 { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
441 /** Computation of derived values.
442 This function recomputes the derived values like euler angles and
443 transformation matrices. It does this unconditionally. */
444 void ComputeDerivedUnconditional(void) const;
446 /** Computation of derived values.
447 This function checks if the derived values like euler angles and
448 transformation matrices are already computed. If so, it
449 returns. If they need to be computed the real worker routine
450 \ref FGQuaternion::ComputeDerivedUnconditional(void) const
452 This function is inlined to avoid function calls in the fast path. */
453 void ComputeDerived(void) const {
455 ComputeDerivedUnconditional();
458 /** The quaternion values itself. This is the master copy. */
461 /** A data validity flag.
462 This class implements caching of the derived values like the
463 orthogonal rotation matrices or the Euler angles. For caching we
464 carry a flag which signals if the values are valid or not.
465 The C++ keyword "mutable" tells the compiler that the data member is
466 allowed to change during a const member function. */
467 mutable bool mCacheValid;
469 /** This stores the transformation matrices. */
470 mutable FGMatrix33 mT;
471 mutable FGMatrix33 mTInv;
473 /** The cached euler angles. */
474 mutable FGColumnVector3 mEulerAngles;
476 /** The cached sines and cosines of the euler angles. */
477 mutable FGColumnVector3 mEulerSines;
478 mutable FGColumnVector3 mEulerCosines;
481 /** Scalar multiplication.
483 @param scalar scalar value to multiply with.
484 @param p Vector to multiply.
486 Multiply the Vector with a scalar value.
488 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
489 return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
492 } // namespace JSBSim