src/FDM/JSBSim/math/FGQuaternion.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGQuaternion.cpp
   4  Author:       Jon Berndt, Mathias 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   Mathias Froehlich implemented a quaternion class from many places
  31            in JSBSim.
  32
  33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  34 SENTRY
  35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  36
  37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38   INCLUDES
  39   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include <string>
  42 #include <iostream>
  43 #include <iomanip>
  44 #include <cmath>
  45 using std::cerr;
  46 using std::cout;
  47 using std::endl;
  48
  49 #include "FGMatrix33.h"
  50 #include "FGColumnVector3.h"
  51
  52 #include "FGQuaternion.h"
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55   DEFINITIONS
  56   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 namespace JSBSim {
  59
  60 static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.16 2010/06/30 03:13:40 jberndt Exp $";
  61 static const char *IdHdr = ID_QUATERNION;
  62
  63 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  64
  65 // Initialize from q
  66 FGQuaternion::FGQuaternion(const FGQuaternion& q) : mCacheValid(q.mCacheValid)
  67 {
  68   data[0] = q(1);
  69   data[1] = q(2);
  70   data[2] = q(3);
  71   data[3] = q(4);
  72   if (mCacheValid) {
  73     mT = q.mT;
  74     mTInv = q.mTInv;
  75     mEulerAngles = q.mEulerAngles;
  76     mEulerSines = q.mEulerSines;
  77     mEulerCosines = q.mEulerCosines;
  78   }
  79 }
  80
  81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82
  83 // Initialize with the three euler angles
  84 FGQuaternion::FGQuaternion(double phi, double tht, double psi): mCacheValid(false)
  85 {
  86   InitializeFromEulerAngles(phi, tht, psi);
  87 }
  88
  89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  90
  91 FGQuaternion::FGQuaternion(FGColumnVector3 vOrient): mCacheValid(false)
  92 {
  93   double phi = vOrient(ePhi);
  94   double tht = vOrient(eTht);
  95   double psi = vOrient(ePsi);
  96
  97   InitializeFromEulerAngles(phi, tht, psi);
  98 }
  99
 100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 101 //
 102 // This function computes the quaternion that describes the relationship of the
 103 // body frame with respect to another frame, such as the ECI or ECEF frames. The
 104 // Euler angles used represent a 3-2-1 (psi, theta, phi) rotation sequence from
 105 // the reference frame to the body frame. See "Quaternions and Rotation
 106 // Sequences", Jack B. Kuipers, sections 9.2 and 7.6.
 107
 108 void FGQuaternion::InitializeFromEulerAngles(double phi, double tht, double psi)
 109 {
 110   mEulerAngles(ePhi) = phi;
 111   mEulerAngles(eTht) = tht;
 112   mEulerAngles(ePsi) = psi;
 113
 114   double thtd2 = 0.5*tht;
 115   double psid2 = 0.5*psi;
 116   double phid2 = 0.5*phi;
 117
 118   double Sthtd2 = sin(thtd2);
 119   double Spsid2 = sin(psid2);
 120   double Sphid2 = sin(phid2);
 121
 122   double Cthtd2 = cos(thtd2);
 123   double Cpsid2 = cos(psid2);
 124   double Cphid2 = cos(phid2);
 125
 126   double Cphid2Cthtd2 = Cphid2*Cthtd2;
 127   double Cphid2Sthtd2 = Cphid2*Sthtd2;
 128   double Sphid2Sthtd2 = Sphid2*Sthtd2;
 129   double Sphid2Cthtd2 = Sphid2*Cthtd2;
 130
 131   data[0] = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
 132   data[1] = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
 133   data[2] = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
 134   data[3] = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
 135
 136   Normalize();
 137 }
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139 // Initialize with a direction cosine (rotation) matrix
 140
 141 FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
 142 {
 143   data[0] = 0.50*sqrt(1.0 + m(1,1) + m(2,2) + m(3,3));
 144   double t = 0.25/data[0];
 145   data[1] = t*(m(2,3) - m(3,2));
 146   data[2] = t*(m(3,1) - m(1,3));
 147   data[3] = t*(m(1,2) - m(2,1));
 148
 149   ComputeDerivedUnconditional();
 150
 151   Normalize();
 152 }
 153
 154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 155
 156 /** Returns the derivative of the quaternion corresponding to the
 157     angular velocities PQR.
 158     See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 159     Equation 1.3-36.
 160     Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
 161 */
 162 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
 163 {
 164   return FGQuaternion(
 165     -0.5*( data[1]*PQR(eP) + data[2]*PQR(eQ) + data[3]*PQR(eR)),
 166      0.5*( data[0]*PQR(eP) - data[3]*PQR(eQ) + data[2]*PQR(eR)),
 167      0.5*( data[3]*PQR(eP) + data[0]*PQR(eQ) - data[1]*PQR(eR)),
 168      0.5*(-data[2]*PQR(eP) + data[1]*PQR(eQ) + data[0]*PQR(eR))
 169   );
 170 }
 171
 172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 173
 174 void FGQuaternion::Normalize()
 175 {
 176   // Note: this does not touch the cache since it does not change the orientation
 177   double norm = Magnitude();
 178   if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
 179
 180   double rnorm = 1.0/norm;
 181
 182   data[0] *= rnorm;
 183   data[1] *= rnorm;
 184   data[2] *= rnorm;
 185   data[3] *= rnorm;
 186 }
 187
 188 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 189
 190 // Compute the derived values if required ...
 191 void FGQuaternion::ComputeDerivedUnconditional(void) const
 192 {
 193   mCacheValid = true;
 194
 195   double q0 = data[0]; // use some aliases/shorthand for the quat elements.
 196   double q1 = data[1];
 197   double q2 = data[2];
 198   double q3 = data[3];
 199
 200   // Now compute the transformation matrix.
 201   double q0q0 = q0*q0;
 202   double q1q1 = q1*q1;
 203   double q2q2 = q2*q2;
 204   double q3q3 = q3*q3;
 205   double q0q1 = q0*q1;
 206   double q0q2 = q0*q2;
 207   double q0q3 = q0*q3;
 208   double q1q2 = q1*q2;
 209   double q1q3 = q1*q3;
 210   double q2q3 = q2*q3;
 211
 212   mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
 213   mT(1,2) = 2.0*(q1q2 + q0q3);         // Stevens and Lewis
 214   mT(1,3) = 2.0*(q1q3 - q0q2);
 215   mT(2,1) = 2.0*(q1q2 - q0q3);
 216   mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
 217   mT(2,3) = 2.0*(q2q3 + q0q1);
 218   mT(3,1) = 2.0*(q1q3 + q0q2);
 219   mT(3,2) = 2.0*(q2q3 - q0q1);
 220   mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
 221
 222   // Since this is an orthogonal matrix, the inverse is simply the transpose.
 223
 224   mTInv = mT;
 225   mTInv.T();
 226
 227   // Compute the Euler-angles
 228   // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
 229
 230   if (mT(3,3) == 0.0)
 231     mEulerAngles(ePhi) = 0.5*M_PI;
 232   else
 233     mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
 234
 235   if (mT(1,3) < -1.0)
 236     mEulerAngles(eTht) = 0.5*M_PI;
 237   else if (1.0 < mT(1,3))
 238     mEulerAngles(eTht) = -0.5*M_PI;
 239   else
 240     mEulerAngles(eTht) = asin(-mT(1,3));
 241
 242   if (mT(1,1) == 0.0)
 243     mEulerAngles(ePsi) = 0.5*M_PI;
 244   else {
 245     double psi = atan2(mT(1,2), mT(1,1));
 246     if (psi < 0.0)
 247       psi += 2*M_PI;
 248     mEulerAngles(ePsi) = psi;
 249   }
 250
 251   // FIXME: may be one can compute those values easier ???
 252   mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
 253   // mEulerSines(eTht) = sin(mEulerAngles(eTht));
 254   mEulerSines(eTht) = -mT(1,3);
 255   mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
 256   mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
 257   mEulerCosines(eTht) = cos(mEulerAngles(eTht));
 258   mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
 259
 260 //  Debug(2);
 261 }
 262
 263 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 264 //    The bitmasked value choices are as follows:
 265 //    unset: In this case (the default) JSBSim would only print
 266 //       out the normally expected messages, essentially echoing
 267 //       the config files as they are read. If the environment
 268 //       variable is not set, debug_lvl is set to 1 internally
 269 //    0: This requests JSBSim not to output any messages
 270 //       whatsoever.
 271 //    1: This value explicity requests the normal JSBSim
 272 //       startup messages
 273 //    2: This value asks for a message to be printed out when
 274 //       a class is instantiated
 275 //    4: When this value is set, a message is displayed when a
 276 //       FGModel object executes its Run() method
 277 //    8: When this value is set, various runtime state variables
 278 //       are printed out periodically
 279 //    16: When set various parameters are sanity checked and
 280 //       a message is printed out when they go out of bounds
 281
 282 void FGQuaternion::Debug(int from) const
 283 {
 284   if (debug_lvl <= 0) return;
 285
 286   if (debug_lvl & 1) { // Standard console startup message output
 287   }
 288   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 289     if (from == 0) cout << "Instantiated: FGQuaternion" << endl;
 290     if (from == 1) cout << "Destroyed:    FGQuaternion" << endl;
 291   }
 292   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 293   }
 294   if (debug_lvl & 8 ) { // Runtime state variables
 295   }
 296   if (debug_lvl & 16) { // Sanity checking
 297     if (!EqualToRoundoff(Magnitude(), 1.0)) {
 298       cout << "Quaternion magnitude differs from 1.0 !" << endl;
 299       cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl;
 300     }
 301   }
 302   if (debug_lvl & 64) {
 303     if (from == 0) { // Constructor
 304       cout << IdSrc << endl;
 305       cout << IdHdr << endl;
 306     }
 307   }
 308 }
 309
 310 } // namespace JSBSim