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 (jsb@hal-pc.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 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 General Public License for more
  18  details.
  19
  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.
  23
  24  Further information about the GNU 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 'velocity' 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   FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
 155
 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; }
 160
 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; }
 165
 166   /** Retrieves the Euler angles.
 167       @return a reference to the triad of euler angles corresponding
 168       to this quaternion rotation.
 169       @units radians  */
 170   const FGColumnVector3& GetEuler(void) const {
 171     ComputeDerived();
 172     return mEulerAngles;
 173   }
 174
 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.
 179       @units radians */
 180   double GetEuler(int i) const {
 181     ComputeDerived();
 182     return mEulerAngles(i);
 183   }
 184
 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.
 189       @units degrees */
 190   double GetEulerDeg(int i) const {
 191     ComputeDerived();
 192     return radtodeg*mEulerAngles(i);
 193   }
 194
 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 {
 199     ComputeDerived();
 200     return mEulerSines(i);
 201   }
 202
 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 {
 207     ComputeDerived();
 208     return mEulerCosines(i);
 209   }
 210
 211   /** Read access the entries of the vector.
 212
 213       @param idx the component index.
 214
 215       Return the value of the matrix entry at the given index.
 216       Indices are counted starting with 1.
 217
 218       Note that the index given in the argument is unchecked.
 219    */
 220   double operator()(unsigned int idx) const { return Entry(idx); }
 221
 222   /** Write access the entries of the vector.
 223
 224       @param idx the component index.
 225
 226       Return a reference to the vector entry at the given index.
 227       Indices are counted starting with 1.
 228
 229       Note that the index given in the argument is unchecked.
 230    */
 231   double& operator()(unsigned int idx) { return Entry(idx); }
 232
 233   /** Read access the entries of the vector.
 234
 235       @param idx the component index.
 236
 237       Return the value of the matrix entry at the given index.
 238       Indices are counted starting with 1.
 239
 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.
 243
 244       Note that the index given in the argument is unchecked.
 245   */
 246   double Entry(unsigned int idx) const { return mData[idx-1]; }
 247
 248   /** Write access the entries of the vector.
 249
 250       @param idx the component index.
 251
 252       Return a reference to the vector entry at the given index.
 253       Indices are counted starting with 1.
 254
 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.
 258
 259       Note that the index given in the argument is unchecked.
 260   */
 261   double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
 262
 263   /** Assignment operator "=".
 264       Assign the value of q to the current object. Cached values are
 265       conserved.
 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 ...
 270     Entry(1) = q(1);
 271     Entry(2) = q(2);
 272     Entry(3) = q(3);
 273     Entry(4) = q(4);
 274     // .. and copy the derived values if they are valid
 275     mCacheValid = q.mCacheValid;
 276     if (mCacheValid) {
 277         mT = q.mT;
 278         mTInv = q.mTInv;
 279         mEulerAngles = q.mEulerAngles;
 280         mEulerSines = q.mEulerSines;
 281         mEulerCosines = q.mEulerCosines;
 282     }
 283     return *this;
 284   }
 285
 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);
 292   }
 293
 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 ...
 300     Entry(1) += q(1);
 301     Entry(2) += q(2);
 302     Entry(3) += q(3);
 303     Entry(4) += q(4);
 304     mCacheValid = false;
 305     return *this;
 306   }
 307
 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 ...
 313     Entry(1) -= q(1);
 314     Entry(2) -= q(2);
 315     Entry(3) -= q(3);
 316     Entry(4) -= q(4);
 317     mCacheValid = false;
 318     return *this;
 319   }
 320
 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) {
 325     Entry(1) *= scalar;
 326     Entry(2) *= scalar;
 327     Entry(3) *= scalar;
 328     Entry(4) *= scalar;
 329     mCacheValid = false;
 330     return *this;
 331   }
 332
 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);
 338   }
 339
 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));
 346   }
 347
 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));
 354   }
 355
 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));
 365   }
 366
 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);
 376     Entry(1) = q0;
 377     Entry(2) = q1;
 378     Entry(3) = q2;
 379     Entry(4) = q3;
 380     mCacheValid = false;
 381     return *this;
 382   }
 383
 384   /** Inverse of the quaternion.
 385
 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.
 389   */
 390   FGQuaternion Inverse(void) const {
 391     double norm = Magnitude();
 392     if (norm == 0.0)
 393       return *this;
 394     double rNorm = 1.0/norm;
 395     return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
 396                          -Entry(3)*rNorm, -Entry(4)*rNorm );
 397   }
 398
 399   /** Conjugate of the quaternion.
 400
 401       Compute and return the conjugate of the quaternion. This one is equal
 402       to the inverse iff the quaternion is normalized.
 403   */
 404   FGQuaternion Conjugate(void) const {
 405     return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
 406   }
 407
 408   friend FGQuaternion operator*(double, const FGQuaternion&);
 409
 410   /** Length of the vector.
 411
 412       Compute and return the euclidean norm of this vector.
 413   */
 414   double Magnitude(void) const { return sqrt(SqrMagnitude()); }
 415
 416   /** Square of the length of the vector.
 417
 418       Compute and return the square of the euclidean norm of this vector.
 419   */
 420   double SqrMagnitude(void) const {
 421     return Entry(1)*Entry(1)+Entry(2)*Entry(2)
 422       +Entry(3)*Entry(3)+Entry(4)*Entry(4);
 423   }
 424
 425   /** Normialze.
 426
 427       Normalize the vector to have the Magnitude() == 1.0. If the vector
 428       is equal to zero it is left untouched.
 429    */
 430   void Normalize(void);
 431
 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 ); }
 435
 436 private:
 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; }
 440
 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;
 445
 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
 451       is called.
 452       This function is inlined to avoid function calls in the fast path. */
 453   void ComputeDerived(void) const {
 454     if (!mCacheValid)
 455       ComputeDerivedUnconditional();
 456   }
 457
 458   /** The quaternion values itself. This is the master copy. */
 459   double mData[4];
 460
 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;
 468
 469   /** This stores the transformation matrices.  */
 470   mutable FGMatrix33 mT;
 471   mutable FGMatrix33 mTInv;
 472
 473   /** The cached euler angles.  */
 474   mutable FGColumnVector3 mEulerAngles;
 475
 476   /** The cached sines and cosines of the euler angles.  */
 477   mutable FGColumnVector3 mEulerSines;
 478   mutable FGColumnVector3 mEulerCosines;
 479 };
 480
 481 /** Scalar multiplication.
 482
 483     @param scalar scalar value to multiply with.
 484     @param p Vector to multiply.
 485
 486     Multiply the Vector with a scalar value.
 487 */
 488 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
 489   return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
 490 }
 491
 492 } // namespace JSBSim
 493
 494 #endif