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 "FGColumnVector3.h"
  45
  46 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  47   DEFINITIONS
  48   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  49
  50 #define ID_QUATERNION "$Id: FGQuaternion.h,v 1.17 2010/06/30 03:13:40 jberndt Exp $"
  51
  52 namespace JSBSim {
  53
  54 class FGMatrix33;
  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 : virtual FGJSBBase {
  92 public:
  93   /** Default initializer.
  94       Default initializer, initializes the class with the identity rotation.  */
  95   FGQuaternion() : mCacheValid(false) {
  96     data[0] = 1.0;
  97     data[1] = data[2] = data[3] = 0.0;
  98   }
  99
 100   /** Copy constructor.
 101       Copy constructor, initializes the quaternion.
 102       @param q  a constant reference to another FGQuaternion instance  */
 103   FGQuaternion(const FGQuaternion& q);
 104
 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);
 111
 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);
 116
 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) {
 124
 125     double angle2 = 0.5*angle;
 126
 127     double Sangle2 = sin(angle2);
 128     double Cangle2 = cos(angle2);
 129
 130     if (idx == ePhi) {
 131       data[0] = Cangle2;
 132       data[1] = Sangle2;
 133       data[2] = 0.0;
 134       data[3] = 0.0;
 135
 136     } else if (idx == eTht) {
 137       data[0] = Cangle2;
 138       data[1] = 0.0;
 139       data[2] = Sangle2;
 140       data[3] = 0.0;
 141
 142     } else {
 143       data[0] = Cangle2;
 144       data[1] = 0.0;
 145       data[2] = 0.0;
 146       data[3] = Sangle2;
 147
 148     }
 149   }
 150
 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);
 156
 157   /// Destructor.
 158   ~FGQuaternion() {}
 159
 160   /** Quaternion derivative for given angular rates.
 161       Computes the quaternion derivative which results from the given
 162       angular velocities
 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,
 166            Equation 1.3-36. */
 167   FGQuaternion GetQDot(const FGColumnVector3& PQR);
 168
 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; }
 173
 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; }
 178
 179   /** Retrieves the Euler angles.
 180       @return a reference to the triad of euler angles corresponding
 181       to this quaternion rotation.
 182       units radians  */
 183   const FGColumnVector3& GetEuler(void) const {
 184     ComputeDerived();
 185     return mEulerAngles;
 186   }
 187
 188   /** Retrieves the Euler angles.
 189       @param i the euler angle index.
 190       units radians.
 191       @return a reference to the i-th euler angles corresponding
 192       to this quaternion rotation.
 193    */
 194   double GetEuler(int i) const {
 195     ComputeDerived();
 196     return mEulerAngles(i);
 197   }
 198
 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.
 203       units degrees */
 204   double GetEulerDeg(int i) const {
 205     ComputeDerived();
 206     return radtodeg*mEulerAngles(i);
 207   }
 208
 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 {
 213     ComputeDerived();
 214     return mEulerSines(i);
 215   }
 216
 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 {
 221     ComputeDerived();
 222     return mEulerCosines(i);
 223   }
 224
 225   /** Read access the entries of the vector.
 226
 227       @param idx the component index.
 228
 229       Return the value of the matrix 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) const { return data[idx-1]; }
 235
 236   /** Write access the entries of the vector.
 237
 238       @param idx the component index.
 239
 240       Return a reference to the vector entry at the given index.
 241       Indices are counted starting with 1.
 242
 243       Note that the index given in the argument is unchecked.
 244    */
 245   double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
 246
 247   /** Read access the entries of the vector.
 248
 249       @param idx the component index.
 250
 251       Return the value of the matrix entry at the given index.
 252       Indices are counted starting with 1.
 253
 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.
 257
 258       Note that the index given in the argument is unchecked.
 259   */
 260   double Entry(unsigned int idx) const { return data[idx-1]; }
 261
 262   /** Write access the entries of the vector.
 263
 264       @param idx the component index.
 265
 266       Return a reference to the vector entry at the given index.
 267       Indices are counted starting with 1.
 268
 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.
 272
 273       Note that the index given in the argument is unchecked.
 274   */
 275   double& Entry(unsigned int idx) {
 276     mCacheValid = false;
 277    return data[idx-1];
 278   }
 279
 280   /** Assignment operator "=".
 281       Assign the value of q to the current object. Cached values are
 282       conserved.
 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 ...
 287     data[0] = q(1);
 288     data[1] = q(2);
 289     data[2] = q(3);
 290     data[3] = q(4);
 291     ComputeDerived();
 292     // .. and copy the derived values if they are valid
 293     mCacheValid = q.mCacheValid;
 294     if (mCacheValid) {
 295         mT = q.mT;
 296         mTInv = q.mTInv;
 297         mEulerAngles = q.mEulerAngles;
 298         mEulerSines = q.mEulerSines;
 299         mEulerCosines = q.mEulerCosines;
 300     }
 301     return *this;
 302   }
 303
 304   /// Conversion from Quat to Matrix
 305   operator FGMatrix33() const { return GetT(); }
 306
 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);
 313   }
 314
 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 ...
 321     data[0] += q(1);
 322     data[1] += q(2);
 323     data[2] += q(3);
 324     data[3] += q(4);
 325     mCacheValid = false;
 326     return *this;
 327   }
 328
 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 ...
 334     data[0] -= q(1);
 335     data[1] -= q(2);
 336     data[2] -= q(3);
 337     data[3] -= q(4);
 338     mCacheValid = false;
 339     return *this;
 340   }
 341
 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) {
 346     data[0] *= scalar;
 347     data[1] *= scalar;
 348     data[2] *= scalar;
 349     data[3] *= scalar;
 350     mCacheValid = false;
 351     return *this;
 352   }
 353
 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);
 359   }
 360
 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));
 367   }
 368
 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));
 375   }
 376
 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));
 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 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);
 397     data[0] = q0;
 398     data[1] = q1;
 399     data[2] = q2;
 400     data[3] = q3;
 401     mCacheValid = false;
 402     return *this;
 403   }
 404
 405   /** Inverse of the quaternion.
 406
 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.
 410   */
 411   FGQuaternion Inverse(void) const {
 412     double norm = SqrMagnitude();
 413     if (norm == 0.0)
 414       return *this;
 415     double rNorm = 1.0/norm;
 416     return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
 417                          -data[2]*rNorm, -data[3]*rNorm );
 418   }
 419
 420   /** Conjugate of the quaternion.
 421
 422       Compute and return the conjugate of the quaternion. This one is equal
 423       to the inverse iff the quaternion is normalized.
 424   */
 425   FGQuaternion Conjugate(void) const {
 426     return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
 427   }
 428
 429   friend FGQuaternion operator*(double, const FGQuaternion&);
 430
 431   /** Length of the vector.
 432
 433       Compute and return the euclidean norm of this vector.
 434   */
 435   double Magnitude(void) const { return sqrt(SqrMagnitude()); }
 436
 437   /** Square of the length of the vector.
 438
 439       Compute and return the square of the euclidean norm of this vector.
 440   */
 441   double SqrMagnitude(void) const {
 442     return  data[0]*data[0] + data[1]*data[1]
 443           + data[2]*data[2] + data[3]*data[3];
 444   }
 445
 446   /** Normalize.
 447
 448       Normalize the vector to have the Magnitude() == 1.0. If the vector
 449       is equal to zero it is left untouched.
 450    */
 451   void Normalize(void);
 452
 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 ); }
 456
 457 private:
 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; }
 461
 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;
 466
 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
 472       is called. */
 473   void ComputeDerived(void) const {
 474     if (!mCacheValid)
 475       ComputeDerivedUnconditional();
 476   }
 477
 478   /** The quaternion values itself. This is the master copy. */
 479   double data[4];
 480
 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;
 488
 489   /** This stores the transformation matrices.  */
 490   mutable FGMatrix33 mT;
 491   mutable FGMatrix33 mTInv;
 492
 493   /** The cached euler angles.  */
 494   mutable FGColumnVector3 mEulerAngles;
 495
 496   /** The cached sines and cosines of the euler angles.  */
 497   mutable FGColumnVector3 mEulerSines;
 498   mutable FGColumnVector3 mEulerCosines;
 499
 500   void Debug(int from) const;
 501
 502   void InitializeFromEulerAngles(double phi, double tht, double psi);
 503 };
 504
 505 /** Scalar multiplication.
 506
 507     @param scalar scalar value to multiply with.
 508     @param q Vector to multiply.
 509
 510     Multiply the Vector with a scalar value.
 511 */
 512 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
 513   return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
 514 }
 515
 516 } // namespace JSBSim
 517
 518 #include "FGMatrix33.h"
 519
 520 #endif