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.17 2010/11/28 13:15:26 bcoconni 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));
152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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,
158 Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
160 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
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))
170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
172 void FGQuaternion::Normalize()
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;
178 double rnorm = 1.0/norm;
186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 // Compute the derived values if required ...
189 void FGQuaternion::ComputeDerivedUnconditional(void) const
193 double q0 = data[0]; // use some aliases/shorthand for the quat elements.
198 // Now compute the transformation matrix.
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;
220 // Since this is an orthogonal matrix, the inverse is simply the transpose.
225 // Compute the Euler-angles
226 // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
229 mEulerAngles(ePhi) = 0.5*M_PI;
231 mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
234 mEulerAngles(eTht) = 0.5*M_PI;
235 else if (1.0 < mT(1,3))
236 mEulerAngles(eTht) = -0.5*M_PI;
238 mEulerAngles(eTht) = asin(-mT(1,3));
241 mEulerAngles(ePsi) = 0.5*M_PI;
243 double psi = atan2(mT(1,2), mT(1,1));
246 mEulerAngles(ePsi) = psi;
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));
261 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
262 // The bitmasked value choices are as follows:
263 // unset: In this case (the default) JSBSim would only print
264 // out the normally expected messages, essentially echoing
265 // the config files as they are read. If the environment
266 // variable is not set, debug_lvl is set to 1 internally
267 // 0: This requests JSBSim not to output any messages
269 // 1: This value explicity requests the normal JSBSim
271 // 2: This value asks for a message to be printed out when
272 // a class is instantiated
273 // 4: When this value is set, a message is displayed when a
274 // FGModel object executes its Run() method
275 // 8: When this value is set, various runtime state variables
276 // are printed out periodically
277 // 16: When set various parameters are sanity checked and
278 // a message is printed out when they go out of bounds
280 void FGQuaternion::Debug(int from) const
282 if (debug_lvl <= 0) return;
284 if (debug_lvl & 1) { // Standard console startup message output
286 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
287 if (from == 0) cout << "Instantiated: FGQuaternion" << endl;
288 if (from == 1) cout << "Destroyed: FGQuaternion" << endl;
290 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
292 if (debug_lvl & 8 ) { // Runtime state variables
294 if (debug_lvl & 16) { // Sanity checking
295 if (!EqualToRoundoff(Magnitude(), 1.0)) {
296 cout << "Quaternion magnitude differs from 1.0 !" << endl;
297 cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl;
300 if (debug_lvl & 64) {
301 if (from == 0) { // Constructor
302 cout << IdSrc << endl;
303 cout << IdHdr << endl;
308 } // namespace JSBSim