src/FDM/JSBSim/math/FGQuaternion.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGQuaternion.h
   4  Author:       Jon Berndt, Mathis Froehlich
   5  Date started: 12/02/98
   6
   7  ------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) ------------------
   8  -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
   9
  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
  13  version.
  14
  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
  18  details.
  19
  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.
  23
  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.
  26
  27 HISTORY
  28 -------------------------------------------------------------------------------
  29 12/02/98   JSB   Created
  30 15/01/04   MF    Quaternion class from old FGColumnVector4
  31
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33 SENTRY
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  35
  36 #ifndef FGQUATERNION_H
  37 #define FGQUATERNION_H
  38
  39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40   INCLUDES
  41   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  42
  43 #include "FGJSBBase.h"
  44 #include "FGMatrix33.h"
  45 #include "FGColumnVector3.h"
  46 #include "input_output/FGPropertyManager.h"
  47
  48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49   DEFINITIONS
  50   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  51
  52 #define ID_QUATERNION "$Id$"
  53
  54 namespace JSBSim {
  55
  56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  57   CLASS DOCUMENTATION
  58   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  59
  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.
  68
  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.
  71
  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
  74     School, January 1994
  75     @see D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
  76     JSC 12960, July 1977
  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
  84             and Jon Berndt
  85 */
  86
  87 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  88   CLASS DECLARATION
  89   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  90
  91 class FGQuaternion
  92   : virtual FGJSBBase {
  93 public:
  94   /** Default initializer.
  95       Default initializer, initializes the class with the identity rotation.  */
  96   FGQuaternion() : mCacheValid(false) {
  97     Entry(1) = 1.0;
  98     Entry(2) = Entry(3) = Entry(4) = 0.0;
  99   }
 100
 101   /** Copy constructor.
 102       Copy constructor, initializes the quaternion.
 103       @param q  a constant reference to another FGQuaternion instance  */
 104   FGQuaternion(const FGQuaternion& q);
 105
 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);
 112
 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;
 121
 122     double Sangle2 = sin(angle2);
 123     double Cangle2 = cos(angle2);
 124
 125     if (idx == ePhi) {
 126       Entry(1) = Cangle2;
 127       Entry(2) = Sangle2;
 128       Entry(3) = 0.0;
 129       Entry(4) = 0.0;
 130
 131     } else if (idx == eTht) {
 132       Entry(1) = Cangle2;
 133       Entry(2) = 0.0;
 134       Entry(3) = Sangle2;
 135       Entry(4) = 0.0;
 136
 137     } else {
 138       Entry(1) = Cangle2;
 139       Entry(2) = 0.0;
 140       Entry(3) = 0.0;
 141       Entry(4) = Sangle2;
 142
 143     }
 144   }
 145
 146   /// Destructor.
 147   ~FGQuaternion() {}
 148
 149   /** Quaternion derivative for given angular rates.
 150       Computes the quaternion derivative which results from the given
 151       angular velocities
 152       @param PQR a constant reference to the body rate vector
 153       @return the quaternion derivative
 154       @see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 155            Equation 1.3-36. */
 156   FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
 157
 158   /** Transformation matrix.
 159       @return a reference to the transformation/rotation matrix
 160       corresponding to this quaternion rotation.  */
 161   const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
 162
 163   /** Backward transformation matrix.
 164       @return a reference to the inverse transformation/rotation matrix
 165       corresponding to this quaternion rotation.  */
 166   const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
 167
 168   /** Retrieves the Euler angles.
 169       @return a reference to the triad of euler angles corresponding
 170       to this quaternion rotation.
 171       units radians  */
 172   const FGColumnVector3& GetEuler(void) const {
 173     ComputeDerived();
 174     return mEulerAngles;
 175   }
 176
 177   /** Retrieves the Euler angles.
 178       @param i the euler angle index.
 179       units radians.
 180       @return a reference to the i-th euler angles corresponding
 181       to this quaternion rotation.
 182    */
 183   double GetEuler(int i) const {
 184     ComputeDerived();
 185     return mEulerAngles(i);
 186   }
 187
 188   /** Retrieves the Euler angles.
 189       @param i the euler angle index.
 190       @return a reference to the i-th euler angles corresponding
 191       to this quaternion rotation.
 192       units degrees */
 193   double GetEulerDeg(int i) const {
 194     ComputeDerived();
 195     return radtodeg*mEulerAngles(i);
 196   }
 197
 198   /** Retrieves sine of the given euler angle.
 199       @return the sine of the Euler angle theta (pitch attitude) corresponding
 200       to this quaternion rotation.  */
 201   double GetSinEuler(int i) const {
 202     ComputeDerived();
 203     return mEulerSines(i);
 204   }
 205
 206   /** Retrieves cosine of the given euler angle.
 207       @return the sine of the Euler angle theta (pitch attitude) corresponding
 208       to this quaternion rotation.  */
 209   double GetCosEuler(int i) const {
 210     ComputeDerived();
 211     return mEulerCosines(i);
 212   }
 213
 214   /** Read access the entries of the vector.
 215
 216       @param idx the component index.
 217
 218       Return the value of the matrix entry at the given index.
 219       Indices are counted starting with 1.
 220
 221       Note that the index given in the argument is unchecked.
 222    */
 223   double operator()(unsigned int idx) const { return Entry(idx); }
 224
 225   /** Write access the entries of the vector.
 226
 227       @param idx the component index.
 228
 229       Return a reference to the vector entry at the given index.
 230       Indices are counted starting with 1.
 231
 232       Note that the index given in the argument is unchecked.
 233    */
 234   double& operator()(unsigned int idx) { return Entry(idx); }
 235
 236   /** Read access the entries of the vector.
 237
 238       @param idx the component index.
 239
 240       Return the value of the matrix entry at the given index.
 241       Indices are counted starting with 1.
 242
 243       This function is just a shortcut for the <tt>double
 244       operator()(unsigned int idx) const</tt> function. It is
 245       used internally to access the elements in a more convenient way.
 246
 247       Note that the index given in the argument is unchecked.
 248   */
 249   double Entry(unsigned int idx) const { return mData[idx-1]; }
 250
 251   /** Write access the entries of the vector.
 252
 253       @param idx the component index.
 254
 255       Return a reference to the vector entry at the given index.
 256       Indices are counted starting with 1.
 257
 258       This function is just a shortcut for the <tt>double&
 259       operator()(unsigned int idx)</tt> function. It is
 260       used internally to access the elements in a more convenient way.
 261
 262       Note that the index given in the argument is unchecked.
 263   */
 264   double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
 265
 266   /** Assignment operator "=".
 267       Assign the value of q to the current object. Cached values are
 268       conserved.
 269       @param q reference to an FGQuaternion instance
 270       @return reference to a quaternion object  */
 271   const FGQuaternion& operator=(const FGQuaternion& q) {
 272     // Copy the master values ...
 273     Entry(1) = q(1);
 274     Entry(2) = q(2);
 275     Entry(3) = q(3);
 276     Entry(4) = q(4);
 277     // .. and copy the derived values if they are valid
 278     mCacheValid = q.mCacheValid;
 279     if (mCacheValid) {
 280         mT = q.mT;
 281         mTInv = q.mTInv;
 282         mEulerAngles = q.mEulerAngles;
 283         mEulerSines = q.mEulerSines;
 284         mEulerCosines = q.mEulerCosines;
 285     }
 286     return *this;
 287   }
 288
 289   /** Comparison operator "==".
 290       @param q a quaternion reference
 291       @return true if both quaternions represent the same rotation.  */
 292   bool operator==(const FGQuaternion& q) const {
 293     return Entry(1) == q(1) && Entry(2) == q(2)
 294       && Entry(3) == q(3) && Entry(4) == q(4);
 295   }
 296
 297   /** Comparison operator "!=".
 298       @param q a quaternion reference
 299       @return true if both quaternions do not represent the same rotation.  */
 300   bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
 301   const FGQuaternion& operator+=(const FGQuaternion& q) {
 302     // Copy the master values ...
 303     Entry(1) += q(1);
 304     Entry(2) += q(2);
 305     Entry(3) += q(3);
 306     Entry(4) += q(4);
 307     mCacheValid = false;
 308     return *this;
 309   }
 310
 311   /** Arithmetic operator "-=".
 312       @param q a quaternion reference.
 313       @return a quaternion reference representing Q, where Q = Q - q. */
 314   const FGQuaternion& operator-=(const FGQuaternion& q) {
 315     // Copy the master values ...
 316     Entry(1) -= q(1);
 317     Entry(2) -= q(2);
 318     Entry(3) -= q(3);
 319     Entry(4) -= q(4);
 320     mCacheValid = false;
 321     return *this;
 322   }
 323
 324   /** Arithmetic operator "*=".
 325       @param scalar a multiplicative value.
 326       @return a quaternion reference representing Q, where Q = Q * scalar. */
 327   const FGQuaternion& operator*=(double scalar) {
 328     Entry(1) *= scalar;
 329     Entry(2) *= scalar;
 330     Entry(3) *= scalar;
 331     Entry(4) *= scalar;
 332     mCacheValid = false;
 333     return *this;
 334   }
 335
 336   /** Arithmetic operator "/=".
 337       @param scalar a divisor value.
 338       @return a quaternion reference representing Q, where Q = Q / scalar. */
 339   const FGQuaternion& operator/=(double scalar) {
 340     return operator*=(1.0/scalar);
 341   }
 342
 343   /** Arithmetic operator "+".
 344       @param q a quaternion to be summed.
 345       @return a quaternion representing Q, where Q = Q + q. */
 346   FGQuaternion operator+(const FGQuaternion& q) const {
 347     return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
 348                         Entry(3)+q(3), Entry(4)+q(4));
 349   }
 350
 351   /** Arithmetic operator "-".
 352       @param q a quaternion to be subtracted.
 353       @return a quaternion representing Q, where Q = Q - q. */
 354   FGQuaternion operator-(const FGQuaternion& q) const {
 355     return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
 356                         Entry(3)-q(3), Entry(4)-q(4));
 357   }
 358
 359   /** Arithmetic operator "*".
 360       Multiplication of two quaternions is like performing successive rotations.
 361       @param q a quaternion to be multiplied.
 362       @return a quaternion representing Q, where Q = Q * q. */
 363   FGQuaternion operator*(const FGQuaternion& q) const {
 364     return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
 365                         Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
 366                         Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
 367                         Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
 368   }
 369
 370   /** Arithmetic operator "*=".
 371       Multiplication of two quaternions is like performing successive rotations.
 372       @param q a quaternion to be multiplied.
 373       @return a quaternion reference representing Q, where Q = Q * q. */
 374   const FGQuaternion& operator*=(const FGQuaternion& q) {
 375     double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
 376     double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
 377     double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
 378     double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
 379     Entry(1) = q0;
 380     Entry(2) = q1;
 381     Entry(3) = q2;
 382     Entry(4) = q3;
 383     mCacheValid = false;
 384     return *this;
 385   }
 386
 387   /** Inverse of the quaternion.
 388
 389       Compute and return the inverse of the quaternion so that the orientation
 390       represented with *this multiplied with the returned value is equal to
 391       the identity orientation.
 392   */
 393   FGQuaternion Inverse(void) const {
 394     double norm = Magnitude();
 395     if (norm == 0.0)
 396       return *this;
 397     double rNorm = 1.0/norm;
 398     return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
 399                          -Entry(3)*rNorm, -Entry(4)*rNorm );
 400   }
 401
 402   /** Conjugate of the quaternion.
 403
 404       Compute and return the conjugate of the quaternion. This one is equal
 405       to the inverse iff the quaternion is normalized.
 406   */
 407   FGQuaternion Conjugate(void) const {
 408     return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
 409   }
 410
 411   friend FGQuaternion operator*(double, const FGQuaternion&);
 412
 413   /** Length of the vector.
 414
 415       Compute and return the euclidean norm of this vector.
 416   */
 417   double Magnitude(void) const { return sqrt(SqrMagnitude()); }
 418
 419   /** Square of the length of the vector.
 420
 421       Compute and return the square of the euclidean norm of this vector.
 422   */
 423   double SqrMagnitude(void) const {
 424     return Entry(1)*Entry(1)+Entry(2)*Entry(2)
 425       +Entry(3)*Entry(3)+Entry(4)*Entry(4);
 426   }
 427
 428   /** Normialze.
 429
 430       Normalize the vector to have the Magnitude() == 1.0. If the vector
 431       is equal to zero it is left untouched.
 432    */
 433   void Normalize(void);
 434
 435   /** Zero quaternion vector. Does not represent any orientation.
 436       Useful for initialization of increments */
 437   static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
 438
 439 private:
 440   /** Copying by assigning the vector valued components.  */
 441   FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
 442     { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
 443
 444   /** Computation of derived values.
 445       This function recomputes the derived values like euler angles and
 446       transformation matrices. It does this unconditionally.  */
 447   void ComputeDerivedUnconditional(void) const;
 448
 449   /** Computation of derived values.
 450       This function checks if the derived values like euler angles and
 451       transformation matrices are already computed. If so, it
 452       returns. If they need to be computed the real worker routine
 453       \ref FGQuaternion::ComputeDerivedUnconditional(void) const
 454       is called.
 455       This function is inlined to avoid function calls in the fast path. */
 456   void ComputeDerived(void) const {
 457     if (!mCacheValid)
 458       ComputeDerivedUnconditional();
 459   }
 460
 461   /** The quaternion values itself. This is the master copy. */
 462   double mData[4];
 463
 464   /** A data validity flag.
 465       This class implements caching of the derived values like the
 466       orthogonal rotation matrices or the Euler angles. For caching we
 467       carry a flag which signals if the values are valid or not.
 468       The C++ keyword "mutable" tells the compiler that the data member is
 469       allowed to change during a const member function.  */
 470   mutable bool mCacheValid;
 471
 472   /** This stores the transformation matrices.  */
 473   mutable FGMatrix33 mT;
 474   mutable FGMatrix33 mTInv;
 475
 476   /** The cached euler angles.  */
 477   mutable FGColumnVector3 mEulerAngles;
 478
 479   /** The cached sines and cosines of the euler angles.  */
 480   mutable FGColumnVector3 mEulerSines;
 481   mutable FGColumnVector3 mEulerCosines;
 482 };
 483
 484 /** Scalar multiplication.
 485
 486     @param scalar scalar value to multiply with.
 487     @param q Vector to multiply.
 488
 489     Multiply the Vector with a scalar value.
 490 */
 491 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
 492   return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
 493 }
 494
 495 } // namespace JSBSim
 496
 497 #endif