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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
48 #include "FGMatrix33.h"
49 #include "FGColumnVector3.h"
51 #include "FGQuaternion.h"
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
59 static const char *IdSrc = "$Id$";
60 static const char *IdHdr = ID_QUATERNION;
62 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 FGQuaternion::FGQuaternion(const FGQuaternion& q)
66 : mCacheValid(q.mCacheValid) {
74 mEulerAngles = q.mEulerAngles;
75 mEulerSines = q.mEulerSines;
76 mEulerCosines = q.mEulerCosines;
80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82 // Initialize with the three euler angles
83 FGQuaternion::FGQuaternion(double phi, double tht, double psi)
84 : mCacheValid(false) {
85 double thtd2 = 0.5*tht;
86 double psid2 = 0.5*psi;
87 double phid2 = 0.5*phi;
89 double Sthtd2 = sin(thtd2);
90 double Spsid2 = sin(psid2);
91 double Sphid2 = sin(phid2);
93 double Cthtd2 = cos(thtd2);
94 double Cpsid2 = cos(psid2);
95 double Cphid2 = cos(phid2);
97 double Cphid2Cthtd2 = Cphid2*Cthtd2;
98 double Cphid2Sthtd2 = Cphid2*Sthtd2;
99 double Sphid2Sthtd2 = Sphid2*Sthtd2;
100 double Sphid2Cthtd2 = Sphid2*Cthtd2;
102 Entry(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
103 Entry(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
104 Entry(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
105 Entry(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 /** Returns the derivative of the quaternion corresponding to the
111 angular velocities PQR.
112 See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
115 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
116 double norm = Magnitude();
118 return FGQuaternion::zero();
119 double rnorm = 1.0/norm;
122 QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
123 QDot(2) = 0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
124 QDot(3) = 0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
125 QDot(4) = 0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131 void FGQuaternion::Normalize()
133 // Note: this does not touch the cache
134 // since it does not change the orientation ...
136 double norm = Magnitude();
140 double rnorm = 1.0/norm;
147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
149 // Compute the derived values if required ...
150 void FGQuaternion::ComputeDerivedUnconditional(void) const
154 // First normalize the 4-vector
155 double norm = Magnitude();
159 double rnorm = 1.0/norm;
160 double q1 = rnorm*Entry(1);
161 double q2 = rnorm*Entry(2);
162 double q3 = rnorm*Entry(3);
163 double q4 = rnorm*Entry(4);
165 // Now compute the transformation matrix.
177 mT(1,1) = q1q1 + q2q2 - q3q3 - q4q4;
178 mT(1,2) = 2.0*(q2q3 + q1q4);
179 mT(1,3) = 2.0*(q2q4 - q1q3);
180 mT(2,1) = 2.0*(q2q3 - q1q4);
181 mT(2,2) = q1q1 - q2q2 + q3q3 - q4q4;
182 mT(2,3) = 2.0*(q3q4 + q1q2);
183 mT(3,1) = 2.0*(q2q4 + q1q3);
184 mT(3,2) = 2.0*(q3q4 - q1q2);
185 mT(3,3) = q1q1 - q2q2 - q3q3 + q4q4;
186 // Since this is an orthogonal matrix, the inverse is simply
191 // Compute the Euler-angles
193 mEulerAngles(ePhi) = 0.5*M_PI;
195 mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
198 mEulerAngles(eTht) = 0.5*M_PI;
199 else if (1.0 < mT(1,3))
200 mEulerAngles(eTht) = -0.5*M_PI;
202 mEulerAngles(eTht) = asin(-mT(1,3));
205 mEulerAngles(ePsi) = 0.5*M_PI;
207 double psi = atan2(mT(1,2), mT(1,1));
210 mEulerAngles(ePsi) = psi;
213 // FIXME: may be one can compute those values easier ???
214 mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
215 // mEulerSines(eTht) = sin(mEulerAngles(eTht));
216 mEulerSines(eTht) = -mT(1,3);
217 mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
218 mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
219 mEulerCosines(eTht) = cos(mEulerAngles(eTht));
220 mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
223 } // namespace JSBSim