← Back to branch summary

~ubuntu-branches/ubuntu/precise/flightgear/precise

« back to all changes in this revision

Viewing changes to src/FDM/JSBSim/math/FGQuaternion.cpp

  • Committer: Package Import Robot
  • Author(s): Ove Kaaven
  • Date: 2011-09-03 22:16:12 UTC
  • mfrom: (3.1.9 sid)
  • Revision ID: package-import@ubuntu.com-20110903221612-2cjy0z7ztj5nkln5
Tags: 2.4.0-1
http://bugs.debian.org/638588
* New upstream release. Closes: #638588.
* Build-Depend on OpenSceneGraph 3.0, and the Subversion library.
* Recommend fgfs-scenery-base.
* Enable parallel builds (shorter compile times on multicore CPUs).
* Removed hack that tried to build without optimizations if
  building with optimizations fails.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/FDM/JSBSim/math/FGQuaternion.cpp
40
40
 
41
41
#include <string>
42
42
#include <iostream>
 
43
#include <iomanip>
43
44
#include <cmath>
44
45
using std::cerr;
45
46
using std::cout;
56
57
 
57
58
namespace JSBSim {
58
59
  
59
 
static const char *IdSrc = "$Id$";
 
60
static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.19 2010/12/07 12:57:14 jberndt Exp $";
60
61
static const char *IdHdr = ID_QUATERNION;
61
62
 
62
63
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
63
64
 
64
65
// Initialize from q
65
 
FGQuaternion::FGQuaternion(const FGQuaternion& q)
66
 
  : mCacheValid(q.mCacheValid) {
67
 
  Entry(1) = q(1);
68
 
  Entry(2) = q(2);
69
 
  Entry(3) = q(3);
70
 
  Entry(4) = q(4);
 
66
FGQuaternion::FGQuaternion(const FGQuaternion& q) : mCacheValid(q.mCacheValid)
 
67
{
 
68
  data[0] = q(1);
 
69
  data[1] = q(2);
 
70
  data[2] = q(3);
 
71
  data[3] = q(4);
71
72
  if (mCacheValid) {
72
73
    mT = q.mT;
73
74
    mTInv = q.mTInv;
80
81
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81
82
 
82
83
// Initialize with the three euler angles
83
 
FGQuaternion::FGQuaternion(double phi, double tht, double psi)
84
 
  : mCacheValid(false) {
 
84
FGQuaternion::FGQuaternion(double phi, double tht, double psi): mCacheValid(false)
 
85
{
 
86
  InitializeFromEulerAngles(phi, tht, psi);
 
87
}
 
88
 
 
89
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
90
 
 
91
FGQuaternion::FGQuaternion(FGColumnVector3 vOrient): mCacheValid(false)
 
92
{
 
93
  double phi = vOrient(ePhi);
 
94
  double tht = vOrient(eTht);
 
95
  double psi = vOrient(ePsi);
 
96
 
 
97
  InitializeFromEulerAngles(phi, tht, psi);
 
98
}
 
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
void FGQuaternion::InitializeFromEulerAngles(double phi, double tht, double psi)
 
109
{
 
110
  mEulerAngles(ePhi) = phi;
 
111
  mEulerAngles(eTht) = tht;
 
112
  mEulerAngles(ePsi) = psi;
 
113
 
85
114
  double thtd2 = 0.5*tht;
86
115
  double psid2 = 0.5*psi;
87
116
  double phid2 = 0.5*phi;
99
128
  double Sphid2Sthtd2 = Sphid2*Sthtd2;
100
129
  double Sphid2Cthtd2 = Sphid2*Cthtd2;
101
130
  
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;
 
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;
 
135
 
 
136
  Normalize();
 
137
}
 
138
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
139
// Initialize with a direction cosine (rotation) matrix
 
140
 
 
141
FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
 
142
{
 
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));
 
148
 
 
149
  Normalize();
106
150
}
107
151
 
108
152
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
111
155
    angular velocities PQR.
112
156
    See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
113
157
    Equation 1.3-36. 
 
158
    Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
114
159
*/
115
 
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
116
 
  double norm = Magnitude();
117
 
  if (norm == 0.0)
118
 
    return FGQuaternion::zero();
119
 
  double rnorm = 1.0/norm;
120
 
 
121
 
  FGQuaternion QDot;
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));
126
 
  return rnorm*QDot;
 
160
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
 
161
{
 
162
  return FGQuaternion(
 
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))
 
167
  );
127
168
}
128
169
 
129
170
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130
171
 
131
172
void FGQuaternion::Normalize()
132
173
{
133
 
  // Note: this does not touch the cache
134
 
  // since it does not change the orientation ...
135
 
  
 
174
  // Note: this does not touch the cache since it does not change the orientation
136
175
  double norm = Magnitude();
137
 
  if (norm == 0.0)
138
 
    return;
139
 
  
 
176
  if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
 
177
 
140
178
  double rnorm = 1.0/norm;
141
 
  Entry(1) *= rnorm;
142
 
  Entry(2) *= rnorm;
143
 
  Entry(3) *= rnorm;
144
 
  Entry(4) *= rnorm;
 
179
 
 
180
  data[0] *= rnorm;
 
181
  data[1] *= rnorm;
 
182
  data[2] *= rnorm;
 
183
  data[3] *= rnorm;
145
184
}
146
185
 
147
186
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
150
189
void FGQuaternion::ComputeDerivedUnconditional(void) const
151
190
{
152
191
  mCacheValid = true;
153
 
  
154
 
  // First normalize the 4-vector
155
 
  double norm = Magnitude();
156
 
  if (norm == 0.0)
157
 
    return;
158
192
 
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);
 
193
  double q0 = data[0]; // use some aliases/shorthand for the quat elements.
 
194
  double q1 = data[1];
 
195
  double q2 = data[2];
 
196
  double q3 = data[3];
164
197
 
165
198
  // Now compute the transformation matrix.
 
199
  double q0q0 = q0*q0;
166
200
  double q1q1 = q1*q1;
167
201
  double q2q2 = q2*q2;
168
202
  double q3q3 = q3*q3;
169
 
  double q4q4 = q4*q4;
 
203
  double q0q1 = q0*q1;
 
204
  double q0q2 = q0*q2;
 
205
  double q0q3 = q0*q3;
170
206
  double q1q2 = q1*q2;
171
207
  double q1q3 = q1*q3;
172
 
  double q1q4 = q1*q4;
173
208
  double q2q3 = q2*q3;
174
 
  double q2q4 = q2*q4;
175
 
  double q3q4 = q3*q4;
176
209
  
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
187
 
  // the transpose.
 
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;
 
219
 
 
220
  // Since this is an orthogonal matrix, the inverse is simply the transpose.
 
221
 
188
222
  mTInv = mT;
189
223
  mTInv.T();
190
224
  
191
225
  // Compute the Euler-angles
 
226
  // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
 
227
 
192
228
  if (mT(3,3) == 0.0)
193
229
    mEulerAngles(ePhi) = 0.5*M_PI;
194
230
  else
220
256
  mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
221
257
}
222
258
 
 
259
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
260
 
 
261
std::ostream& operator<<(std::ostream& os, const FGQuaternion& q)
 
262
{
 
263
  os << q(1) << " , " << q(2) << " , " << q(3) << " , " << q(4);
 
264
  return os;
 
265
}
 
266
 
223
267
} // namespace JSBSim