src/FDM/JSBSim/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 "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   /// Destructor.
 114   ~FGQuaternion() {}
 115
 116   /** Quaternion 'velocity' for given angular rates.
 117       Computes the quaternion derivative which results from the given
 118       angular velocities
 119       @param PQR a constant reference to the body rate vector
 120       @return the quaternion derivative */
 121   FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
 122
 123   /** Transformation matrix.
 124       @return a reference to the transformation/rotation matrix
 125       corresponding to this quaternion rotation.  */
 126   const FGMatrix33& GetT() const { ComputeDerived(); return mT; }
 127
 128   /** Backward transformation matrix.
 129       @return a reference to the inverse transformation/rotation matrix
 130       corresponding to this quaternion rotation.  */
 131   const FGMatrix33& GetTInv() const { ComputeDerived(); return mTInv; }
 132
 133   /** Retrieves the Euler angles.
 134       @return a reference to the triad of euler angles corresponding
 135       to this quaternion rotation.
 136       @units radians  */
 137   const FGColumnVector3& GetEuler() const {
 138     ComputeDerived();
 139     return mEulerAngles;
 140   }
 141
 142   /** Euler angle theta.
 143       @return the euler angle theta (pitch attitude) corresponding to this
 144       quaternion rotation.
 145       @units radians  */
 146   double GetEulerTheta() const {
 147     ComputeDerived();
 148     return mEulerAngles(eTht);
 149   }
 150
 151   /** Euler angle theta.
 152       @return the euler angle theta (pitch attitude) corresponding to
 153       this quaternion rotation.
 154       @units degrees  */
 155   double GetEulerThetaDeg() const {
 156     ComputeDerived();
 157     return radtodeg*mEulerAngles(eTht);
 158   }
 159
 160   /** Euler angle psi.
 161       @return the heading euler angle (psi) corresponding to this quaternion
 162       rotation.
 163       @units radians  */
 164   double GetEulerPsi() const {
 165     ComputeDerived();
 166     return mEulerAngles(ePsi);
 167   }
 168
 169   /** Retrieves the heading angle.
 170       @return the Euler angle psi (heading) corresponding to this quaternion
 171       rotation.
 172       @units degrees  */
 173   double GetEulerPsiDeg() const {
 174     ComputeDerived();
 175     return radtodeg*mEulerAngles(ePsi);
 176   }
 177
 178   /** Retrieves the roll angle.
 179       @return the euler angle phi (roll) corresponding to this quaternion
 180       rotation.
 181       @units radians  */
 182   double GetEulerPhi() const {
 183     ComputeDerived();
 184     return mEulerAngles(ePhi);
 185   }
 186
 187   /** Retrieves the roll angle.
 188       Returns the Euler angle phi (roll) corresponding to this quaternion rotation.
 189       @units degrees  */
 190   double GetEulerPhiDeg() const {
 191     ComputeDerived();
 192     return radtodeg*mEulerAngles(ePhi);
 193   }
 194
 195   /** Retrieves sine theta.
 196       @return the sine of the Euler angle theta (pitch attitude) corresponding
 197       to this quaternion rotation.  */
 198   double GetSinEulerTheta() const {
 199     ComputeDerived();
 200     return mEulerSines(eTht);
 201   }
 202
 203   /** Retrieves sine psi.
 204       @return the sine of the Euler angle psi (heading) corresponding to this
 205       quaternion rotation.  */
 206   double GetSinEulerPsi() const {
 207     ComputeDerived();
 208     return mEulerSines(ePsi);
 209   }
 210
 211   /** Sine of euler angle phi.
 212       @return the sine of the Euler angle phi (roll) corresponding to this
 213       quaternion rotation.  */
 214   double GetSinEulerPhi() const {
 215     ComputeDerived();
 216     return mEulerSines(ePhi);
 217   }
 218
 219   /** Cosine of euler angle theta.
 220       @return the cosine of the Euler angle theta (pitch) corresponding to this
 221       quaternion rotation.  */
 222   double GetCosEulerTheta() const {
 223     ComputeDerived();
 224     return mEulerCosines(eTht);
 225   }
 226
 227   /** Cosine of euler angle psi.
 228       @return the cosine of the Euler angle psi (heading) corresponding to this
 229       quaternion rotation.  */
 230   double GetCosEulerPsi() const {
 231     ComputeDerived();
 232     return mEulerCosines(ePsi);
 233   }
 234
 235   /** Cosine of euler angle phi.
 236       @return the cosine of the Euler angle phi (roll) corresponding to this
 237       quaternion rotation.  */
 238   double GetCosEulerPhi() const {
 239     ComputeDerived();
 240     return mEulerCosines(ePhi);
 241   }
 242
 243   /** Read access the entries of the vector.
 244
 245       @param idx the component index.
 246
 247       Return the value of the matrix entry at the given index.
 248       Indices are counted starting with 1.
 249
 250       Note that the index given in the argument is unchecked.
 251    */
 252   double operator()(unsigned int idx) const { return Entry(idx); }
 253
 254   /** Write access the entries of the vector.
 255
 256       @param idx the component index.
 257
 258       Return a reference to the vector entry at the given index.
 259       Indices are counted starting with 1.
 260
 261       Note that the index given in the argument is unchecked.
 262    */
 263   double& operator()(unsigned int idx) { return Entry(idx); }
 264
 265   /** Read access the entries of the vector.
 266
 267       @param idx the component index.
 268
 269       Return the value of the matrix entry at the given index.
 270       Indices are counted starting with 1.
 271
 272       This function is just a shortcut for the @ref double
 273       operator()(unsigned int idx) const function. It is
 274       used internally to access the elements in a more convenient way.
 275
 276       Note that the index given in the argument is unchecked.
 277   */
 278   double Entry(unsigned int idx) const { return mData[idx-1]; }
 279
 280   /** Write access the entries of the vector.
 281
 282       @param idx the component index.
 283
 284       Return a reference to the vector entry at the given index.
 285       Indices are counted starting with 1.
 286
 287       This function is just a shortcut for the @ref double&
 288       operator()(unsigned int idx) function. It is
 289       used internally to access the elements in a more convenient way.
 290
 291       Note that the index given in the argument is unchecked.
 292   */
 293   double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
 294
 295   /** Assignment operator "=".
 296       Assign the value of q to the current object. Cached values are
 297       conserved.
 298       @param q reference to an FGQuaternion instance
 299       @return reference to a quaternion object  */
 300   const FGQuaternion& operator=(const FGQuaternion& q) {
 301     // Copy the master values ...
 302     Entry(1) = q(1);
 303     Entry(2) = q(2);
 304     Entry(3) = q(3);
 305     Entry(4) = q(4);
 306     // .. and copy the derived values if they are valid
 307     mCacheValid = q.mCacheValid;
 308     if (mCacheValid) {
 309         mT = q.mT;
 310         mTInv = q.mTInv;
 311         mEulerAngles = q.mEulerAngles;
 312         mEulerSines = q.mEulerSines;
 313         mEulerCosines = q.mEulerCosines;
 314     }
 315     return *this;
 316   }
 317
 318   /** Comparison operator "==".
 319       @param q a quaternion reference
 320       @return true if both quaternions represent the same rotation.  */
 321   bool operator==(const FGQuaternion& q) const {
 322     return Entry(1) == q(1) && Entry(2) == q(2)
 323       && Entry(3) == q(3) && Entry(4) == q(4);
 324   }
 325
 326   /** Comparison operator "!=".
 327       @param q a quaternion reference
 328       @return true if both quaternions do not represent the same rotation.  */
 329   bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
 330   const FGQuaternion& operator+=(const FGQuaternion& q) {
 331     // Copy the master values ...
 332     Entry(1) += q(1);
 333     Entry(2) += q(2);
 334     Entry(3) += q(3);
 335     Entry(4) += q(4);
 336     mCacheValid = false;
 337     return *this;
 338   }
 339
 340   /** Arithmetic operator "-=".
 341       @param q a quaternion reference.
 342       @return a quaternion reference representing Q, where Q = Q - q. */
 343   const FGQuaternion& operator-=(const FGQuaternion& q) {
 344     // Copy the master values ...
 345     Entry(1) -= q(1);
 346     Entry(2) -= q(2);
 347     Entry(3) -= q(3);
 348     Entry(4) -= q(4);
 349     mCacheValid = false;
 350     return *this;
 351   }
 352
 353   /** Arithmetic operator "*=".
 354       @param scalar a multiplicative value.
 355       @return a quaternion reference representing Q, where Q = Q * scalar. */
 356   const FGQuaternion& operator*=(double scalar) {
 357     Entry(1) *= scalar;
 358     Entry(2) *= scalar;
 359     Entry(3) *= scalar;
 360     Entry(4) *= scalar;
 361     mCacheValid = false;
 362     return *this;
 363   }
 364
 365   /** Arithmetic operator "/=".
 366       @param scalar a divisor value.
 367       @return a quaternion reference representing Q, where Q = Q / scalar. */
 368   const FGQuaternion& operator/=(double scalar) {
 369     return operator*=(1.0/scalar);
 370   }
 371
 372   /** Arithmetic operator "+".
 373       @param q a quaternion to be summed.
 374       @return a quaternion representing Q, where Q = Q + q. */
 375   FGQuaternion operator+(const FGQuaternion& q) const {
 376     return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
 377                         Entry(3)+q(3), Entry(4)+q(4));
 378   }
 379
 380   /** Arithmetic operator "-".
 381       @param q a quaternion to be subtracted.
 382       @return a quaternion representing Q, where Q = Q - q. */
 383   FGQuaternion operator-(const FGQuaternion& q) const {
 384     return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
 385                         Entry(3)-q(3), Entry(4)-q(4));
 386   }
 387
 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 representing Q, where Q = Q * q. */
 392   FGQuaternion operator*(const FGQuaternion& q) const {
 393     return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
 394                         Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
 395                         Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
 396                         Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
 397   }
 398
 399   /** Arithmetic operator "*=".
 400       Multiplication of two quaternions is like performing successive rotations.
 401       @param q a quaternion to be multiplied.
 402       @return a quaternion reference representing Q, where Q = Q * q. */
 403   const FGQuaternion& operator*=(const FGQuaternion& q) {
 404     double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
 405     double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
 406     double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
 407     double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
 408     Entry(1) = q0;
 409     Entry(2) = q1;
 410     Entry(3) = q2;
 411     Entry(4) = q3;
 412     mCacheValid = false;
 413     return *this;
 414   }
 415
 416   friend FGQuaternion operator*(double, const FGQuaternion&);
 417
 418   /** Length of the vector.
 419
 420       Compute and return the euclidean norm of this vector.
 421   */
 422   double Magnitude() const { return sqrt(SqrMagnitude()); }
 423
 424   /** Square of the length of the vector.
 425
 426       Compute and return the square of the euclidean norm of this vector.
 427   */
 428   double SqrMagnitude() const {
 429     return Entry(1)*Entry(1)+Entry(2)*Entry(2)
 430       +Entry(3)*Entry(3)+Entry(4)*Entry(4);
 431   }
 432
 433   /** Normialze.
 434
 435       Normalize the vector to have the Magnitude() == 1.0. If the vector
 436       is equal to zero it is left untouched.
 437    */
 438   void Normalize();
 439
 440   /** Zero quaternion vector. Does not represent any orientation.
 441       Useful for initialization of increments */
 442   static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
 443
 444 private:
 445   /** Copying by assigning the vector valued components.  */
 446   FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
 447     { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
 448
 449   /** Computation of derived values.
 450       This function recomputes the derived values like euler angles and
 451       transformation matrices. It does this unconditionally.  */
 452   void ComputeDerivedUnconditional(void) const;
 453
 454   /** Computation of derived values.
 455       This function checks if the derived values like euler angles and
 456       transformation matrices are already computed. If so, it
 457       returns. If they need to be computed this is done here.  */
 458   void ComputeDerived(void) const {
 459     if (!mCacheValid)
 460       ComputeDerivedUnconditional();
 461   }
 462
 463   /** The quaternion values itself. This is the master copy. */
 464   double mData[4];
 465
 466   /** A data validity flag.
 467       This class implements caching of the derived values like the
 468       orthogonal rotation matrices or the Euler angles. For caching we
 469       carry a flag which signals if the values are valid or not.
 470       The C++ keyword "mutable" tells the compiler that the data member is
 471       allowed to change during a const member function.  */
 472   mutable bool mCacheValid;
 473
 474   /** This stores the transformation matrices.  */
 475   mutable FGMatrix33 mT;
 476   mutable FGMatrix33 mTInv;
 477
 478   /** The cached euler angles.  */
 479   mutable FGColumnVector3 mEulerAngles;
 480
 481   /** The cached sines and cosines of the euler angles.  */
 482   mutable FGColumnVector3 mEulerSines;
 483   mutable FGColumnVector3 mEulerCosines;
 484 };
 485
 486 /** Scalar multiplication.
 487
 488     @param scalar scalar value to multiply with.
 489     @param p Vector to multiply.
 490
 491     Multiply the Vector with a scalar value.
 492 */
 493 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
 494   return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
 495 }
 496
 497 } // namespace JSBSim
 498
 499 #endif