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.20 2011/10/31 14:54:40 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) const
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));
259 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
261 std::ostream& operator<<(std::ostream& os, const FGQuaternion& q)
263 os << q(1) << " , " << q(2) << " , " << q(3) << " , " << q(4);
267 } // namespace JSBSim