1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Jon Berndt, Mathis 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 MF Quaternion class from old FGColumnVector4
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
36 #ifndef FGQUATERNION_H
37 #define FGQUATERNION_H
39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 #include "FGJSBBase.h"
44 #include "FGMatrix33.h"
45 #include "FGColumnVector3.h"
46 #include "input_output/FGPropertyManager.h"
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 #define ID_QUATERNION "$Id$"
56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 /** Models the Quaternion representation of rotations.
61 FGQuaternion is a representation of an arbitrary rotation through a
62 quaternion. It has vector properties. This class also contains access
63 functions to the euler angle representation of rotations and access to
64 transformation matrices for 3D vectors. Transformations and euler angles are
65 therefore computed once they are requested for the first time. Then they are
66 cached for later usage as long as the class is not accessed trough
67 a nonconst member function.
69 Note: The order of rotations used in this class corresponds to a 3-2-1 sequence,
70 or Y-P-R, or Z-Y-X, if you prefer.
72 @see Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
73 Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
75 @see D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
77 @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
78 NASA-Ames", NASA CR-2497, January 1975
79 @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
80 Wiley & Sons, 1979 ISBN 0-471-03032-5
81 @see Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
82 1982 ISBN 0-471-08936-2
83 @author Mathias Froehlich, extended FGColumnVector4 originally by Tony Peden
87 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
89 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
94 /** Default initializer.
95 Default initializer, initializes the class with the identity rotation. */
96 FGQuaternion() : mCacheValid(false) {
98 Entry(2) = Entry(3) = Entry(4) = 0.0;
101 /** Copy constructor.
102 Copy constructor, initializes the quaternion.
103 @param q a constant reference to another FGQuaternion instance */
104 FGQuaternion(const FGQuaternion& q);
106 /** Initializer by euler angles.
107 Initialize the quaternion with the euler angles.
108 @param phi The euler X axis (roll) angle in radians
109 @param tht The euler Y axis (attitude) angle in radians
110 @param psi The euler Z axis (heading) angle in radians */
111 FGQuaternion(double phi, double tht, double psi);
113 /** Initializer by one euler angle.
114 Initialize the quaternion with the single euler angle where its index
115 is given in the first argument.
116 @param idx Index of the euler angle to initialize
117 @param angle The euler angle in radians */
118 FGQuaternion(int idx, double angle)
119 : mCacheValid(false) {
120 double angle2 = 0.5*angle;
122 double Sangle2 = sin(angle2);
123 double Cangle2 = cos(angle2);
131 } else if (idx == eTht) {
149 /** Quaternion derivative for given angular rates.
150 Computes the quaternion derivative which results from the given
152 @param PQR a constant reference to the body rate vector
153 @return the quaternion derivative
154 @see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
156 FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
158 /** Transformation matrix.
159 @return a reference to the transformation/rotation matrix
160 corresponding to this quaternion rotation. */
161 const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
163 /** Backward transformation matrix.
164 @return a reference to the inverse transformation/rotation matrix
165 corresponding to this quaternion rotation. */
166 const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
168 /** Retrieves the Euler angles.
169 @return a reference to the triad of euler angles corresponding
170 to this quaternion rotation.
172 const FGColumnVector3& GetEuler(void) const {
177 /** Retrieves the Euler angles.
178 @param i the euler angle index.
180 @return a reference to the i-th euler angles corresponding
181 to this quaternion rotation.
183 double GetEuler(int i) const {
185 return mEulerAngles(i);
188 /** Retrieves the Euler angles.
189 @param i the euler angle index.
190 @return a reference to the i-th euler angles corresponding
191 to this quaternion rotation.
193 double GetEulerDeg(int i) const {
195 return radtodeg*mEulerAngles(i);
198 /** Retrieves sine of the given euler angle.
199 @return the sine of the Euler angle theta (pitch attitude) corresponding
200 to this quaternion rotation. */
201 double GetSinEuler(int i) const {
203 return mEulerSines(i);
206 /** Retrieves cosine of the given euler angle.
207 @return the sine of the Euler angle theta (pitch attitude) corresponding
208 to this quaternion rotation. */
209 double GetCosEuler(int i) const {
211 return mEulerCosines(i);
214 /** Read access the entries of the vector.
216 @param idx the component index.
218 Return the value of the matrix entry at the given index.
219 Indices are counted starting with 1.
221 Note that the index given in the argument is unchecked.
223 double operator()(unsigned int idx) const { return Entry(idx); }
225 /** Write access the entries of the vector.
227 @param idx the component index.
229 Return a reference to the vector entry at the given index.
230 Indices are counted starting with 1.
232 Note that the index given in the argument is unchecked.
234 double& operator()(unsigned int idx) { return Entry(idx); }
236 /** Read access the entries of the vector.
238 @param idx the component index.
240 Return the value of the matrix entry at the given index.
241 Indices are counted starting with 1.
243 This function is just a shortcut for the <tt>double
244 operator()(unsigned int idx) const</tt> function. It is
245 used internally to access the elements in a more convenient way.
247 Note that the index given in the argument is unchecked.
249 double Entry(unsigned int idx) const { return mData[idx-1]; }
251 /** Write access the entries of the vector.
253 @param idx the component index.
255 Return a reference to the vector entry at the given index.
256 Indices are counted starting with 1.
258 This function is just a shortcut for the <tt>double&
259 operator()(unsigned int idx)</tt> function. It is
260 used internally to access the elements in a more convenient way.
262 Note that the index given in the argument is unchecked.
264 double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
266 /** Assignment operator "=".
267 Assign the value of q to the current object. Cached values are
269 @param q reference to an FGQuaternion instance
270 @return reference to a quaternion object */
271 const FGQuaternion& operator=(const FGQuaternion& q) {
272 // Copy the master values ...
277 // .. and copy the derived values if they are valid
278 mCacheValid = q.mCacheValid;
282 mEulerAngles = q.mEulerAngles;
283 mEulerSines = q.mEulerSines;
284 mEulerCosines = q.mEulerCosines;
289 /** Comparison operator "==".
290 @param q a quaternion reference
291 @return true if both quaternions represent the same rotation. */
292 bool operator==(const FGQuaternion& q) const {
293 return Entry(1) == q(1) && Entry(2) == q(2)
294 && Entry(3) == q(3) && Entry(4) == q(4);
297 /** Comparison operator "!=".
298 @param q a quaternion reference
299 @return true if both quaternions do not represent the same rotation. */
300 bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
301 const FGQuaternion& operator+=(const FGQuaternion& q) {
302 // Copy the master values ...
311 /** Arithmetic operator "-=".
312 @param q a quaternion reference.
313 @return a quaternion reference representing Q, where Q = Q - q. */
314 const FGQuaternion& operator-=(const FGQuaternion& q) {
315 // Copy the master values ...
324 /** Arithmetic operator "*=".
325 @param scalar a multiplicative value.
326 @return a quaternion reference representing Q, where Q = Q * scalar. */
327 const FGQuaternion& operator*=(double scalar) {
336 /** Arithmetic operator "/=".
337 @param scalar a divisor value.
338 @return a quaternion reference representing Q, where Q = Q / scalar. */
339 const FGQuaternion& operator/=(double scalar) {
340 return operator*=(1.0/scalar);
343 /** Arithmetic operator "+".
344 @param q a quaternion to be summed.
345 @return a quaternion representing Q, where Q = Q + q. */
346 FGQuaternion operator+(const FGQuaternion& q) const {
347 return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
348 Entry(3)+q(3), Entry(4)+q(4));
351 /** Arithmetic operator "-".
352 @param q a quaternion to be subtracted.
353 @return a quaternion representing Q, where Q = Q - q. */
354 FGQuaternion operator-(const FGQuaternion& q) const {
355 return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
356 Entry(3)-q(3), Entry(4)-q(4));
359 /** Arithmetic operator "*".
360 Multiplication of two quaternions is like performing successive rotations.
361 @param q a quaternion to be multiplied.
362 @return a quaternion representing Q, where Q = Q * q. */
363 FGQuaternion operator*(const FGQuaternion& q) const {
364 return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
365 Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
366 Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
367 Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
370 /** Arithmetic operator "*=".
371 Multiplication of two quaternions is like performing successive rotations.
372 @param q a quaternion to be multiplied.
373 @return a quaternion reference representing Q, where Q = Q * q. */
374 const FGQuaternion& operator*=(const FGQuaternion& q) {
375 double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
376 double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
377 double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
378 double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
387 /** Inverse of the quaternion.
389 Compute and return the inverse of the quaternion so that the orientation
390 represented with *this multiplied with the returned value is equal to
391 the identity orientation.
393 FGQuaternion Inverse(void) const {
394 double norm = Magnitude();
397 double rNorm = 1.0/norm;
398 return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
399 -Entry(3)*rNorm, -Entry(4)*rNorm );
402 /** Conjugate of the quaternion.
404 Compute and return the conjugate of the quaternion. This one is equal
405 to the inverse iff the quaternion is normalized.
407 FGQuaternion Conjugate(void) const {
408 return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
411 friend FGQuaternion operator*(double, const FGQuaternion&);
413 /** Length of the vector.
415 Compute and return the euclidean norm of this vector.
417 double Magnitude(void) const { return sqrt(SqrMagnitude()); }
419 /** Square of the length of the vector.
421 Compute and return the square of the euclidean norm of this vector.
423 double SqrMagnitude(void) const {
424 return Entry(1)*Entry(1)+Entry(2)*Entry(2)
425 +Entry(3)*Entry(3)+Entry(4)*Entry(4);
430 Normalize the vector to have the Magnitude() == 1.0. If the vector
431 is equal to zero it is left untouched.
433 void Normalize(void);
435 /** Zero quaternion vector. Does not represent any orientation.
436 Useful for initialization of increments */
437 static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); }
440 /** Copying by assigning the vector valued components. */
441 FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
442 { Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
444 /** Computation of derived values.
445 This function recomputes the derived values like euler angles and
446 transformation matrices. It does this unconditionally. */
447 void ComputeDerivedUnconditional(void) const;
449 /** Computation of derived values.
450 This function checks if the derived values like euler angles and
451 transformation matrices are already computed. If so, it
452 returns. If they need to be computed the real worker routine
453 \ref FGQuaternion::ComputeDerivedUnconditional(void) const
455 This function is inlined to avoid function calls in the fast path. */
456 void ComputeDerived(void) const {
458 ComputeDerivedUnconditional();
461 /** The quaternion values itself. This is the master copy. */
464 /** A data validity flag.
465 This class implements caching of the derived values like the
466 orthogonal rotation matrices or the Euler angles. For caching we
467 carry a flag which signals if the values are valid or not.
468 The C++ keyword "mutable" tells the compiler that the data member is
469 allowed to change during a const member function. */
470 mutable bool mCacheValid;
472 /** This stores the transformation matrices. */
473 mutable FGMatrix33 mT;
474 mutable FGMatrix33 mTInv;
476 /** The cached euler angles. */
477 mutable FGColumnVector3 mEulerAngles;
479 /** The cached sines and cosines of the euler angles. */
480 mutable FGColumnVector3 mEulerSines;
481 mutable FGColumnVector3 mEulerCosines;
484 /** Scalar multiplication.
486 @param scalar scalar value to multiply with.
487 @param q Vector to multiply.
489 Multiply the Vector with a scalar value.
491 inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
492 return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4));
495 } // namespace JSBSim