1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGQuaternion.cpp
4 Author: Jon Berndt, Mathias Froehlich
7 ------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------------
8 ------- (C) 2004 Mathias Froehlich (Mathias.Froehlich@web.de) ----
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
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
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.
24 Further information about the GNU General Public License can also be found on
25 the world wide web at http://www.gnu.org.
28 -------------------------------------------------------------------------------
30 15/01/04 Mathias Froehlich implemented a quaternion class from many places
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 # include <simgear/compiler.h>
44 # include STL_IOSTREAM
50 # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
51 # include <iostream.h>
55 # if defined(sgi) && !defined(__GNUC__)
66 #include "FGMatrix33.h"
67 #include "FGColumnVector3.h"
69 #include "FGQuaternion.h"
71 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
73 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
77 static const char *IdSrc = "$Id$";
78 static const char *IdHdr = ID_QUATERNION;
80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 FGQuaternion::FGQuaternion(const FGQuaternion& q)
84 : mCacheValid(q.mCacheValid) {
92 mEulerAngles = q.mEulerAngles;
93 mEulerSines = q.mEulerSines;
94 mEulerCosines = q.mEulerCosines;
98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
107 double Sthtd2 = sin(thtd2);
108 double Spsid2 = sin(psid2);
109 double Sphid2 = sin(phid2);
111 double Cthtd2 = cos(thtd2);
112 double Cpsid2 = cos(psid2);
113 double Cphid2 = cos(phid2);
115 double Cphid2Cthtd2 = Cphid2*Cthtd2;
116 double Cphid2Sthtd2 = Cphid2*Sthtd2;
117 double Sphid2Sthtd2 = Sphid2*Sthtd2;
118 double Sphid2Cthtd2 = Sphid2*Cthtd2;
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;
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 Returns the derivative of the quaternion coresponding to the
130 angular velocities PQR.
132 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
133 double norm = Magnitude();
135 return FGQuaternion::zero();
136 double rnorm = 1.0/norm;
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));
146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
148 void FGQuaternion::Normalize()
150 // Note: this does not touch the cache
151 // since it does not change the orientation ...
153 double norm = Magnitude();
157 double rnorm = 1.0/norm;
164 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
166 // Compute the derived values if required ...
167 void FGQuaternion::ComputeDerivedUnconditional(void) const
171 // First normalize the 4-vector
172 double norm = Magnitude();
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);
182 // Now compute the transformation matrix.
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
208 // Compute the Euler-angles
210 mEulerAngles(ePhi) = 0.5*M_PI;
212 mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
215 mEulerAngles(eTht) = 0.5*M_PI;
216 else if (1.0 < mT(1,3))
217 mEulerAngles(eTht) = -0.5*M_PI;
219 mEulerAngles(eTht) = asin(-mT(1,3));
222 mEulerAngles(ePsi) = 0.5*M_PI;
224 double psi = atan2(mT(1,2), mT(1,1));
227 mEulerAngles(ePsi) = psi;
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));
240 } // namespace JSBSim