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.22 2010/12/07 12:57:14 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 : public 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 the Euler angle vector.
210 @return an Euler angle column vector corresponding
211 to this quaternion rotation.
213 FGColumnVector3 const GetEulerDeg(void) const {
215 return radtodeg*mEulerAngles;
218 /** Retrieves sine of the given euler angle.
219 @return the sine of the Euler angle theta (pitch attitude) corresponding
220 to this quaternion rotation. */
221 double GetSinEuler(int i) const {
223 return mEulerSines(i);
226 /** Retrieves cosine of the given euler angle.
227 @return the sine of the Euler angle theta (pitch attitude) corresponding
228 to this quaternion rotation. */
229 double GetCosEuler(int i) const {
231 return mEulerCosines(i);
234 /** Read access the entries of the vector.
236 @param idx the component index.
238 Return the value of the matrix entry at the given index.
239 Indices are counted starting with 1.
241 Note that the index given in the argument is unchecked.
243 double operator()(unsigned int idx) const { return data[idx-1]; }
245 /** Write access the entries of the vector.
247 @param idx the component index.
249 Return a reference to the vector entry at the given index.
250 Indices are counted starting with 1.
252 Note that the index given in the argument is unchecked.
254 double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
256 /** Read access the entries of the vector.
258 @param idx the component index.
260 Return the value of the matrix entry at the given index.
261 Indices are counted starting with 1.
263 This function is just a shortcut for the <tt>double
264 operator()(unsigned int idx) const</tt> function. It is
265 used internally to access the elements in a more convenient way.
267 Note that the index given in the argument is unchecked.
269 double Entry(unsigned int idx) const { return data[idx-1]; }
271 /** Write access the entries of the vector.
273 @param idx the component index.
275 Return a reference to the vector entry at the given index.
276 Indices are counted starting with 1.
278 This function is just a shortcut for the <tt>double&
279 operator()(unsigned int idx)</tt> function. It is
280 used internally to access the elements in a more convenient way.
282 Note that the index given in the argument is unchecked.
284 double& Entry(unsigned int idx) {
289 /** Assignment operator "=".
290 Assign the value of q to the current object. Cached values are
292 @param q reference to an FGQuaternion instance
293 @return reference to a quaternion object */
294 const FGQuaternion& operator=(const FGQuaternion& q) {
295 // Copy the master values ...
301 // .. and copy the derived values if they are valid
302 mCacheValid = q.mCacheValid;
306 mEulerAngles = q.mEulerAngles;
307 mEulerSines = q.mEulerSines;
308 mEulerCosines = q.mEulerCosines;
313 /// Conversion from Quat to Matrix
314 operator FGMatrix33() const { return GetT(); }
316 /** Comparison operator "==".
317 @param q a quaternion reference
318 @return true if both quaternions represent the same rotation. */
319 bool operator==(const FGQuaternion& q) const {
320 return data[0] == q.data[0] && data[1] == q.data[1]
321 && data[2] == q.data[2] && data[3] == q.data[3];
324 /** Comparison operator "!=".
325 @param q a quaternion reference
326 @return true if both quaternions do not represent the same rotation. */
327 bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
328 const FGQuaternion& operator+=(const FGQuaternion& q) {
329 // Copy the master values ...
330 data[0] += q.data[0];
331 data[1] += q.data[1];
332 data[2] += q.data[2];
333 data[3] += q.data[3];
338 /** Arithmetic operator "-=".
339 @param q a quaternion reference.
340 @return a quaternion reference representing Q, where Q = Q - q. */
341 const FGQuaternion& operator-=(const FGQuaternion& q) {
342 // Copy the master values ...
343 data[0] -= q.data[0];
344 data[1] -= q.data[1];
345 data[2] -= q.data[2];
346 data[3] -= q.data[3];
351 /** Arithmetic operator "*=".
352 @param scalar a multiplicative value.
353 @return a quaternion reference representing Q, where Q = Q * scalar. */
354 const FGQuaternion& operator*=(double scalar) {
363 /** Arithmetic operator "/=".
364 @param scalar a divisor value.
365 @return a quaternion reference representing Q, where Q = Q / scalar. */
366 const FGQuaternion& operator/=(double scalar) {
367 return operator*=(1.0/scalar);
370 /** Arithmetic operator "+".
371 @param q a quaternion to be summed.
372 @return a quaternion representing Q, where Q = Q + q. */
373 FGQuaternion operator+(const FGQuaternion& q) const {
374 return FGQuaternion(data[0]+q.data[0], data[1]+q.data[1],
375 data[2]+q.data[2], data[3]+q.data[3]);
378 /** Arithmetic operator "-".
379 @param q a quaternion to be subtracted.
380 @return a quaternion representing Q, where Q = Q - q. */
381 FGQuaternion operator-(const FGQuaternion& q) const {
382 return FGQuaternion(data[0]-q.data[0], data[1]-q.data[1],
383 data[2]-q.data[2], data[3]-q.data[3]);
386 /** Arithmetic operator "*".
387 Multiplication of two quaternions is like performing successive rotations.
388 @param q a quaternion to be multiplied.
389 @return a quaternion representing Q, where Q = Q * q. */
390 FGQuaternion operator*(const FGQuaternion& q) const {
391 return FGQuaternion(data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3],
392 data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2],
393 data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1],
394 data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0]);
397 /** Arithmetic operator "*=".
398 Multiplication of two quaternions is like performing successive rotations.
399 @param q a quaternion to be multiplied.
400 @return a quaternion reference representing Q, where Q = Q * q. */
401 const FGQuaternion& operator*=(const FGQuaternion& q) {
402 double q0 = data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3];
403 double q1 = data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2];
404 double q2 = data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1];
405 double q3 = data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0];
414 /** Inverse of the quaternion.
416 Compute and return the inverse of the quaternion so that the orientation
417 represented with *this multiplied with the returned value is equal to
418 the identity orientation.
420 FGQuaternion Inverse(void) const {
421 double norm = SqrMagnitude();
424 double rNorm = 1.0/norm;
425 return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
426 -data[2]*rNorm, -data[3]*rNorm );
429 /** Conjugate of the quaternion.
431 Compute and return the conjugate of the quaternion. This one is equal
432 to the inverse iff the quaternion is normalized.
434 FGQuaternion Conjugate(void) const {
435 return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
438 friend FGQuaternion operator*(double, const FGQuaternion&);
440 /** Length of the vector.
442 Compute and return the euclidean norm of this vector.
444 double Magnitude(void) const { return sqrt(SqrMagnitude()); }
446 /** Square of the length of the vector.
448 Compute and return the square of the euclidean norm of this vector.
450 double SqrMagnitude(void) const {
451 return data[0]*data[0] + data[1]*data[1]
452 + data[2]*data[2] + data[3]*data[3];
457 Normalize the vector to have the Magnitude() == 1.0. If the vector
458 is equal to zero it is left untouched.
460 void Normalize(void);
462 /** Zero quaternion vector. Does not represent any orientation.
463 Useful for initialization of increments */
464 static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
467 /** Copying by assigning the vector valued components. */
468 FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
469 { data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }
471 /** Computation of derived values.
472 This function recomputes the derived values like euler angles and
473 transformation matrices. It does this unconditionally. */
474 void ComputeDerivedUnconditional(void) const;
476 /** Computation of derived values.
477 This function checks if the derived values like euler angles and
478 transformation matrices are already computed. If so, it
479 returns. If they need to be computed the real worker routine
480 FGQuaternion::ComputeDerivedUnconditional(void) const
482 void ComputeDerived(void) const {
484 ComputeDerivedUnconditional();
487 /** The quaternion values itself. This is the master copy. */
490 /** A data validity flag.
491 This class implements caching of the derived values like the
492 orthogonal rotation matrices or the Euler angles. For caching we
493 carry a flag which signals if the values are valid or not.
494 The C++ keyword "mutable" tells the compiler that the data member is
495 allowed to change during a const member function. */
496 mutable bool mCacheValid;
498 /** This stores the transformation matrices. */
499 mutable FGMatrix33 mT;
500 mutable FGMatrix33 mTInv;
502 /** The cached euler angles. */
503 mutable FGColumnVector3 mEulerAngles;
505 /** The cached sines and cosines of the euler angles. */
506 mutable FGColumnVector3 mEulerSines;
507 mutable FGColumnVector3 mEulerCosines;
509 void InitializeFromEulerAngles(double phi, double tht, double psi);
512 /** Scalar multiplication.
514 @param scalar scalar value to multiply with.
515 @param q Vector to multiply.
517 Multiply the Vector with a scalar value.
519 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
520 return FGQuaternion(scalar*q.data[0], scalar*q.data[1], scalar*q.data[2], scalar*q.data[3]);
523 /** Write quaternion to a stream.
524 @param os Stream to write to.
525 @param q Quaternion to write.
526 Write the quaternion to a stream.*/
527 std::ostream& operator<<(std::ostream& os, const FGQuaternion& q);
529 } // namespace JSBSim
531 #include "FGMatrix33.h"