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   double norm = Magnitude();
 134   if (norm == 0.0)
 135     return FGQuaternion::zero();
 136   double rnorm = 1.0/norm;
 137
 138   FGQuaternion QDot;
 139   QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
 140   QDot(2) =  0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
 141   QDot(3) =  0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
 142   QDot(4) =  0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
 143   return rnorm*QDot;
 144 }
 145
 146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 147
 148 void FGQuaternion::Normalize()
 149 {
 150   // Note: this does not touch the cache
 151   // since it does not change the orientation ...
 152
 153   double norm = Magnitude();
 154   if (norm == 0.0)
 155     return;
 156
 157   double rnorm = 1.0/norm;
 158   Entry(1) *= rnorm;
 159   Entry(2) *= rnorm;
 160   Entry(3) *= rnorm;
 161   Entry(4) *= rnorm;
 162 }
 163
 164 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 165
 166 // Compute the derived values if required ...
 167 void FGQuaternion::ComputeDerivedUnconditional(void) const
 168 {
 169   mCacheValid = true;
 170
 171   // First normalize the 4-vector
 172   double norm = Magnitude();
 173   if (norm == 0.0)
 174     return;
 175
 176   double rnorm = 1.0/norm;
 177   double q1 = rnorm*Entry(1);
 178   double q2 = rnorm*Entry(2);
 179   double q3 = rnorm*Entry(3);
 180   double q4 = rnorm*Entry(4);
 181
 182   // Now compute the transformation matrix.
 183   double q1q1 = q1*q1;
 184   double q2q2 = q2*q2;
 185   double q3q3 = q3*q3;
 186   double q4q4 = q4*q4;
 187   double q1q2 = q1*q2;
 188   double q1q3 = q1*q3;
 189   double q1q4 = q1*q4;
 190   double q2q3 = q2*q3;
 191   double q2q4 = q2*q4;
 192   double q3q4 = q3*q4;
 193
 194   mT(1,1) = q1q1 + q2q2 - q3q3 - q4q4;
 195   mT(1,2) = 2.0*(q2q3 + q1q4);
 196   mT(1,3) = 2.0*(q2q4 - q1q3);
 197   mT(2,1) = 2.0*(q2q3 - q1q4);
 198   mT(2,2) = q1q1 - q2q2 + q3q3 - q4q4;
 199   mT(2,3) = 2.0*(q3q4 + q1q2);
 200   mT(3,1) = 2.0*(q2q4 + q1q3);
 201   mT(3,2) = 2.0*(q3q4 - q1q2);
 202   mT(3,3) = q1q1 - q2q2 - q3q3 + q4q4;
 203   // Since this is an orthogonal matrix, the inverse is simply
 204   // the transpose.
 205   mTInv = mT;
 206   mTInv.T();
 207
 208   // Compute the Euler-angles
 209   if (mT(3,3) == 0.0)
 210     mEulerAngles(ePhi) = 0.5*M_PI;
 211   else
 212     mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
 213
 214   if (mT(1,3) < -1.0)
 215     mEulerAngles(eTht) = 0.5*M_PI;
 216   else if (1.0 < mT(1,3))
 217     mEulerAngles(eTht) = -0.5*M_PI;
 218   else
 219     mEulerAngles(eTht) = asin(-mT(1,3));
 220
 221   if (mT(1,1) == 0.0)
 222     mEulerAngles(ePsi) = 0.5*M_PI;
 223   else {
 224     double psi = atan2(mT(1,2), mT(1,1));
 225     if (psi < 0.0)
 226       psi += 2*M_PI;
 227     mEulerAngles(ePsi) = psi;
 228   }
 229
 230   // FIXME: may be one can compute those values easier ???
 231   mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
 232   // mEulerSines(eTht) = sin(mEulerAngles(eTht));
 233   mEulerSines(eTht) = -mT(1,3);
 234   mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
 235   mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
 236   mEulerCosines(eTht) = cos(mEulerAngles(eTht));
 237   mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
 238 }
 239
 240 } // namespace JSBSim