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