src/FDM/JSBSim/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 (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   Mathias Froehlich implemented a quaternion class from many places
  31            in JSBSim.
  32
  33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  34 SENTRY
  35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  36
  37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38   INCLUDES
  39   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #ifdef FGFS
  42 #  include <math.h>
  43 #  include <simgear/compiler.h>
  44 #  include STL_IOSTREAM
  45    SG_USING_STD(cerr);
  46    SG_USING_STD(cout);
  47    SG_USING_STD(endl);
  48 #else
  49 #  include <string>
  50 #  if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
  51 #    include <iostream.h>
  52 #    include <math.h>
  53 #  else
  54 #    include <iostream>
  55 #    if defined(sgi) && !defined(__GNUC__)
  56 #      include <math.h>
  57 #    else
  58 #      include <cmath>
  59 #    endif
  60      using std::cerr;
  61      using std::cout;
  62      using std::endl;
  63 #  endif
  64 #endif
  65
  66 #include "FGMatrix33.h"
  67 #include "FGColumnVector3.h"
  68
  69 #include "FGQuaternion.h"
  70
  71 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  72   DEFINITIONS
  73   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  74
  75 namespace JSBSim {
  76
  77 static const char *IdSrc = "$Id$";
  78 static const char *IdHdr = ID_QUATERNION;
  79
  80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  81
  82 // Initialize from q
  83 FGQuaternion::FGQuaternion(const FGQuaternion& q)
  84   : mCacheValid(q.mCacheValid) {
  85   Entry(1) = q(1);
  86   Entry(2) = q(2);
  87   Entry(3) = q(3);
  88   Entry(4) = q(4);
  89   if (mCacheValid) {
  90     mT = q.mT;
  91     mTInv = q.mTInv;
  92     mEulerAngles = q.mEulerAngles;
  93     mEulerSines = q.mEulerSines;
  94     mEulerCosines = q.mEulerCosines;
  95   }
  96 }
  97
  98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  99
 100 // Initialize with the three euler angles
 101 FGQuaternion::FGQuaternion(double phi, double tht, double psi)
 102   : mCacheValid(false) {
 103   double thtd2 = 0.5*tht;
 104   double psid2 = 0.5*psi;
 105   double phid2 = 0.5*phi;
 106
 107   double Sthtd2 = sin(thtd2);
 108   double Spsid2 = sin(psid2);
 109   double Sphid2 = sin(phid2);
 110
 111   double Cthtd2 = cos(thtd2);
 112   double Cpsid2 = cos(psid2);
 113   double Cphid2 = cos(phid2);
 114
 115   double Cphid2Cthtd2 = Cphid2*Cthtd2;
 116   double Cphid2Sthtd2 = Cphid2*Sthtd2;
 117   double Sphid2Sthtd2 = Sphid2*Sthtd2;
 118   double Sphid2Cthtd2 = Sphid2*Cthtd2;
 119
 120   Entry(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
 121   Entry(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
 122   Entry(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
 123   Entry(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 /**
 129    Returns the derivative of the quaternion coresponding to the
 130    angular velocities PQR.
 131 */
 132 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
 133   FGQuaternion QDot;
 134   QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
 135   QDot(2) =  0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
 136   QDot(3) =  0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
 137   QDot(4) =  0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
 138   return QDot;
 139 }
 140
 141 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 142
 143 void FGQuaternion::Normalize()
 144 {
 145   // Note: this does not touch the cache
 146   // since it does not change the orientation ...
 147
 148   double norm = Magnitude();
 149   if (norm == 0.0)
 150     return;
 151
 152   double rnorm = 1.0/norm;
 153   Entry(1) *= rnorm;
 154   Entry(2) *= rnorm;
 155   Entry(3) *= rnorm;
 156   Entry(4) *= rnorm;
 157 }
 158
 159 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 160
 161 // Compute the derived values if required ...
 162 void FGQuaternion::ComputeDerivedUnconditional(void) const
 163 {
 164   mCacheValid = true;
 165
 166   // First normalize the 4-vector
 167   double norm = Magnitude();
 168   if (norm == 0.0)
 169     return;
 170
 171   double rnorm = 1.0/norm;
 172   double q1 = rnorm*Entry(1);
 173   double q2 = rnorm*Entry(2);
 174   double q3 = rnorm*Entry(3);
 175   double q4 = rnorm*Entry(4);
 176
 177   // Now compute the transformation matrix.
 178   double q1q1 = q1*q1;
 179   double q2q2 = q2*q2;
 180   double q3q3 = q3*q3;
 181   double q4q4 = q4*q4;
 182   double q1q2 = q1*q2;
 183   double q1q3 = q1*q3;
 184   double q1q4 = q1*q4;
 185   double q2q3 = q2*q3;
 186   double q2q4 = q2*q4;
 187   double q3q4 = q3*q4;
 188
 189   mT(1,1) = q1q1 + q2q2 - q3q3 - q4q4;
 190   mT(1,2) = 2.0*(q2q3 + q1q4);
 191   mT(1,3) = 2.0*(q2q4 - q1q3);
 192   mT(2,1) = 2.0*(q2q3 - q1q4);
 193   mT(2,2) = q1q1 - q2q2 + q3q3 - q4q4;
 194   mT(2,3) = 2.0*(q3q4 + q1q2);
 195   mT(3,1) = 2.0*(q2q4 + q1q3);
 196   mT(3,2) = 2.0*(q3q4 - q1q2);
 197   mT(3,3) = q1q1 - q2q2 - q3q3 + q4q4;
 198   // Since this is an orthogonal matrix, the inverse is simply
 199   // the transpose.
 200   mTInv = mT;
 201   mTInv.T();
 202
 203   // Compute the Euler-angles
 204   if (mT(3,3) == 0.0)
 205     mEulerAngles(ePhi) = 0.5*M_PI;
 206   else
 207     mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
 208
 209   if (mT(1,3) < -1.0)
 210     mEulerAngles(eTht) = 0.5*M_PI;
 211   else if (1.0 < mT(1,3))
 212     mEulerAngles(eTht) = -0.5*M_PI;
 213   else
 214     mEulerAngles(eTht) = asin(-mT(1,3));
 215
 216   if (mT(1,1) == 0.0)
 217     mEulerAngles(ePsi) = 0.5*M_PI;
 218   else {
 219     double psi = atan2(mT(1,2), mT(1,1));
 220     if (psi < 0.0)
 221       psi += 2*M_PI;
 222     mEulerAngles(ePsi) = psi;
 223   }
 224
 225   // FIXME: may be one can compute those values easier ???
 226   mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
 227   // mEulerSines(eTht) = sin(mEulerAngles(eTht));
 228   mEulerSines(eTht) = -mT(1,3);
 229   mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
 230   mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
 231   mEulerCosines(eTht) = cos(mEulerAngles(eTht));
 232   mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
 233 }
 234
 235 } // namespace JSBSim