1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Jon Berndt, Mathis Froehlich
7 ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.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 Lesser 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 Lesser General Public License for more
20 You should have received a copy of the GNU Lesser 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 Lesser 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 "FGColumnVector3.h"
46 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
48 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
50 #define ID_QUATERNION "$Id: FGQuaternion.h,v 1.17 2010/06/30 03:13:40 jberndt Exp $"
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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
91 class FGQuaternion : virtual FGJSBBase {
93 /** Default initializer.
94 Default initializer, initializes the class with the identity rotation. */
95 FGQuaternion() : mCacheValid(false) {
97 data[1] = data[2] = data[3] = 0.0;
100 /** Copy constructor.
101 Copy constructor, initializes the quaternion.
102 @param q a constant reference to another FGQuaternion instance */
103 FGQuaternion(const FGQuaternion& q);
105 /** Initializer by euler angles.
106 Initialize the quaternion with the euler angles.
107 @param phi The euler X axis (roll) angle in radians
108 @param tht The euler Y axis (attitude) angle in radians
109 @param psi The euler Z axis (heading) angle in radians */
110 FGQuaternion(double phi, double tht, double psi);
112 /** Initializer by euler angle vector.
113 Initialize the quaternion with the euler angle vector.
114 @param vOrient The euler axis angle vector in radians (phi, tht, psi) */
115 FGQuaternion(FGColumnVector3 vOrient);
117 /** Initializer by one euler angle.
118 Initialize the quaternion with the single euler angle where its index
119 is given in the first argument.
120 @param idx Index of the euler angle to initialize
121 @param angle The euler angle in radians */
122 FGQuaternion(int idx, double angle)
123 : mCacheValid(false) {
125 double angle2 = 0.5*angle;
127 double Sangle2 = sin(angle2);
128 double Cangle2 = cos(angle2);
136 } else if (idx == eTht) {
151 /** Initializer by matrix.
152 Initialize the quaternion with the matrix representing a transform from one frame
153 to another using the standard aerospace sequence, Yaw-Pitch-Roll (3-2-1).
154 @param m the rotation matrix */
155 FGQuaternion(const FGMatrix33& m);
160 /** Quaternion derivative for given angular rates.
161 Computes the quaternion derivative which results from the given
163 @param PQR a constant reference to a rotation rate vector
164 @return the quaternion derivative
165 @see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
167 FGQuaternion GetQDot(const FGColumnVector3& PQR);
169 /** Transformation matrix.
170 @return a reference to the transformation/rotation matrix
171 corresponding to this quaternion rotation. */
172 const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
174 /** Backward transformation matrix.
175 @return a reference to the inverse transformation/rotation matrix
176 corresponding to this quaternion rotation. */
177 const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
179 /** Retrieves the Euler angles.
180 @return a reference to the triad of euler angles corresponding
181 to this quaternion rotation.
183 const FGColumnVector3& GetEuler(void) const {
188 /** Retrieves the Euler angles.
189 @param i the euler angle index.
191 @return a reference to the i-th euler angles corresponding
192 to this quaternion rotation.
194 double GetEuler(int i) const {
196 return mEulerAngles(i);
199 /** Retrieves the Euler angles.
200 @param i the euler angle index.
201 @return a reference to the i-th euler angles corresponding
202 to this quaternion rotation.
204 double GetEulerDeg(int i) const {
206 return radtodeg*mEulerAngles(i);
209 /** Retrieves sine of the given euler angle.
210 @return the sine of the Euler angle theta (pitch attitude) corresponding
211 to this quaternion rotation. */
212 double GetSinEuler(int i) const {
214 return mEulerSines(i);
217 /** Retrieves cosine of the given euler angle.
218 @return the sine of the Euler angle theta (pitch attitude) corresponding
219 to this quaternion rotation. */
220 double GetCosEuler(int i) const {
222 return mEulerCosines(i);
225 /** Read access the entries of the vector.
227 @param idx the component index.
229 Return the value of the matrix entry at the given index.
230 Indices are counted starting with 1.
232 Note that the index given in the argument is unchecked.
234 double operator()(unsigned int idx) const { return data[idx-1]; }
236 /** Write access the entries of the vector.
238 @param idx the component index.
240 Return a reference to the vector entry at the given index.
241 Indices are counted starting with 1.
243 Note that the index given in the argument is unchecked.
245 double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
247 /** Read access the entries of the vector.
249 @param idx the component index.
251 Return the value of the matrix entry at the given index.
252 Indices are counted starting with 1.
254 This function is just a shortcut for the <tt>double
255 operator()(unsigned int idx) const</tt> function. It is
256 used internally to access the elements in a more convenient way.
258 Note that the index given in the argument is unchecked.
260 double Entry(unsigned int idx) const { return data[idx-1]; }
262 /** Write access the entries of the vector.
264 @param idx the component index.
266 Return a reference to the vector entry at the given index.
267 Indices are counted starting with 1.
269 This function is just a shortcut for the <tt>double&
270 operator()(unsigned int idx)</tt> function. It is
271 used internally to access the elements in a more convenient way.
273 Note that the index given in the argument is unchecked.
275 double& Entry(unsigned int idx) {
280 /** Assignment operator "=".
281 Assign the value of q to the current object. Cached values are
283 @param q reference to an FGQuaternion instance
284 @return reference to a quaternion object */
285 const FGQuaternion& operator=(const FGQuaternion& q) {
286 // Copy the master values ...
292 // .. and copy the derived values if they are valid
293 mCacheValid = q.mCacheValid;
297 mEulerAngles = q.mEulerAngles;
298 mEulerSines = q.mEulerSines;
299 mEulerCosines = q.mEulerCosines;
304 /// Conversion from Quat to Matrix
305 operator FGMatrix33() const { return GetT(); }
307 /** Comparison operator "==".
308 @param q a quaternion reference
309 @return true if both quaternions represent the same rotation. */
310 bool operator==(const FGQuaternion& q) const {
311 return data[0] == q(1) && data[1] == q(2)
312 && data[2] == q(3) && data[3] == q(4);
315 /** Comparison operator "!=".
316 @param q a quaternion reference
317 @return true if both quaternions do not represent the same rotation. */
318 bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
319 const FGQuaternion& operator+=(const FGQuaternion& q) {
320 // Copy the master values ...
329 /** Arithmetic operator "-=".
330 @param q a quaternion reference.
331 @return a quaternion reference representing Q, where Q = Q - q. */
332 const FGQuaternion& operator-=(const FGQuaternion& q) {
333 // Copy the master values ...
342 /** Arithmetic operator "*=".
343 @param scalar a multiplicative value.
344 @return a quaternion reference representing Q, where Q = Q * scalar. */
345 const FGQuaternion& operator*=(double scalar) {
354 /** Arithmetic operator "/=".
355 @param scalar a divisor value.
356 @return a quaternion reference representing Q, where Q = Q / scalar. */
357 const FGQuaternion& operator/=(double scalar) {
358 return operator*=(1.0/scalar);
361 /** Arithmetic operator "+".
362 @param q a quaternion to be summed.
363 @return a quaternion representing Q, where Q = Q + q. */
364 FGQuaternion operator+(const FGQuaternion& q) const {
365 return FGQuaternion(data[0]+q(1), data[1]+q(2),
366 data[2]+q(3), data[3]+q(4));
369 /** Arithmetic operator "-".
370 @param q a quaternion to be subtracted.
371 @return a quaternion representing Q, where Q = Q - q. */
372 FGQuaternion operator-(const FGQuaternion& q) const {
373 return FGQuaternion(data[0]-q(1), data[1]-q(2),
374 data[2]-q(3), data[3]-q(4));
377 /** Arithmetic operator "*".
378 Multiplication of two quaternions is like performing successive rotations.
379 @param q a quaternion to be multiplied.
380 @return a quaternion representing Q, where Q = Q * q. */
381 FGQuaternion operator*(const FGQuaternion& q) const {
382 return FGQuaternion(data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4),
383 data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3),
384 data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2),
385 data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1));
388 /** Arithmetic operator "*=".
389 Multiplication of two quaternions is like performing successive rotations.
390 @param q a quaternion to be multiplied.
391 @return a quaternion reference representing Q, where Q = Q * q. */
392 const FGQuaternion& operator*=(const FGQuaternion& q) {
393 double q0 = data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4);
394 double q1 = data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3);
395 double q2 = data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2);
396 double q3 = data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1);
405 /** Inverse of the quaternion.
407 Compute and return the inverse of the quaternion so that the orientation
408 represented with *this multiplied with the returned value is equal to
409 the identity orientation.
411 FGQuaternion Inverse(void) const {
412 double norm = SqrMagnitude();
415 double rNorm = 1.0/norm;
416 return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
417 -data[2]*rNorm, -data[3]*rNorm );
420 /** Conjugate of the quaternion.
422 Compute and return the conjugate of the quaternion. This one is equal
423 to the inverse iff the quaternion is normalized.
425 FGQuaternion Conjugate(void) const {
426 return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
429 friend FGQuaternion operator*(double, const FGQuaternion&);
431 /** Length of the vector.
433 Compute and return the euclidean norm of this vector.
435 double Magnitude(void) const { return sqrt(SqrMagnitude()); }
437 /** Square of the length of the vector.
439 Compute and return the square of the euclidean norm of this vector.
441 double SqrMagnitude(void) const {
442 return data[0]*data[0] + data[1]*data[1]
443 + data[2]*data[2] + data[3]*data[3];
448 Normalize the vector to have the Magnitude() == 1.0. If the vector
449 is equal to zero it is left untouched.
451 void Normalize(void);
453 /** Zero quaternion vector. Does not represent any orientation.
454 Useful for initialization of increments */
455 static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
458 /** Copying by assigning the vector valued components. */
459 FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
460 { data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }
462 /** Computation of derived values.
463 This function recomputes the derived values like euler angles and
464 transformation matrices. It does this unconditionally. */
465 void ComputeDerivedUnconditional(void) const;
467 /** Computation of derived values.
468 This function checks if the derived values like euler angles and
469 transformation matrices are already computed. If so, it
470 returns. If they need to be computed the real worker routine
471 FGQuaternion::ComputeDerivedUnconditional(void) const
473 void ComputeDerived(void) const {
475 ComputeDerivedUnconditional();
478 /** The quaternion values itself. This is the master copy. */
481 /** A data validity flag.
482 This class implements caching of the derived values like the
483 orthogonal rotation matrices or the Euler angles. For caching we
484 carry a flag which signals if the values are valid or not.
485 The C++ keyword "mutable" tells the compiler that the data member is
486 allowed to change during a const member function. */
487 mutable bool mCacheValid;
489 /** This stores the transformation matrices. */
490 mutable FGMatrix33 mT;
491 mutable FGMatrix33 mTInv;
493 /** The cached euler angles. */
494 mutable FGColumnVector3 mEulerAngles;
496 /** The cached sines and cosines of the euler angles. */
497 mutable FGColumnVector3 mEulerSines;
498 mutable FGColumnVector3 mEulerCosines;
500 void Debug(int from) const;
502 void InitializeFromEulerAngles(double phi, double tht, double psi);
505 /** Scalar multiplication.
507 @param scalar scalar value to multiply with.
508 @param q Vector to multiply.
510 Multiply the Vector with a scalar value.
512 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
513 return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
516 } // namespace JSBSim
518 #include "FGMatrix33.h"