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.19 2010/12/07 12:57:14 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   Normalize();
 150 }
 151
 152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 153
 154 /** Returns the derivative of the quaternion corresponding to the
 155     angular velocities PQR.
 156     See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
 157     Equation 1.3-36.
 158     Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
 159 */
 160 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
 161 {
 162   return FGQuaternion(
 163     -0.5*( data[1]*PQR(eP) + data[2]*PQR(eQ) + data[3]*PQR(eR)),
 164      0.5*( data[0]*PQR(eP) - data[3]*PQR(eQ) + data[2]*PQR(eR)),
 165      0.5*( data[3]*PQR(eP) + data[0]*PQR(eQ) - data[1]*PQR(eR)),
 166      0.5*(-data[2]*PQR(eP) + data[1]*PQR(eQ) + data[0]*PQR(eR))
 167   );
 168 }
 169
 170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 171
 172 void FGQuaternion::Normalize()
 173 {
 174   // Note: this does not touch the cache since it does not change the orientation
 175   double norm = Magnitude();
 176   if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
 177
 178   double rnorm = 1.0/norm;
 179
 180   data[0] *= rnorm;
 181   data[1] *= rnorm;
 182   data[2] *= rnorm;
 183   data[3] *= rnorm;
 184 }
 185
 186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 187
 188 // Compute the derived values if required ...
 189 void FGQuaternion::ComputeDerivedUnconditional(void) const
 190 {
 191   mCacheValid = true;
 192
 193   double q0 = data[0]; // use some aliases/shorthand for the quat elements.
 194   double q1 = data[1];
 195   double q2 = data[2];
 196   double q3 = data[3];
 197
 198   // Now compute the transformation matrix.
 199   double q0q0 = q0*q0;
 200   double q1q1 = q1*q1;
 201   double q2q2 = q2*q2;
 202   double q3q3 = q3*q3;
 203   double q0q1 = q0*q1;
 204   double q0q2 = q0*q2;
 205   double q0q3 = q0*q3;
 206   double q1q2 = q1*q2;
 207   double q1q3 = q1*q3;
 208   double q2q3 = q2*q3;
 209
 210   mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
 211   mT(1,2) = 2.0*(q1q2 + q0q3);         // Stevens and Lewis
 212   mT(1,3) = 2.0*(q1q3 - q0q2);
 213   mT(2,1) = 2.0*(q1q2 - q0q3);
 214   mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
 215   mT(2,3) = 2.0*(q2q3 + q0q1);
 216   mT(3,1) = 2.0*(q1q3 + q0q2);
 217   mT(3,2) = 2.0*(q2q3 - q0q1);
 218   mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
 219
 220   // Since this is an orthogonal matrix, the inverse is simply the transpose.
 221
 222   mTInv = mT;
 223   mTInv.T();
 224
 225   // Compute the Euler-angles
 226   // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
 227
 228   if (mT(3,3) == 0.0)
 229     mEulerAngles(ePhi) = 0.5*M_PI;
 230   else
 231     mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
 232
 233   if (mT(1,3) < -1.0)
 234     mEulerAngles(eTht) = 0.5*M_PI;
 235   else if (1.0 < mT(1,3))
 236     mEulerAngles(eTht) = -0.5*M_PI;
 237   else
 238     mEulerAngles(eTht) = asin(-mT(1,3));
 239
 240   if (mT(1,1) == 0.0)
 241     mEulerAngles(ePsi) = 0.5*M_PI;
 242   else {
 243     double psi = atan2(mT(1,2), mT(1,1));
 244     if (psi < 0.0)
 245       psi += 2*M_PI;
 246     mEulerAngles(ePsi) = psi;
 247   }
 248
 249   // FIXME: may be one can compute those values easier ???
 250   mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
 251   // mEulerSines(eTht) = sin(mEulerAngles(eTht));
 252   mEulerSines(eTht) = -mT(1,3);
 253   mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
 254   mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
 255   mEulerCosines(eTht) = cos(mEulerAngles(eTht));
 256   mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
 257 }
 258
 259 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 260
 261 std::ostream& operator<<(std::ostream& os, const FGQuaternion& q)
 262 {
 263   os << q(1) << " , " << q(2) << " , " << q(3) << " , " << q(4);
 264   return os;
 265 }
 266
 267 } // namespace JSBSim