1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGMatrix33.cpp
4 Author: Tony Peden, Jon Berndt, Mathias Frolich
5 Date started: 1998
6 Purpose: FGMatrix33 class
7 Called by: Various
8
9 ------------- Copyright (C) 1998 by the authors above -------------
10
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU Lesser General Public License as published by the Free
13 Software Foundation; either version 2 of the License, or (at your option) any
14 later version.
15
16 This program is distributed in the hope that it will be useful, but WITHOUT
17 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 details.
20
21 You should have received a copy of the GNU Lesser General Public License along
22 with this program; if not, write to the Free Software Foundation, Inc., 59
23 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25 Further information about the GNU Lesser General Public License can also be
26 found on the world wide web at http://www.gnu.org.
27
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30
31 HISTORY
32 --------------------------------------------------------------------------------
33 ??/??/?? TP Created
34 03/16/2000 JSB Added exception throwing
35
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 INCLUDES
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39
40 #include "FGMatrix33.h"
41 #include "FGColumnVector3.h"
42 #include "FGQuaternion.h"
43 #include <sstream>
44 #include <iomanip>
45
46 #include <iostream>
47
48 using namespace std;
49
50 namespace JSBSim {
51
52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 CLASS IMPLEMENTATION
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55
56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
57
FGMatrix33(void)58 FGMatrix33::FGMatrix33(void)
59 {
60 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
61 data[6] = data[7] = data[8] = 0.0;
62 }
63
64 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65
Dump(const string & delimiter) const66 string FGMatrix33::Dump(const string& delimiter) const
67 {
68 ostringstream buffer;
69 buffer << setw(12) << setprecision(10) << data[0] << delimiter;
70 buffer << setw(12) << setprecision(10) << data[3] << delimiter;
71 buffer << setw(12) << setprecision(10) << data[6] << delimiter;
72 buffer << setw(12) << setprecision(10) << data[1] << delimiter;
73 buffer << setw(12) << setprecision(10) << data[4] << delimiter;
74 buffer << setw(12) << setprecision(10) << data[7] << delimiter;
75 buffer << setw(12) << setprecision(10) << data[2] << delimiter;
76 buffer << setw(12) << setprecision(10) << data[5] << delimiter;
77 buffer << setw(12) << setprecision(10) << data[8];
78 return buffer.str();
79 }
80
81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82
Dump(const string & delimiter,const string & prefix) const83 string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
84 {
85 ostringstream buffer;
86
87 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
88 buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
89 buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
90
91 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
92 buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
93 buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
94
95 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
96 buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
97 buffer << right << fixed << setw(9) << setprecision(6) << data[8];
98
99 buffer << setw(0) << left;
100
101 return buffer.str();
102 }
103
104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
105
GetQuaternion(void) const106 FGQuaternion FGMatrix33::GetQuaternion(void) const
107 {
108 FGQuaternion Q;
109
110 double tempQ[4];
111 int idx;
112
113 tempQ[0] = 1.0 + data[0] + data[4] + data[8];
114 tempQ[1] = 1.0 + data[0] - data[4] - data[8];
115 tempQ[2] = 1.0 - data[0] + data[4] - data[8];
116 tempQ[3] = 1.0 - data[0] - data[4] + data[8];
117
118 // Find largest of the above
119 idx = 0;
120 for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
121
122 switch(idx) {
123 case 0:
124 Q(1) = 0.50*sqrt(tempQ[0]);
125 Q(2) = 0.25*(data[7] - data[5])/Q(1);
126 Q(3) = 0.25*(data[2] - data[6])/Q(1);
127 Q(4) = 0.25*(data[3] - data[1])/Q(1);
128 break;
129 case 1:
130 Q(2) = 0.50*sqrt(tempQ[1]);
131 Q(1) = 0.25*(data[7] - data[5])/Q(2);
132 Q(3) = 0.25*(data[3] + data[1])/Q(2);
133 Q(4) = 0.25*(data[2] + data[6])/Q(2);
134 break;
135 case 2:
136 Q(3) = 0.50*sqrt(tempQ[2]);
137 Q(1) = 0.25*(data[2] - data[6])/Q(3);
138 Q(2) = 0.25*(data[3] + data[1])/Q(3);
139 Q(4) = 0.25*(data[7] + data[5])/Q(3);
140 break;
141 case 3:
142 Q(4) = 0.50*sqrt(tempQ[3]);
143 Q(1) = 0.25*(data[3] - data[1])/Q(4);
144 Q(2) = 0.25*(data[6] + data[2])/Q(4);
145 Q(3) = 0.25*(data[7] + data[5])/Q(4);
146 break;
147 default:
148 //error
149 break;
150 }
151
152 return (Q);
153 }
154
155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
156 // Compute the Euler-angles
157 // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
158
GetEuler(void) const159 FGColumnVector3 FGMatrix33::GetEuler(void) const
160 {
161 FGColumnVector3 mEulerAngles;
162 bool GimbalLock = false;
163
164 if (data[6] <= -1.0) {
165 mEulerAngles(2) = 0.5*M_PI;
166 GimbalLock = true;
167 }
168 else if (1.0 <= data[6]) {
169 mEulerAngles(2) = -0.5*M_PI;
170 GimbalLock = true;
171 }
172 else
173 mEulerAngles(2) = asin(-data[6]);
174
175 if (GimbalLock)
176 mEulerAngles(1) = atan2(-data[5], data[4]);
177 else
178 mEulerAngles(1) = atan2(data[7], data[8]);
179
180 if (GimbalLock)
181 mEulerAngles(3) = 0.0;
182 else {
183 double psi = atan2(data[3], data[0]);
184 if (psi < 0.0)
185 psi += 2*M_PI;
186 mEulerAngles(3) = psi;
187 }
188
189 return mEulerAngles;
190 }
191
192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
193
operator <<(ostream & os,const FGMatrix33 & M)194 ostream& operator<<(ostream& os, const FGMatrix33& M)
195 {
196 for (unsigned int i=1; i<=M.Rows(); i++) {
197 for (unsigned int j=1; j<=M.Cols(); j++) {
198 if (i == M.Rows() && j == M.Cols())
199 os << M(i,j);
200 else
201 os << M(i,j) << ", ";
202 }
203 }
204 return os;
205 }
206
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
208
operator >>(istream & is,FGMatrix33 & M)209 istream& operator>>(istream& is, FGMatrix33& M)
210 {
211 for (unsigned int i=1; i<=M.Rows(); i++) {
212 for (unsigned int j=1; j<=M.Cols(); j++) {
213 is >> M(i,j);
214 }
215 }
216 return is;
217 }
218
219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
220
Determinant(void) const221 double FGMatrix33::Determinant(void) const {
222 return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
223 + data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
224 - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
225 }
226
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
228
Inverse(void) const229 FGMatrix33 FGMatrix33::Inverse(void) const {
230 // Compute the inverse of a general matrix using Cramers rule.
231 // I guess googling for cramers rule gives tons of references
232 // for this. :)
233
234 if (Determinant() != 0.0) {
235 double rdet = 1.0/Determinant();
236
237 double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
238 double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
239 double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
240 double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
241 double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
242 double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
243 double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
244 double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
245 double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
246
247 return FGMatrix33( i11, i12, i13,
248 i21, i22, i23,
249 i31, i32, i33 );
250 } else {
251 return FGMatrix33( 0, 0, 0,
252 0, 0, 0,
253 0, 0, 0 );
254 }
255 }
256
257 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
258
InitMatrix(void)259 void FGMatrix33::InitMatrix(void)
260 {
261 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
262 data[6] = data[7] = data[8] = 0.0;
263 }
264
265 // *****************************************************************************
266 // binary operators ************************************************************
267 // *****************************************************************************
268
operator -(const FGMatrix33 & M) const269 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
270 {
271 return FGMatrix33( data[0] - M.data[0],
272 data[3] - M.data[3],
273 data[6] - M.data[6],
274 data[1] - M.data[1],
275 data[4] - M.data[4],
276 data[7] - M.data[7],
277 data[2] - M.data[2],
278 data[5] - M.data[5],
279 data[8] - M.data[8] );
280 }
281
282 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
283
operator -=(const FGMatrix33 & M)284 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
285 {
286 data[0] -= M.data[0];
287 data[1] -= M.data[1];
288 data[2] -= M.data[2];
289 data[3] -= M.data[3];
290 data[4] -= M.data[4];
291 data[5] -= M.data[5];
292 data[6] -= M.data[6];
293 data[7] -= M.data[7];
294 data[8] -= M.data[8];
295
296 return *this;
297 }
298
299 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
300
operator +(const FGMatrix33 & M) const301 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
302 {
303 return FGMatrix33( data[0] + M.data[0],
304 data[3] + M.data[3],
305 data[6] + M.data[6],
306 data[1] + M.data[1],
307 data[4] + M.data[4],
308 data[7] + M.data[7],
309 data[2] + M.data[2],
310 data[5] + M.data[5],
311 data[8] + M.data[8] );
312 }
313
314 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
315
operator +=(const FGMatrix33 & M)316 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
317 {
318 data[0] += M.data[0];
319 data[3] += M.data[3];
320 data[6] += M.data[6];
321 data[1] += M.data[1];
322 data[4] += M.data[4];
323 data[7] += M.data[7];
324 data[2] += M.data[2];
325 data[5] += M.data[5];
326 data[8] += M.data[8];
327
328 return *this;
329 }
330
331 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
332
operator *(const double scalar) const333 FGMatrix33 FGMatrix33::operator*(const double scalar) const
334 {
335 return FGMatrix33( scalar * data[0],
336 scalar * data[3],
337 scalar * data[6],
338 scalar * data[1],
339 scalar * data[4],
340 scalar * data[7],
341 scalar * data[2],
342 scalar * data[5],
343 scalar * data[8] );
344 }
345
346 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
347 /*
348 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
349 {
350 return FGMatrix33( scalar * M(1,1),
351 scalar * M(1,2),
352 scalar * M(1,3),
353 scalar * M(2,1),
354 scalar * M(2,2),
355 scalar * M(2,3),
356 scalar * M(3,1),
357 scalar * M(3,2),
358 scalar * M(3,3) );
359 }
360 */
361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
362
operator *=(const double scalar)363 FGMatrix33& FGMatrix33::operator*=(const double scalar)
364 {
365 data[0] *= scalar;
366 data[3] *= scalar;
367 data[6] *= scalar;
368 data[1] *= scalar;
369 data[4] *= scalar;
370 data[7] *= scalar;
371 data[2] *= scalar;
372 data[5] *= scalar;
373 data[8] *= scalar;
374
375 return *this;
376 }
377
378 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
379
operator *(const FGMatrix33 & M) const380 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
381 {
382 FGMatrix33 Product;
383
384 Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
385 Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
386 Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
387 Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
388 Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
389 Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
390 Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
391 Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
392 Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
393
394 return Product;
395 }
396
397 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
398
operator *=(const FGMatrix33 & M)399 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
400 {
401 // FIXME: Make compiler friendlier
402 double a,b,c;
403
404 a = data[0]; b=data[3]; c=data[6];
405 data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
406 data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
407 data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
408
409 a = data[1]; b=data[4]; c=data[7];
410 data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
411 data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
412 data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
413
414 a = data[2]; b=data[5]; c=data[8];
415 data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
416 data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
417 data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
418
419 return *this;
420 }
421
422 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
423
operator /(const double scalar) const424 FGMatrix33 FGMatrix33::operator/(const double scalar) const
425 {
426 FGMatrix33 Quot;
427
428 if ( scalar != 0 ) {
429 double tmp = 1.0/scalar;
430 Quot.data[0] = data[0] * tmp;
431 Quot.data[3] = data[3] * tmp;
432 Quot.data[6] = data[6] * tmp;
433 Quot.data[1] = data[1] * tmp;
434 Quot.data[4] = data[4] * tmp;
435 Quot.data[7] = data[7] * tmp;
436 Quot.data[2] = data[2] * tmp;
437 Quot.data[5] = data[5] * tmp;
438 Quot.data[8] = data[8] * tmp;
439 } else {
440 throw MatrixException{"Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)"};
441 }
442 return Quot;
443 }
444
445 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
446
operator /=(const double scalar)447 FGMatrix33& FGMatrix33::operator/=(const double scalar)
448 {
449 if ( scalar != 0 ) {
450 double tmp = 1.0/scalar;
451 data[0] *= tmp;
452 data[3] *= tmp;
453 data[6] *= tmp;
454 data[1] *= tmp;
455 data[4] *= tmp;
456 data[7] *= tmp;
457 data[2] *= tmp;
458 data[5] *= tmp;
459 data[8] *= tmp;
460 } else {
461 throw MatrixException{"Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)"};
462 }
463 return *this;
464 }
465
466 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
467
T(void)468 void FGMatrix33::T(void)
469 {
470 double tmp;
471
472 tmp = data[3];
473 data[3] = data[1];
474 data[1] = tmp;
475
476 tmp = data[6];
477 data[6] = data[2];
478 data[2] = tmp;
479
480 tmp = data[7];
481 data[7] = data[5];
482 data[5] = tmp;
483 }
484
485 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
486
operator *(const FGColumnVector3 & v) const487 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
488 {
489 double v1 = v(1);
490 double v2 = v(2);
491 double v3 = v(3);
492
493 double tmp1 = v1*data[0]; //[(col-1)*eRows+row-1]
494 double tmp2 = v1*data[1];
495 double tmp3 = v1*data[2];
496
497 tmp1 += v2*data[3];
498 tmp2 += v2*data[4];
499 tmp3 += v2*data[5];
500
501 tmp1 += v3*data[6];
502 tmp2 += v3*data[7];
503 tmp3 += v3*data[8];
504
505 return FGColumnVector3( tmp1, tmp2, tmp3 );
506 }
507
508 }
509