1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Tony Peden, Jon Berndt, Mathias Frolich
6 Purpose: FGMatrix33 class
9 ------------- Copyright (C) 1998 by the authors above -------------
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 Software
13 Foundation; either version 2 of the License, or (at your option) any later
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
21 You should have received a copy of the GNU Lesser General Public License along with
22 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 Place - Suite 330, Boston, MA 02111-1307, USA.
25 Further information about the GNU Lesser General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
32 --------------------------------------------------------------------------------
34 03/16/2000 JSB Added exception throwing
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40 #include "FGMatrix33.h"
41 #include "FGColumnVector3.h"
51 static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.10 2010/07/01 23:13:19 jberndt Exp $";
52 static const char *IdHdr = ID_MATRIX33;
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 FGMatrix33::FGMatrix33(void)
62 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
63 data[6] = data[7] = data[8] = 0.0;
68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
70 string FGMatrix33::Dump(const string& delimiter) const
73 buffer << setw(12) << setprecision(10) << data[0] << delimiter;
74 buffer << setw(12) << setprecision(10) << data[3] << delimiter;
75 buffer << setw(12) << setprecision(10) << data[6] << delimiter;
76 buffer << setw(12) << setprecision(10) << data[1] << delimiter;
77 buffer << setw(12) << setprecision(10) << data[4] << delimiter;
78 buffer << setw(12) << setprecision(10) << data[7] << delimiter;
79 buffer << setw(12) << setprecision(10) << data[2] << delimiter;
80 buffer << setw(12) << setprecision(10) << data[5] << delimiter;
81 buffer << setw(12) << setprecision(10) << data[8];
85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
91 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
92 buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
93 buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
95 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
96 buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
97 buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
99 buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
100 buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
101 buffer << right << fixed << setw(9) << setprecision(6) << data[8];
103 buffer << setw(0) << left;
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 FGQuaternion FGMatrix33::GetQuaternion(void)
117 tempQ[0] = 1.0 + data[0] + data[4] + data[8];
118 tempQ[1] = 1.0 + data[0] - data[4] - data[8];
119 tempQ[2] = 1.0 - data[0] + data[4] - data[8];
120 tempQ[3] = 1.0 - data[0] - data[4] + data[8];
122 // Find largest of the above
124 for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
128 Q(1) = 0.50*sqrt(tempQ[0]);
129 Q(2) = 0.25*(data[7] - data[5])/Q(1);
130 Q(3) = 0.25*(data[2] - data[6])/Q(1);
131 Q(4) = 0.25*(data[3] - data[1])/Q(1);
134 Q(2) = 0.50*sqrt(tempQ[1]);
135 Q(1) = 0.25*(data[7] - data[5])/Q(2);
136 Q(3) = 0.25*(data[3] + data[1])/Q(2);
137 Q(4) = 0.25*(data[2] + data[6])/Q(2);
140 Q(3) = 0.50*sqrt(tempQ[2]);
141 Q(1) = 0.25*(data[2] - data[6])/Q(3);
142 Q(2) = 0.25*(data[3] + data[1])/Q(3);
143 Q(4) = 0.25*(data[7] + data[5])/Q(3);
146 Q(4) = 0.50*sqrt(tempQ[3]);
147 Q(1) = 0.25*(data[3] - data[1])/Q(4);
148 Q(2) = 0.25*(data[6] + data[2])/Q(4);
149 Q(3) = 0.25*(data[7] + data[5])/Q(4);
159 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
161 ostream& operator<<(ostream& os, const FGMatrix33& M)
163 for (unsigned int i=1; i<=M.Rows(); i++) {
164 for (unsigned int j=1; j<=M.Cols(); j++) {
165 if (i == M.Rows() && j == M.Cols())
168 os << M(i,j) << ", ";
174 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
176 istream& operator>>(istream& is, FGMatrix33& M)
178 for (unsigned int i=1; i<=M.Rows(); i++) {
179 for (unsigned int j=1; j<=M.Cols(); j++) {
186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 double FGMatrix33::Determinant(void) const {
189 return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
190 + data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
191 - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
196 FGMatrix33 FGMatrix33::Inverse(void) const {
197 // Compute the inverse of a general matrix using Cramers rule.
198 // I guess googling for cramers rule gives tons of references
201 if (Determinant() != 0.0) {
202 double rdet = 1.0/Determinant();
204 double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
205 double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
206 double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
207 double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
208 double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
209 double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
210 double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
211 double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
212 double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
214 return FGMatrix33( i11, i12, i13,
218 return FGMatrix33( 0, 0, 0,
224 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
226 void FGMatrix33::InitMatrix(void)
228 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
229 data[6] = data[7] = data[8] = 0.0;
232 // *****************************************************************************
233 // binary operators ************************************************************
234 // *****************************************************************************
236 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
238 return FGMatrix33( data[0] - M.data[0],
246 data[8] - M.data[8] );
249 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
251 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
253 data[0] -= M.data[0];
254 data[1] -= M.data[1];
255 data[2] -= M.data[2];
256 data[3] -= M.data[3];
257 data[4] -= M.data[4];
258 data[5] -= M.data[5];
259 data[6] -= M.data[6];
260 data[7] -= M.data[7];
261 data[8] -= M.data[8];
266 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
268 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
270 return FGMatrix33( data[0] + M.data[0],
278 data[8] + M.data[8] );
281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
283 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
285 data[0] += M.data[0];
286 data[3] += M.data[3];
287 data[6] += M.data[6];
288 data[1] += M.data[1];
289 data[4] += M.data[4];
290 data[7] += M.data[7];
291 data[2] += M.data[2];
292 data[5] += M.data[5];
293 data[8] += M.data[8];
298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
300 FGMatrix33 FGMatrix33::operator*(const double scalar) const
302 return FGMatrix33( scalar * data[0],
313 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
315 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
317 return FGMatrix33( scalar * M(1,1),
328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330 FGMatrix33& FGMatrix33::operator*=(const double scalar)
345 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
347 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
351 Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
352 Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
353 Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
354 Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
355 Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
356 Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
357 Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
358 Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
359 Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
366 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
368 // FIXME: Make compiler friendlier
371 a = data[0]; b=data[3]; c=data[6];
372 data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
373 data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
374 data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
376 a = data[1]; b=data[4]; c=data[7];
377 data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
378 data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
379 data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
381 a = data[2]; b=data[5]; c=data[8];
382 data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
383 data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
384 data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
389 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
391 FGMatrix33 FGMatrix33::operator/(const double scalar) const
396 double tmp = 1.0/scalar;
397 Quot.data[0] = data[0] * tmp;
398 Quot.data[3] = data[3] * tmp;
399 Quot.data[6] = data[6] * tmp;
400 Quot.data[1] = data[1] * tmp;
401 Quot.data[4] = data[4] * tmp;
402 Quot.data[7] = data[7] * tmp;
403 Quot.data[2] = data[2] * tmp;
404 Quot.data[5] = data[5] * tmp;
405 Quot.data[8] = data[8] * tmp;
408 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
414 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
416 FGMatrix33& FGMatrix33::operator/=(const double scalar)
419 double tmp = 1.0/scalar;
431 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
437 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
439 void FGMatrix33::T(void)
456 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
458 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
464 double tmp1 = v1*data[0]; //[(col-1)*eRows+row-1]
465 double tmp2 = v1*data[1];
466 double tmp3 = v1*data[2];
476 return FGColumnVector3( tmp1, tmp2, tmp3 );
479 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
480 // The bitmasked value choices are as follows:
481 // unset: In this case (the default) JSBSim would only print
482 // out the normally expected messages, essentially echoing
483 // the config files as they are read. If the environment
484 // variable is not set, debug_lvl is set to 1 internally
485 // 0: This requests JSBSim not to output any messages
487 // 1: This value explicity requests the normal JSBSim
489 // 2: This value asks for a message to be printed out when
490 // a class is instantiated
491 // 4: When this value is set, a message is displayed when a
492 // FGModel object executes its Run() method
493 // 8: When this value is set, various runtime state variables
494 // are printed out periodically
495 // 16: When set various parameters are sanity checked and
496 // a message is printed out when they go out of bounds
498 void FGMatrix33::Debug(int from)
500 if (debug_lvl <= 0) return;
502 if (debug_lvl & 1) { // Standard console startup message output
503 if (from == 0) { // Constructor
507 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
508 if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
509 if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
511 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
513 if (debug_lvl & 8 ) { // Runtime state variables
515 if (debug_lvl & 16) { // Sanity checking
517 if (debug_lvl & 64) {
518 if (from == 0) { // Constructor
519 cout << IdSrc << endl;
520 cout << IdHdr << endl;