Branch data Line data Source code
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.16 2010/06/30 03:13:40 jberndt Exp $";
61 : : static const char *IdHdr = ID_QUATERNION;
62 : :
63 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
64 : :
65 : : // Initialize from q
66 : 54013 : FGQuaternion::FGQuaternion(const FGQuaternion& q) : mCacheValid(q.mCacheValid)
67 : : {
68 : 54013 : data[0] = q(1);
69 : 54013 : data[1] = q(2);
70 : 54013 : data[2] = q(3);
71 : 54013 : data[3] = q(4);
72 [ - + # # ]: 54013 : if (mCacheValid) {
73 : 0 : mT = q.mT;
74 : 0 : mTInv = q.mTInv;
75 : 0 : mEulerAngles = q.mEulerAngles;
76 : 0 : mEulerSines = q.mEulerSines;
77 : 0 : mEulerCosines = q.mEulerCosines;
78 : : }
79 : 54013 : }
80 : :
81 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82 : :
83 : : // Initialize with the three euler angles
84 : 4 : FGQuaternion::FGQuaternion(double phi, double tht, double psi): mCacheValid(false)
85 : : {
86 : 4 : InitializeFromEulerAngles(phi, tht, psi);
87 : 4 : }
88 : :
89 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 : :
91 : 1 : FGQuaternion::FGQuaternion(FGColumnVector3 vOrient): mCacheValid(false)
92 : : {
93 : 1 : double phi = vOrient(ePhi);
94 : 1 : double tht = vOrient(eTht);
95 : 1 : double psi = vOrient(ePsi);
96 : :
97 : 1 : InitializeFromEulerAngles(phi, tht, psi);
98 : 1 : }
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 : 5 : void FGQuaternion::InitializeFromEulerAngles(double phi, double tht, double psi)
109 : : {
110 : 10 : mEulerAngles(ePhi) = phi;
111 : 10 : mEulerAngles(eTht) = tht;
112 : 10 : mEulerAngles(ePsi) = psi;
113 : :
114 : 5 : double thtd2 = 0.5*tht;
115 : 5 : double psid2 = 0.5*psi;
116 : 5 : double phid2 = 0.5*phi;
117 : :
118 : 5 : double Sthtd2 = sin(thtd2);
119 : 5 : double Spsid2 = sin(psid2);
120 : 5 : double Sphid2 = sin(phid2);
121 : :
122 : 5 : double Cthtd2 = cos(thtd2);
123 : 5 : double Cpsid2 = cos(psid2);
124 : 5 : double Cphid2 = cos(phid2);
125 : :
126 : 5 : double Cphid2Cthtd2 = Cphid2*Cthtd2;
127 : 5 : double Cphid2Sthtd2 = Cphid2*Sthtd2;
128 : 5 : double Sphid2Sthtd2 = Sphid2*Sthtd2;
129 : 5 : double Sphid2Cthtd2 = Sphid2*Cthtd2;
130 : :
131 : 5 : data[0] = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
132 : 5 : data[1] = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
133 : 5 : data[2] = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
134 : 5 : data[3] = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
135 : :
136 : 5 : Normalize();
137 : 5 : }
138 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139 : : // Initialize with a direction cosine (rotation) matrix
140 : :
141 : 0 : FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
142 : : {
143 : 0 : data[0] = 0.50*sqrt(1.0 + m(1,1) + m(2,2) + m(3,3));
144 : 0 : double t = 0.25/data[0];
145 : 0 : data[1] = t*(m(2,3) - m(3,2));
146 : 0 : data[2] = t*(m(3,1) - m(1,3));
147 : 0 : data[3] = t*(m(1,2) - m(2,1));
148 : :
149 : 0 : ComputeDerivedUnconditional();
150 : :
151 : 0 : Normalize();
152 : 0 : }
153 : :
154 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
155 : :
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,
159 : : Equation 1.3-36.
160 : : Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
161 : : */
162 : 54006 : FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
163 : : {
164 : : return FGQuaternion(
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))
169 : 702078 : );
170 : : }
171 : :
172 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
173 : :
174 : 54010 : void FGQuaternion::Normalize()
175 : : {
176 : : // Note: this does not touch the cache since it does not change the orientation
177 : 54010 : double norm = Magnitude();
178 [ + - ][ + + ]: 54010 : if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
179 : :
180 : 149 : double rnorm = 1.0/norm;
181 : :
182 : 149 : data[0] *= rnorm;
183 : 149 : data[1] *= rnorm;
184 : 149 : data[2] *= rnorm;
185 : 54010 : data[3] *= rnorm;
186 : : }
187 : :
188 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
189 : :
190 : : // Compute the derived values if required ...
191 : 162022 : void FGQuaternion::ComputeDerivedUnconditional(void) const
192 : : {
193 : 162022 : mCacheValid = true;
194 : :
195 : 162022 : double q0 = data[0]; // use some aliases/shorthand for the quat elements.
196 : 162022 : double q1 = data[1];
197 : 162022 : double q2 = data[2];
198 : 162022 : double q3 = data[3];
199 : :
200 : : // Now compute the transformation matrix.
201 : 162022 : double q0q0 = q0*q0;
202 : 162022 : double q1q1 = q1*q1;
203 : 162022 : double q2q2 = q2*q2;
204 : 162022 : double q3q3 = q3*q3;
205 : 162022 : double q0q1 = q0*q1;
206 : 162022 : double q0q2 = q0*q2;
207 : 162022 : double q0q3 = q0*q3;
208 : 162022 : double q1q2 = q1*q2;
209 : 162022 : double q1q3 = q1*q3;
210 : 162022 : double q2q3 = q2*q3;
211 : :
212 : 324044 : mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
213 : 324044 : mT(1,2) = 2.0*(q1q2 + q0q3); // Stevens and Lewis
214 : 324044 : mT(1,3) = 2.0*(q1q3 - q0q2);
215 : 324044 : mT(2,1) = 2.0*(q1q2 - q0q3);
216 : 324044 : mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
217 : 324044 : mT(2,3) = 2.0*(q2q3 + q0q1);
218 : 324044 : mT(3,1) = 2.0*(q1q3 + q0q2);
219 : 324044 : mT(3,2) = 2.0*(q2q3 - q0q1);
220 : 324044 : mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
221 : :
222 : : // Since this is an orthogonal matrix, the inverse is simply the transpose.
223 : :
224 : 162022 : mTInv = mT;
225 : 162022 : mTInv.T();
226 : :
227 : : // Compute the Euler-angles
228 : : // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
229 : :
230 [ - + ]: 162022 : if (mT(3,3) == 0.0)
231 : 0 : mEulerAngles(ePhi) = 0.5*M_PI;
232 : : else
233 : 162022 : mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
234 : :
235 [ + + ]: 162022 : if (mT(1,3) < -1.0)
236 : 1456 : mEulerAngles(eTht) = 0.5*M_PI;
237 [ - + ]: 160566 : else if (1.0 < mT(1,3))
238 : 0 : mEulerAngles(eTht) = -0.5*M_PI;
239 : : else
240 : 160566 : mEulerAngles(eTht) = asin(-mT(1,3));
241 : :
242 [ - + ]: 162022 : if (mT(1,1) == 0.0)
243 : 0 : mEulerAngles(ePsi) = 0.5*M_PI;
244 : : else {
245 : 162022 : double psi = atan2(mT(1,2), mT(1,1));
246 [ + + ]: 162022 : if (psi < 0.0)
247 : 54725 : psi += 2*M_PI;
248 : 162022 : mEulerAngles(ePsi) = psi;
249 : : }
250 : :
251 : : // FIXME: may be one can compute those values easier ???
252 : 162022 : mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
253 : : // mEulerSines(eTht) = sin(mEulerAngles(eTht));
254 : 486066 : mEulerSines(eTht) = -mT(1,3);
255 : 162022 : mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
256 : 162022 : mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
257 : 162022 : mEulerCosines(eTht) = cos(mEulerAngles(eTht));
258 : 162022 : mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
259 : :
260 : : // Debug(2);
261 : 162022 : }
262 : :
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
270 : : // whatsoever.
271 : : // 1: This value explicity requests the normal JSBSim
272 : : // startup messages
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
281 : :
282 : 0 : void FGQuaternion::Debug(int from) const
283 : : {
284 [ # # ]: 0 : if (debug_lvl <= 0) return;
285 : :
286 : 0 : if (debug_lvl & 1) { // Standard console startup message output
287 : : }
288 [ # # ]: 0 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
289 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGQuaternion" << endl;
290 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGQuaternion" << endl;
291 : : }
292 : 0 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
293 : : }
294 : 0 : if (debug_lvl & 8 ) { // Runtime state variables
295 : : }
296 [ # # ]: 0 : if (debug_lvl & 16) { // Sanity checking
297 [ # # ]: 0 : if (!EqualToRoundoff(Magnitude(), 1.0)) {
298 : 0 : cout << "Quaternion magnitude differs from 1.0 !" << endl;
299 : 0 : cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl;
300 : : }
301 : : }
302 [ # # ]: 0 : if (debug_lvl & 64) {
303 [ # # ]: 0 : if (from == 0) { // Constructor
304 : 0 : cout << IdSrc << endl;
305 : 0 : cout << IdHdr << endl;
306 : : }
307 : : }
308 : : }
309 : :
310 [ + + ][ + - ]: 12 : } // namespace JSBSim
|
