1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Header: FGQuaternion.h
4 Author: Jon Berndt, Mathis 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 MF Quaternion class from old FGColumnVector4
31
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 SENTRY
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
35
36 #ifndef FGQUATERNION_H
37 #define FGQUATERNION_H
38
39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 INCLUDES
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
42
43 #include <string>
44 #include "FGJSBBase.h"
45 #include "FGColumnVector3.h"
46
47 namespace JSBSim {
48
49 class FGMatrix33;
50
51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 CLASS DOCUMENTATION
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54
55 /** Models the Quaternion representation of rotations.
56 FGQuaternion is a representation of an arbitrary rotation through a
57 quaternion. It has vector properties. This class also contains access
58 functions to the euler angle representation of rotations and access to
59 transformation matrices for 3D vectors. Transformations and euler angles are
60 therefore computed once they are requested for the first time. Then they are
61 cached for later usage as long as the class is not accessed trough
62 a nonconst member function.
63
64 Note: The order of rotations used in this class corresponds to a 3-2-1 sequence,
65 or Y-P-R, or Z-Y-X, if you prefer.
66
67 @see Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
68 Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
69 School, January 1994
70 @see D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
71 JSC 12960, July 1977
72 @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
73 NASA-Ames", NASA CR-2497, January 1975
74 @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
75 Wiley & Sons, 1979 ISBN 0-471-03032-5
76 @see Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
77 1982 ISBN 0-471-08936-2
78 @author Mathias Froehlich, extended FGColumnVector4 originally by Tony Peden
79 and Jon Berndt
80 */
81
82 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 CLASS DECLARATION
84 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
85
86 class FGQuaternion : public FGJSBBase {
87 public:
88 /** Default initializer.
89 Default initializer, initializes the class with the identity rotation. */
FGQuaternion()90 FGQuaternion() : mCacheValid(false) {
91 data[0] = 1.0;
92 data[1] = data[2] = data[3] = 0.0;
93 }
94
95 /** Copy constructor.
96 Copy constructor, initializes the quaternion.
97 @param q a constant reference to another FGQuaternion instance */
98 FGQuaternion(const FGQuaternion& q);
99
100 /** Initializer by euler angles.
101 Initialize the quaternion with the euler angles.
102 @param phi The euler X axis (roll) angle in radians
103 @param tht The euler Y axis (attitude) angle in radians
104 @param psi The euler Z axis (heading) angle in radians */
105 FGQuaternion(double phi, double tht, double psi);
106
107 /** Initializer by euler angle vector.
108 Initialize the quaternion with the euler angle vector.
109 @param vOrient The euler axis angle vector in radians (phi, tht, psi) */
110 FGQuaternion(FGColumnVector3 vOrient);
111
112 /** Initializer by one euler angle.
113 Initialize the quaternion with the single euler angle where its index
114 is given in the first argument.
115 @param idx Index of the euler angle to initialize
116 @param angle The euler angle in radians */
FGQuaternion(int idx,double angle)117 FGQuaternion(int idx, double angle)
118 : mCacheValid(false) {
119
120 double angle2 = 0.5*angle;
121
122 double Sangle2 = sin(angle2);
123 double Cangle2 = cos(angle2);
124
125 if (idx == ePhi) {
126 data[0] = Cangle2;
127 data[1] = Sangle2;
128 data[2] = 0.0;
129 data[3] = 0.0;
130
131 } else if (idx == eTht) {
132 data[0] = Cangle2;
133 data[1] = 0.0;
134 data[2] = Sangle2;
135 data[3] = 0.0;
136
137 } else {
138 data[0] = Cangle2;
139 data[1] = 0.0;
140 data[2] = 0.0;
141 data[3] = Sangle2;
142
143 }
144 }
145
146 /** Initializer by a rotation axis and an angle.
147 Initialize the quaternion to represent the rotation around a given
148 angle and an arbitrary axis.
149 @param angle The angle in radians
150 @param axis The rotation axis
151 */
FGQuaternion(double angle,const FGColumnVector3 & axis)152 FGQuaternion(double angle, const FGColumnVector3& axis)
153 : mCacheValid(false) {
154
155 double angle2 = 0.5 * angle;
156
157 double length = axis.Magnitude();
158 double Sangle2 = sin(angle2) / length;
159 double Cangle2 = cos(angle2);
160
161 data[0] = Cangle2;
162 data[1] = Sangle2 * axis(1);
163 data[2] = Sangle2 * axis(2);
164 data[3] = Sangle2 * axis(3);
165 }
166
167 /** Initializer by matrix.
168 Initialize the quaternion with the matrix representing a transform from one frame
169 to another using the standard aerospace sequence, Yaw-Pitch-Roll (3-2-1).
170 @param m the rotation matrix */
171 FGQuaternion(const FGMatrix33& m);
172
173 /// Destructor.
~FGQuaternion()174 ~FGQuaternion() {}
175
176 /** Quaternion derivative for given angular rates.
177 Computes the quaternion derivative which results from the given
178 angular velocities
179 @param PQR a constant reference to a rotation rate vector
180 @return the quaternion derivative
181 @see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
182 Equation 1.3-36. */
183 FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
184
185 /** Transformation matrix.
186 @return a reference to the transformation/rotation matrix
187 corresponding to this quaternion rotation. */
GetT(void)188 const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
189
190 /** Backward transformation matrix.
191 @return a reference to the inverse transformation/rotation matrix
192 corresponding to this quaternion rotation. */
GetTInv(void)193 const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
194
195 /** Retrieves the Euler angles.
196 @return a reference to the triad of Euler angles corresponding
197 to this quaternion rotation.
198 units radians */
GetEuler(void)199 const FGColumnVector3& GetEuler(void) const {
200 ComputeDerived();
201 return mEulerAngles;
202 }
203
204 /** Retrieves the Euler angles.
205 @param i the Euler angle index.
206 units radians.
207 @return a reference to the i-th euler angles corresponding
208 to this quaternion rotation.
209 */
GetEuler(int i)210 double GetEuler(int i) const {
211 ComputeDerived();
212 return mEulerAngles(i);
213 }
214
215 /** Retrieves the Euler angles.
216 @param i the Euler angle index.
217 @return a reference to the i-th euler angles corresponding
218 to this quaternion rotation.
219 units degrees */
GetEulerDeg(int i)220 double GetEulerDeg(int i) const {
221 ComputeDerived();
222 return radtodeg*mEulerAngles(i);
223 }
224
225 /** Retrieves the Euler angle vector.
226 @return an Euler angle column vector corresponding
227 to this quaternion rotation.
228 units degrees */
GetEulerDeg(void)229 FGColumnVector3 const GetEulerDeg(void) const {
230 ComputeDerived();
231 return radtodeg*mEulerAngles;
232 }
233
234 /** Retrieves sine of the given euler angle.
235 @return the sine of the Euler angle theta (pitch attitude) corresponding
236 to this quaternion rotation. */
GetSinEuler(int i)237 double GetSinEuler(int i) const {
238 ComputeDerived();
239 return mEulerSines(i);
240 }
241
242 /** Retrieves cosine of the given euler angle.
243 @return the sine of the Euler angle theta (pitch attitude) corresponding
244 to this quaternion rotation. */
GetCosEuler(int i)245 double GetCosEuler(int i) const {
246 ComputeDerived();
247 return mEulerCosines(i);
248 }
249
250 /** Read access the entries of the vector.
251
252 @param idx the component index.
253
254 Return the value of the matrix entry at the given index.
255 Indices are counted starting with 1.
256
257 Note that the index given in the argument is unchecked.
258 */
operator()259 double operator()(unsigned int idx) const { return data[idx-1]; }
260
261 /** Write access the entries of the vector.
262
263 @param idx the component index.
264
265 Return a reference to the vector entry at the given index.
266 Indices are counted starting with 1.
267
268 Note that the index given in the argument is unchecked.
269 */
operator()270 double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
271
272 /** Read access the entries of the vector.
273
274 @param idx the component index.
275
276 Return the value of the matrix entry at the given index.
277 Indices are counted starting with 1.
278
279 This function is just a shortcut for the <tt>double
280 operator()(unsigned int idx) const</tt> function. It is
281 used internally to access the elements in a more convenient way.
282
283 Note that the index given in the argument is unchecked.
284 */
Entry(unsigned int idx)285 double Entry(unsigned int idx) const { return data[idx-1]; }
286
287 /** Write access the entries of the vector.
288
289 @param idx the component index.
290
291 Return a reference to the vector entry at the given index.
292 Indices are counted starting with 1.
293
294 This function is just a shortcut for the <tt>double&
295 operator()(unsigned int idx)</tt> function. It is
296 used internally to access the elements in a more convenient way.
297
298 Note that the index given in the argument is unchecked.
299 */
Entry(unsigned int idx)300 double& Entry(unsigned int idx) {
301 mCacheValid = false;
302 return data[idx-1];
303 }
304
305 /** Assignment operator "=".
306 Assign the value of q to the current object. Cached values are
307 conserved.
308 @param q reference to an FGQuaternion instance
309 @return reference to a quaternion object */
310 const FGQuaternion& operator=(const FGQuaternion& q) {
311 // Copy the master values ...
312 data[0] = q.data[0];
313 data[1] = q.data[1];
314 data[2] = q.data[2];
315 data[3] = q.data[3];
316 ComputeDerived();
317 // .. and copy the derived values if they are valid
318 mCacheValid = q.mCacheValid;
319 if (mCacheValid) {
320 mT = q.mT;
321 mTInv = q.mTInv;
322 mEulerAngles = q.mEulerAngles;
323 mEulerSines = q.mEulerSines;
324 mEulerCosines = q.mEulerCosines;
325 }
326 return *this;
327 }
328
329 /// Conversion from Quat to Matrix
FGMatrix33()330 operator FGMatrix33() const { return GetT(); }
331
332 /** Comparison operator "==".
333 @param q a quaternion reference
334 @return true if both quaternions represent the same rotation. */
335 bool operator==(const FGQuaternion& q) const {
336 return data[0] == q.data[0] && data[1] == q.data[1]
337 && data[2] == q.data[2] && data[3] == q.data[3];
338 }
339
340 /** Comparison operator "!=".
341 @param q a quaternion reference
342 @return true if both quaternions do not represent the same rotation. */
343 bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
344 const FGQuaternion& operator+=(const FGQuaternion& q) {
345 // Copy the master values ...
346 data[0] += q.data[0];
347 data[1] += q.data[1];
348 data[2] += q.data[2];
349 data[3] += q.data[3];
350 mCacheValid = false;
351 return *this;
352 }
353
354 /** Arithmetic operator "-=".
355 @param q a quaternion reference.
356 @return a quaternion reference representing Q, where Q = Q - q. */
357 const FGQuaternion& operator-=(const FGQuaternion& q) {
358 // Copy the master values ...
359 data[0] -= q.data[0];
360 data[1] -= q.data[1];
361 data[2] -= q.data[2];
362 data[3] -= q.data[3];
363 mCacheValid = false;
364 return *this;
365 }
366
367 /** Arithmetic operator "*=".
368 @param scalar a multiplicative value.
369 @return a quaternion reference representing Q, where Q = Q * scalar. */
370 const FGQuaternion& operator*=(double scalar) {
371 data[0] *= scalar;
372 data[1] *= scalar;
373 data[2] *= scalar;
374 data[3] *= scalar;
375 mCacheValid = false;
376 return *this;
377 }
378
379 /** Arithmetic operator "/=".
380 @param scalar a divisor value.
381 @return a quaternion reference representing Q, where Q = Q / scalar. */
382 const FGQuaternion& operator/=(double scalar) {
383 return operator*=(1.0/scalar);
384 }
385
386 /** Arithmetic operator "+".
387 @param q a quaternion to be summed.
388 @return a quaternion representing Q, where Q = Q + q. */
389 FGQuaternion operator+(const FGQuaternion& q) const {
390 return FGQuaternion(data[0]+q.data[0], data[1]+q.data[1],
391 data[2]+q.data[2], data[3]+q.data[3]);
392 }
393
394 /** Arithmetic operator "-".
395 @param q a quaternion to be subtracted.
396 @return a quaternion representing Q, where Q = Q - q. */
397 FGQuaternion operator-(const FGQuaternion& q) const {
398 return FGQuaternion(data[0]-q.data[0], data[1]-q.data[1],
399 data[2]-q.data[2], data[3]-q.data[3]);
400 }
401
402 /** Arithmetic operator "*".
403 Multiplication of two quaternions is like performing successive rotations.
404 @param q a quaternion to be multiplied.
405 @return a quaternion representing Q, where Q = Q * q. */
406 FGQuaternion operator*(const FGQuaternion& q) const {
407 return FGQuaternion(data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3],
408 data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2],
409 data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1],
410 data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0]);
411 }
412
413 /** Arithmetic operator "*=".
414 Multiplication of two quaternions is like performing successive rotations.
415 @param q a quaternion to be multiplied.
416 @return a quaternion reference representing Q, where Q = Q * q. */
417 const FGQuaternion& operator*=(const FGQuaternion& q) {
418 double q0 = data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3];
419 double q1 = data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2];
420 double q2 = data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1];
421 double q3 = data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0];
422 data[0] = q0;
423 data[1] = q1;
424 data[2] = q2;
425 data[3] = q3;
426 mCacheValid = false;
427 return *this;
428 }
429
430 /** Inverse of the quaternion.
431
432 Compute and return the inverse of the quaternion so that the orientation
433 represented with *this multiplied with the returned value is equal to
434 the identity orientation.
435 */
Inverse(void)436 FGQuaternion Inverse(void) const {
437 double norm = SqrMagnitude();
438 if (norm == 0.0)
439 return *this;
440 double rNorm = 1.0/norm;
441 return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
442 -data[2]*rNorm, -data[3]*rNorm );
443 }
444
445 /** Conjugate of the quaternion.
446
447 Compute and return the conjugate of the quaternion. This one is equal
448 to the inverse iff the quaternion is normalized.
449 */
Conjugate(void)450 FGQuaternion Conjugate(void) const {
451 return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
452 }
453
454 friend FGQuaternion operator*(double, const FGQuaternion&);
455
456 /** Length of the vector.
457
458 Compute and return the euclidean norm of this vector.
459 */
Magnitude(void)460 double Magnitude(void) const { return sqrt(SqrMagnitude()); }
461
462 /** Square of the length of the vector.
463
464 Compute and return the square of the euclidean norm of this vector.
465 */
SqrMagnitude(void)466 double SqrMagnitude(void) const {
467 return data[0]*data[0] + data[1]*data[1]
468 + data[2]*data[2] + data[3]*data[3];
469 }
470
471 /** Normalize.
472
473 Normalize the vector to have the Magnitude() == 1.0. If the vector
474 is equal to zero it is left untouched.
475 */
476 void Normalize(void);
477
478 /** Zero quaternion vector. Does not represent any orientation.
479 Useful for initialization of increments */
zero(void)480 static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
481
482 std::string Dump(const std::string& delimiter) const;
483
484 friend FGQuaternion QExp(const FGColumnVector3& omega);
485
486 private:
487 /** Copying by assigning the vector valued components. */
FGQuaternion(double q1,double q2,double q3,double q4)488 FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
489 { data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }
490
491 /** Computation of derived values.
492 This function recomputes the derived values like euler angles and
493 transformation matrices. It does this unconditionally. */
494 void ComputeDerivedUnconditional(void) const;
495
496 /** Computation of derived values.
497 This function checks if the derived values like euler angles and
498 transformation matrices are already computed. If so, it
499 returns. If they need to be computed the real worker routine
500 FGQuaternion::ComputeDerivedUnconditional(void) const
501 is called. */
ComputeDerived(void)502 void ComputeDerived(void) const {
503 if (!mCacheValid)
504 ComputeDerivedUnconditional();
505 }
506
507 /** The quaternion values itself. This is the master copy. */
508 double data[4];
509
510 /** A data validity flag.
511 This class implements caching of the derived values like the
512 orthogonal rotation matrices or the Euler angles. For caching we
513 carry a flag which signals if the values are valid or not.
514 The C++ keyword "mutable" tells the compiler that the data member is
515 allowed to change during a const member function. */
516 mutable bool mCacheValid;
517
518 /** This stores the transformation matrices. */
519 mutable FGMatrix33 mT;
520 mutable FGMatrix33 mTInv;
521
522 /** The cached euler angles. */
523 mutable FGColumnVector3 mEulerAngles;
524
525 /** The cached sines and cosines of the euler angles. */
526 mutable FGColumnVector3 mEulerSines;
527 mutable FGColumnVector3 mEulerCosines;
528
529 void InitializeFromEulerAngles(double phi, double tht, double psi);
530 };
531
532 /** Scalar multiplication.
533
534 @param scalar scalar value to multiply with.
535 @param q Vector to multiply.
536
537 Multiply the Vector with a scalar value.
538 */
539 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
540 return FGQuaternion(scalar*q.data[0], scalar*q.data[1], scalar*q.data[2], scalar*q.data[3]);
541 }
542
543 /** Quaternion exponential
544 @param omega rotation velocity
545 Calculate the unit quaternion which is the result of the exponentiation of
546 the vector 'omega'.
547 */
QExp(const FGColumnVector3 & omega)548 inline FGQuaternion QExp(const FGColumnVector3& omega) {
549 FGQuaternion qexp;
550 double angle = omega.Magnitude();
551 double sina_a = angle > 0.0 ? sin(angle)/angle : 1.0;
552
553 qexp.data[0] = cos(angle);
554 qexp.data[1] = omega(1) * sina_a;
555 qexp.data[2] = omega(2) * sina_a;
556 qexp.data[3] = omega(3) * sina_a;
557
558 return qexp;
559 }
560
561 /** Write quaternion to a stream.
562 @param os Stream to write to.
563 @param q Quaternion to write.
564 Write the quaternion to a stream.*/
565 std::ostream& operator<<(std::ostream& os, const FGQuaternion& q);
566
567 } // namespace JSBSim
568 #endif
569