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.22 2010/12/07 12:57:14 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 : public 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 the Euler angle vector.
 210       @return an Euler angle column vector corresponding
 211       to this quaternion rotation.
 212       units degrees */
 213   FGColumnVector3 const GetEulerDeg(void) const {
 214     ComputeDerived();
 215     return radtodeg*mEulerAngles;
 216   }
 217
 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 {
 222     ComputeDerived();
 223     return mEulerSines(i);
 224   }
 225
 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 {
 230     ComputeDerived();
 231     return mEulerCosines(i);
 232   }
 233
 234   /** Read access the entries of the vector.
 235
 236       @param idx the component index.
 237
 238       Return the value of the matrix entry at the given index.
 239       Indices are counted starting with 1.
 240
 241       Note that the index given in the argument is unchecked.
 242    */
 243   double operator()(unsigned int idx) const { return data[idx-1]; }
 244
 245   /** Write access the entries of the vector.
 246
 247       @param idx the component index.
 248
 249       Return a reference to the vector entry at the given index.
 250       Indices are counted starting with 1.
 251
 252       Note that the index given in the argument is unchecked.
 253    */
 254   double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
 255
 256   /** Read access the entries of the vector.
 257
 258       @param idx the component index.
 259
 260       Return the value of the matrix entry at the given index.
 261       Indices are counted starting with 1.
 262
 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.
 266
 267       Note that the index given in the argument is unchecked.
 268   */
 269   double Entry(unsigned int idx) const { return data[idx-1]; }
 270
 271   /** Write access the entries of the vector.
 272
 273       @param idx the component index.
 274
 275       Return a reference to the vector entry at the given index.
 276       Indices are counted starting with 1.
 277
 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.
 281
 282       Note that the index given in the argument is unchecked.
 283   */
 284   double& Entry(unsigned int idx) {
 285     mCacheValid = false;
 286    return data[idx-1];
 287   }
 288
 289   /** Assignment operator "=".
 290       Assign the value of q to the current object. Cached values are
 291       conserved.
 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 ...
 296     data[0] = q.data[0];
 297     data[1] = q.data[1];
 298     data[2] = q.data[2];
 299     data[3] = q.data[3];
 300     ComputeDerived();
 301     // .. and copy the derived values if they are valid
 302     mCacheValid = q.mCacheValid;
 303     if (mCacheValid) {
 304         mT = q.mT;
 305         mTInv = q.mTInv;
 306         mEulerAngles = q.mEulerAngles;
 307         mEulerSines = q.mEulerSines;
 308         mEulerCosines = q.mEulerCosines;
 309     }
 310     return *this;
 311   }
 312
 313   /// Conversion from Quat to Matrix
 314   operator FGMatrix33() const { return GetT(); }
 315
 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];
 322   }
 323
 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];
 334     mCacheValid = false;
 335     return *this;
 336   }
 337
 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];
 347     mCacheValid = false;
 348     return *this;
 349   }
 350
 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) {
 355     data[0] *= scalar;
 356     data[1] *= scalar;
 357     data[2] *= scalar;
 358     data[3] *= scalar;
 359     mCacheValid = false;
 360     return *this;
 361   }
 362
 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);
 368   }
 369
 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]);
 376   }
 377
 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]);
 384   }
 385
 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]);
 395   }
 396
 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];
 406     data[0] = q0;
 407     data[1] = q1;
 408     data[2] = q2;
 409     data[3] = q3;
 410     mCacheValid = false;
 411     return *this;
 412   }
 413
 414   /** Inverse of the quaternion.
 415
 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.
 419   */
 420   FGQuaternion Inverse(void) const {
 421     double norm = SqrMagnitude();
 422     if (norm == 0.0)
 423       return *this;
 424     double rNorm = 1.0/norm;
 425     return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
 426                          -data[2]*rNorm, -data[3]*rNorm );
 427   }
 428
 429   /** Conjugate of the quaternion.
 430
 431       Compute and return the conjugate of the quaternion. This one is equal
 432       to the inverse iff the quaternion is normalized.
 433   */
 434   FGQuaternion Conjugate(void) const {
 435     return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
 436   }
 437
 438   friend FGQuaternion operator*(double, const FGQuaternion&);
 439
 440   /** Length of the vector.
 441
 442       Compute and return the euclidean norm of this vector.
 443   */
 444   double Magnitude(void) const { return sqrt(SqrMagnitude()); }
 445
 446   /** Square of the length of the vector.
 447
 448       Compute and return the square of the euclidean norm of this vector.
 449   */
 450   double SqrMagnitude(void) const {
 451     return  data[0]*data[0] + data[1]*data[1]
 452           + data[2]*data[2] + data[3]*data[3];
 453   }
 454
 455   /** Normalize.
 456
 457       Normalize the vector to have the Magnitude() == 1.0. If the vector
 458       is equal to zero it is left untouched.
 459    */
 460   void Normalize(void);
 461
 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 ); }
 465
 466 private:
 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; }
 470
 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;
 475
 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
 481       is called. */
 482   void ComputeDerived(void) const {
 483     if (!mCacheValid)
 484       ComputeDerivedUnconditional();
 485   }
 486
 487   /** The quaternion values itself. This is the master copy. */
 488   double data[4];
 489
 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;
 497
 498   /** This stores the transformation matrices.  */
 499   mutable FGMatrix33 mT;
 500   mutable FGMatrix33 mTInv;
 501
 502   /** The cached euler angles.  */
 503   mutable FGColumnVector3 mEulerAngles;
 504
 505   /** The cached sines and cosines of the euler angles.  */
 506   mutable FGColumnVector3 mEulerSines;
 507   mutable FGColumnVector3 mEulerCosines;
 508
 509   void InitializeFromEulerAngles(double phi, double tht, double psi);
 510 };
 511
 512 /** Scalar multiplication.
 513
 514     @param scalar scalar value to multiply with.
 515     @param q Vector to multiply.
 516
 517     Multiply the Vector with a scalar value.
 518 */
 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]);
 521 }
 522
 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);
 528
 529 } // namespace JSBSim
 530
 531 #include "FGMatrix33.h"
 532
 533 #endif