1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGQuaternion.cpp
4 Author: Jon Berndt, Mathias Froehlich
7 ------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.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 Lesser 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 Lesser General Public License for more
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.
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.
28 -------------------------------------------------------------------------------
30 15/01/04 Mathias Froehlich implemented a quaternion class from many places
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
49 #include "FGMatrix33.h"
50 #include "FGColumnVector3.h"
52 #include "FGQuaternion.h"
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.16 2010/06/30 03:13:40 jberndt Exp $";
61 static const char *IdHdr = ID_QUATERNION;
63 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
66 FGQuaternion::FGQuaternion(const FGQuaternion& q) : mCacheValid(q.mCacheValid)
75 mEulerAngles = q.mEulerAngles;
76 mEulerSines = q.mEulerSines;
77 mEulerCosines = q.mEulerCosines;
81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 // Initialize with the three euler angles
84 FGQuaternion::FGQuaternion(double phi, double tht, double psi): mCacheValid(false)
86 InitializeFromEulerAngles(phi, tht, psi);
89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 FGQuaternion::FGQuaternion(FGColumnVector3 vOrient): mCacheValid(false)
93 double phi = vOrient(ePhi);
94 double tht = vOrient(eTht);
95 double psi = vOrient(ePsi);
97 InitializeFromEulerAngles(phi, tht, psi);
100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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.
108 void FGQuaternion::InitializeFromEulerAngles(double phi, double tht, double psi)
110 mEulerAngles(ePhi) = phi;
111 mEulerAngles(eTht) = tht;
112 mEulerAngles(ePsi) = psi;
114 double thtd2 = 0.5*tht;
115 double psid2 = 0.5*psi;
116 double phid2 = 0.5*phi;
118 double Sthtd2 = sin(thtd2);
119 double Spsid2 = sin(psid2);
120 double Sphid2 = sin(phid2);
122 double Cthtd2 = cos(thtd2);
123 double Cpsid2 = cos(psid2);
124 double Cphid2 = cos(phid2);
126 double Cphid2Cthtd2 = Cphid2*Cthtd2;
127 double Cphid2Sthtd2 = Cphid2*Sthtd2;
128 double Sphid2Sthtd2 = Sphid2*Sthtd2;
129 double Sphid2Cthtd2 = Sphid2*Cthtd2;
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;
138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139 // Initialize with a direction cosine (rotation) matrix
141 FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
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));
149 ComputeDerivedUnconditional();
154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
156 /** Returns the derivative of the quaternion corresponding to the
157 angular velocities PQR.
158 See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
160 Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
162 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
165 -0.5*( data[1]*PQR(eP) + data[2]*PQR(eQ) + data[3]*PQR(eR)),
166 0.5*( data[0]*PQR(eP) - data[3]*PQR(eQ) + data[2]*PQR(eR)),
167 0.5*( data[3]*PQR(eP) + data[0]*PQR(eQ) - data[1]*PQR(eR)),
168 0.5*(-data[2]*PQR(eP) + data[1]*PQR(eQ) + data[0]*PQR(eR))
172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
174 void FGQuaternion::Normalize()
176 // Note: this does not touch the cache since it does not change the orientation
177 double norm = Magnitude();
178 if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
180 double rnorm = 1.0/norm;
188 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
190 // Compute the derived values if required ...
191 void FGQuaternion::ComputeDerivedUnconditional(void) const
195 double q0 = data[0]; // use some aliases/shorthand for the quat elements.
200 // Now compute the transformation matrix.
212 mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
213 mT(1,2) = 2.0*(q1q2 + q0q3); // Stevens and Lewis
214 mT(1,3) = 2.0*(q1q3 - q0q2);
215 mT(2,1) = 2.0*(q1q2 - q0q3);
216 mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
217 mT(2,3) = 2.0*(q2q3 + q0q1);
218 mT(3,1) = 2.0*(q1q3 + q0q2);
219 mT(3,2) = 2.0*(q2q3 - q0q1);
220 mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
222 // Since this is an orthogonal matrix, the inverse is simply the transpose.
227 // Compute the Euler-angles
228 // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
231 mEulerAngles(ePhi) = 0.5*M_PI;
233 mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
236 mEulerAngles(eTht) = 0.5*M_PI;
237 else if (1.0 < mT(1,3))
238 mEulerAngles(eTht) = -0.5*M_PI;
240 mEulerAngles(eTht) = asin(-mT(1,3));
243 mEulerAngles(ePsi) = 0.5*M_PI;
245 double psi = atan2(mT(1,2), mT(1,1));
248 mEulerAngles(ePsi) = psi;
251 // FIXME: may be one can compute those values easier ???
252 mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
253 // mEulerSines(eTht) = sin(mEulerAngles(eTht));
254 mEulerSines(eTht) = -mT(1,3);
255 mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
256 mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
257 mEulerCosines(eTht) = cos(mEulerAngles(eTht));
258 mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
263 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
264 // The bitmasked value choices are as follows:
265 // unset: In this case (the default) JSBSim would only print
266 // out the normally expected messages, essentially echoing
267 // the config files as they are read. If the environment
268 // variable is not set, debug_lvl is set to 1 internally
269 // 0: This requests JSBSim not to output any messages
271 // 1: This value explicity requests the normal JSBSim
273 // 2: This value asks for a message to be printed out when
274 // a class is instantiated
275 // 4: When this value is set, a message is displayed when a
276 // FGModel object executes its Run() method
277 // 8: When this value is set, various runtime state variables
278 // are printed out periodically
279 // 16: When set various parameters are sanity checked and
280 // a message is printed out when they go out of bounds
282 void FGQuaternion::Debug(int from) const
284 if (debug_lvl <= 0) return;
286 if (debug_lvl & 1) { // Standard console startup message output
288 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
289 if (from == 0) cout << "Instantiated: FGQuaternion" << endl;
290 if (from == 1) cout << "Destroyed: FGQuaternion" << endl;
292 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
294 if (debug_lvl & 8 ) { // Runtime state variables
296 if (debug_lvl & 16) { // Sanity checking
297 if (!EqualToRoundoff(Magnitude(), 1.0)) {
298 cout << "Quaternion magnitude differs from 1.0 !" << endl;
299 cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl;
302 if (debug_lvl & 64) {
303 if (from == 0) { // Constructor
304 cout << IdSrc << endl;
305 cout << IdHdr << endl;
310 } // namespace JSBSim