1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Tony Peden, Jon Berndt, Mathias Frolich
6 Purpose: FGMatrix33 class
10 --------------------------------------------------------------------------------
13 --------------------------------------------------------------------------------
15 03/16/2000 JSB Added exception throwing
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
21 #include "FGMatrix33.h"
22 #include "FGColumnVector3.h"
26 static const char *IdSrc = "$Id$";
27 static const char *IdHdr = ID_MATRIX33;
29 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
31 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
33 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 FGMatrix33::FGMatrix33(void)
37 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
38 data[6] = data[7] = data[8] = 0.0;
43 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
45 ostream& operator<<(ostream& os, const FGMatrix33& M)
47 for (unsigned int i=1; i<=M.Rows(); i++) {
48 for (unsigned int j=1; j<=M.Cols(); j++) {
49 if (i == M.Rows() && j == M.Cols())
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 istream& operator>>(istream& is, FGMatrix33& M)
62 for (unsigned int i=1; i<=M.Rows(); i++) {
63 for (unsigned int j=1; j<=M.Cols(); j++) {
70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72 double FGMatrix33::Determinant(void) const {
73 return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
74 + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
75 - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
78 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80 FGMatrix33 FGMatrix33::Inverse(void) const {
81 // Compute the inverse of a general matrix using Cramers rule.
82 // I guess googling for cramers rule gives tons of references
84 double rdet = 1.0/Determinant();
86 double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
87 double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
88 double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
89 double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
90 double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
91 double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
92 double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
93 double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
94 double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
96 return FGMatrix33( i11, i12, i13,
101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 void FGMatrix33::InitMatrix(void)
105 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
106 data[6] = data[7] = data[8] = 0.0;
109 // *****************************************************************************
110 // binary operators ************************************************************
111 // *****************************************************************************
113 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
115 return FGMatrix33( Entry(1,1) - M(1,1),
123 Entry(3,3) - M(3,3) );
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
130 data[0] -= M.data[0];
131 data[1] -= M.data[1];
132 data[2] -= M.data[2];
133 data[3] -= M.data[3];
134 data[4] -= M.data[4];
135 data[5] -= M.data[5];
136 data[6] -= M.data[6];
137 data[7] -= M.data[7];
138 data[8] -= M.data[8];
143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
147 return FGMatrix33( Entry(1,1) + M(1,1),
155 Entry(3,3) + M(3,3) );
158 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
160 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
162 Entry(1,1) += M(1,1);
163 Entry(1,2) += M(1,2);
164 Entry(1,3) += M(1,3);
165 Entry(2,1) += M(2,1);
166 Entry(2,2) += M(2,2);
167 Entry(2,3) += M(2,3);
168 Entry(3,1) += M(3,1);
169 Entry(3,2) += M(3,2);
170 Entry(3,3) += M(3,3);
175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
177 FGMatrix33 FGMatrix33::operator*(const double scalar) const
179 return FGMatrix33( scalar * Entry(1,1),
187 scalar * Entry(3,3) );
190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
192 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
194 return FGMatrix33( scalar * M(1,1),
205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
207 FGMatrix33& FGMatrix33::operator*=(const double scalar)
209 Entry(1,1) *= scalar;
210 Entry(1,2) *= scalar;
211 Entry(1,3) *= scalar;
212 Entry(2,1) *= scalar;
213 Entry(2,2) *= scalar;
214 Entry(2,3) *= scalar;
215 Entry(3,1) *= scalar;
216 Entry(3,2) *= scalar;
217 Entry(3,3) *= scalar;
222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
224 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
226 // FIXME: Make compiler friendlier
229 Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
230 Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
231 Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
232 Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
233 Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
234 Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
235 Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
236 Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
237 Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
242 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
244 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
246 // FIXME: Make compiler friendlier
249 a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
250 Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
251 Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
252 Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
254 a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
255 Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
256 Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
257 Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
259 a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
260 Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
261 Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
262 Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
269 FGMatrix33 FGMatrix33::operator/(const double scalar) const
274 double tmp = 1.0/scalar;
275 Quot(1,1) = Entry(1,1) * tmp;
276 Quot(1,2) = Entry(1,2) * tmp;
277 Quot(1,3) = Entry(1,3) * tmp;
278 Quot(2,1) = Entry(2,1) * tmp;
279 Quot(2,2) = Entry(2,2) * tmp;
280 Quot(2,3) = Entry(2,3) * tmp;
281 Quot(3,1) = Entry(3,1) * tmp;
282 Quot(3,2) = Entry(3,2) * tmp;
283 Quot(3,3) = Entry(3,3) * tmp;
286 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
294 FGMatrix33& FGMatrix33::operator/=(const double scalar)
297 double tmp = 1.0/scalar;
309 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
317 void FGMatrix33::T(void)
319 for (unsigned int i=1; i<=3; i++) {
320 for (unsigned int j=i+1; j<=3; j++) {
321 double tmp = Entry(i,j);
322 Entry(i,j) = Entry(j,i);
328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
331 double tmp1 = v(1)*Entry(1,1);
332 double tmp2 = v(1)*Entry(2,1);
333 double tmp3 = v(1)*Entry(3,1);
335 tmp1 += v(2)*Entry(1,2);
336 tmp2 += v(2)*Entry(2,2);
337 tmp3 += v(2)*Entry(3,2);
339 tmp1 += v(3)*Entry(1,3);
340 tmp2 += v(3)*Entry(2,3);
341 tmp3 += v(3)*Entry(3,3);
343 return FGColumnVector3( tmp1, tmp2, tmp3 );
346 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
347 // The bitmasked value choices are as follows:
348 // unset: In this case (the default) JSBSim would only print
349 // out the normally expected messages, essentially echoing
350 // the config files as they are read. If the environment
351 // variable is not set, debug_lvl is set to 1 internally
352 // 0: This requests JSBSim not to output any messages
354 // 1: This value explicity requests the normal JSBSim
356 // 2: This value asks for a message to be printed out when
357 // a class is instantiated
358 // 4: When this value is set, a message is displayed when a
359 // FGModel object executes its Run() method
360 // 8: When this value is set, various runtime state variables
361 // are printed out periodically
362 // 16: When set various parameters are sanity checked and
363 // a message is printed out when they go out of bounds
365 void FGMatrix33::Debug(int from)
367 if (debug_lvl <= 0) return;
369 if (debug_lvl & 1) { // Standard console startup message output
370 if (from == 0) { // Constructor
374 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
375 if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
376 if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
378 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
380 if (debug_lvl & 8 ) { // Runtime state variables
382 if (debug_lvl & 16) { // Sanity checking
384 if (debug_lvl & 64) {
385 if (from == 0) { // Constructor
386 cout << IdSrc << endl;
387 cout << IdHdr << endl;