]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGMatrix33.cpp
Change from JSBSim:
[flightgear.git] / src / FDM / JSBSim / FGMatrix33.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGMatrix33.cpp
4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
5 Date started: 1998
6 Purpose: FGMatrix33 class
7 Called by: Various
8
9 FUNCTIONAL DESCRIPTION
10 --------------------------------------------------------------------------------
11
12 HISTORY
13 --------------------------------------------------------------------------------
14 ??/??/??   TP   Created
15 03/16/2000 JSB  Added exception throwing
16
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
18 INCLUDES
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
20
21 #include "FGMatrix33.h"
22 #include "FGColumnVector3.h"
23
24 static const char *IdSrc = "$Id$";
25 static const char *IdHdr = ID_MATRIX33;
26
27 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
28 CLASS IMPLEMENTATION
29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
30
31 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32
33 FGMatrix33::FGMatrix33(void)
34 {
35   InitMatrix();
36   rowCtr = colCtr = 1;
37   
38   Debug(0);
39 }
40
41 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42
43 FGMatrix33::FGMatrix33(int r, int c)
44 {
45   InitMatrix();
46   rowCtr = colCtr = 1;
47   
48   Debug(0);
49 }
50
51 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52
53 FGMatrix33::FGMatrix33(const FGMatrix33& M)
54 {
55   rowCtr = colCtr = 1;
56
57   data[1][1] = M.data[1][1];
58   data[1][2] = M.data[1][2];
59   data[1][3] = M.data[1][3];
60   data[2][1] = M.data[2][1];
61   data[2][2] = M.data[2][2];
62   data[2][3] = M.data[2][3];
63   data[3][1] = M.data[3][1];
64   data[3][2] = M.data[3][2];
65   data[3][3] = M.data[3][3];
66
67   Debug(0);
68 }
69
70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71
72 FGMatrix33::~FGMatrix33(void)
73 {
74   rowCtr = colCtr = 1;
75
76   Debug(1);
77 }
78
79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80
81 ostream& operator<<(ostream& os, const FGMatrix33& M)
82 {
83   for (unsigned int i=1; i<=M.Rows(); i++) {
84     for (unsigned int j=1; j<=M.Cols(); j++) {
85       if (i == M.Rows() && j == M.Cols())
86         os << M.data[i][j];
87       else
88         os << M.data[i][j] << ", ";
89     }
90   }
91   return os;
92 }
93
94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95
96 FGMatrix33& FGMatrix33::operator<<(const double ff)
97 {
98   data[rowCtr][colCtr] = ff;
99   if (++colCtr > Cols()) {
100     colCtr = 1;
101     if (++rowCtr > Rows())
102       rowCtr = 1;
103   }
104   return *this;
105 }
106
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
108
109 istream& operator>>(istream& is, FGMatrix33& M)
110 {
111   for (unsigned int i=1; i<=M.Rows(); i++) {
112     for (unsigned int j=1; j<=M.Cols(); j++) {
113       is >> M.data[i][j];
114     }
115   }
116   return is;
117 }
118
119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
120
121 FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
122 {
123   if (&M != this) {
124     data[1][1] = M.data[1][1];
125     data[1][2] = M.data[1][2];
126     data[1][3] = M.data[1][3];
127     data[2][1] = M.data[2][1];
128     data[2][2] = M.data[2][2];
129     data[2][3] = M.data[2][3];
130     data[3][1] = M.data[3][1];
131     data[3][2] = M.data[3][2];
132     data[3][3] = M.data[3][3];
133   }
134   return *this;
135 }
136
137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
138
139 void FGMatrix33::InitMatrix(double value)
140 {
141   if (data) {
142     data[1][1] = value;
143     data[1][2] = value;
144     data[1][3] = value;
145     data[2][1] = value;
146     data[2][2] = value;
147     data[2][3] = value;
148     data[3][1] = value;
149     data[3][2] = value;
150     data[3][3] = value;
151   }
152 }
153
154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
155
156 void FGMatrix33::InitMatrix(void)
157 {
158   this->InitMatrix(0);
159 }
160
161 // *****************************************************************************
162 // binary operators ************************************************************
163 // *****************************************************************************
164
165 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
166 {
167   FGMatrix33 Diff;
168
169   Diff(1,1) = data[1][1] - M(1,1);
170   Diff(1,2) = data[1][2] - M(1,2);
171   Diff(1,3) = data[1][3] - M(1,3);
172   Diff(2,1) = data[2][1] - M(2,1);
173   Diff(2,2) = data[2][2] - M(2,2);
174   Diff(2,3) = data[2][3] - M(2,3);
175   Diff(3,1) = data[3][1] - M(3,1);
176   Diff(3,2) = data[3][2] - M(3,2);
177   Diff(3,3) = data[3][3] - M(3,3);
178
179   return Diff;
180 }
181
182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183
184 void FGMatrix33::operator-=(const FGMatrix33 &M)
185 {
186   data[1][1] -= M(1,1);
187   data[1][2] -= M(1,2);
188   data[1][3] -= M(1,3);
189   data[2][1] -= M(2,1);
190   data[2][2] -= M(2,2);
191   data[2][3] -= M(2,3);
192   data[3][1] -= M(3,1);
193   data[3][2] -= M(3,2);
194   data[3][3] -= M(3,3);
195 }
196
197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
198
199 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
200 {
201   FGMatrix33 Sum;
202
203   Sum(1,1) = data[1][1] + M(1,1);
204   Sum(1,2) = data[1][2] + M(1,2);
205   Sum(1,3) = data[1][3] + M(1,3);
206   Sum(2,1) = data[2][1] + M(2,1);
207   Sum(2,2) = data[2][2] + M(2,2);
208   Sum(2,3) = data[2][3] + M(2,3);
209   Sum(3,1) = data[3][1] + M(3,1);
210   Sum(3,2) = data[3][2] + M(3,2);
211   Sum(3,3) = data[3][3] + M(3,3);
212
213   return Sum;
214 }
215
216 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
217
218 void FGMatrix33::operator+=(const FGMatrix33 &M)
219 {
220   data[1][1] += M(1,1);
221   data[1][2] += M(1,2);
222   data[1][3] += M(1,3);
223   data[2][1] += M(2,1);
224   data[2][2] += M(2,2);
225   data[2][3] += M(2,3);
226   data[3][1] += M(3,1);
227   data[3][2] += M(3,2);
228   data[3][3] += M(3,3);
229 }
230
231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
232
233 FGMatrix33 FGMatrix33::operator*(const double scalar)
234 {
235   FGMatrix33 Product;
236   
237   Product(1,1) = data[1][1] * scalar;
238   Product(1,2) = data[1][2] * scalar;
239   Product(1,3) = data[1][3] * scalar;
240   Product(2,1) = data[2][1] * scalar;
241   Product(2,2) = data[2][2] * scalar;
242   Product(2,3) = data[2][3] * scalar;
243   Product(3,1) = data[3][1] * scalar;
244   Product(3,2) = data[3][2] * scalar;
245   Product(3,3) = data[3][3] * scalar;
246
247   return Product;
248 }
249
250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
251
252 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
253 {
254   FGMatrix33 Product;
255   
256   Product(1,1) = M(1,1) * scalar;
257   Product(1,2) = M(1,2) * scalar;
258   Product(1,3) = M(1,3) * scalar;
259   Product(2,1) = M(2,1) * scalar;
260   Product(2,2) = M(2,2) * scalar;
261   Product(2,3) = M(2,3) * scalar;
262   Product(3,1) = M(3,1) * scalar;
263   Product(3,2) = M(3,2) * scalar;
264   Product(3,3) = M(3,3) * scalar;
265
266   return Product;
267 }
268
269 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
270
271 void FGMatrix33::operator*=(const double scalar)
272 {
273   data[1][1] *= scalar;
274   data[1][2] *= scalar;
275   data[1][3] *= scalar;
276   data[2][1] *= scalar;
277   data[2][2] *= scalar;
278   data[2][3] *= scalar;
279   data[3][1] *= scalar;
280   data[3][2] *= scalar;
281   data[3][3] *= scalar;
282 }
283
284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
285
286 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
287 {
288   FGMatrix33 Product;
289   
290   Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
291   Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
292   Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
293   Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
294   Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
295   Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
296   Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
297   Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
298   Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
299   
300   return Product;
301 }
302
303 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
304
305 void FGMatrix33::operator*=(const FGMatrix33& M)
306 {
307   double a,b,c;
308   
309   a = data[1][1]; b=data[1][2]; c=data[1][3];
310   data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
311   data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
312   data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
313   
314   a = data[2][1]; b=data[2][2]; c=data[2][3];
315   data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
316   data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
317   data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
318  
319   a = data[3][1]; b=data[3][2]; c=data[3][3];
320   data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
321   data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
322   data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
323 }
324
325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
326
327 FGMatrix33 FGMatrix33::operator/(const double scalar)
328 {
329   FGMatrix33 Quot;
330   
331   if ( scalar != 0 ) {
332           double tmp = 1.0/scalar;
333     Quot(1,1) = data[1][1] * tmp;
334     Quot(1,2) = data[1][2] * tmp;
335     Quot(1,3) = data[1][3] * tmp;
336     Quot(2,1) = data[2][1] * tmp;
337     Quot(2,2) = data[2][2] * tmp;
338     Quot(2,3) = data[2][3] * tmp;
339     Quot(3,1) = data[3][1] * tmp;
340     Quot(3,2) = data[3][2] * tmp;
341     Quot(3,3) = data[3][3] * tmp;
342   } else {
343     MatrixException mE;
344     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
345     throw mE;
346   }
347   return Quot;  
348 }
349
350 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
351
352 void FGMatrix33::operator/=(const double scalar)
353 {
354   if ( scalar != 0 ) {
355     double tmp = 1.0/scalar;
356     data[1][1] *= tmp;
357     data[1][2] *= tmp;
358     data[1][3] *= tmp;
359     data[2][1] *= tmp;
360     data[2][2] *= tmp;
361     data[2][3] *= tmp;
362     data[3][1] *= tmp;
363     data[3][2] *= tmp;
364     data[3][3] *= tmp;
365   } else {
366     MatrixException mE;
367     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
368     throw mE;
369   }
370 }
371
372 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
373
374 void FGMatrix33::T(void)
375 {
376   for (unsigned int i=1; i<=3; i++) {
377     for (unsigned int j=i+1; j<=3; j++) {
378       double tmp = data[i][j];
379       data[i][j] = data[j][i];
380       data[j][i] = tmp;
381     }
382   }
383 }
384
385 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
386
387 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
388 {
389   FGColumnVector3 Product;
390
391   Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
392   Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
393   Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
394
395   return Product;
396 }
397
398 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
399 //    The bitmasked value choices are as follows:
400 //    unset: In this case (the default) JSBSim would only print
401 //       out the normally expected messages, essentially echoing
402 //       the config files as they are read. If the environment
403 //       variable is not set, debug_lvl is set to 1 internally
404 //    0: This requests JSBSim not to output any messages
405 //       whatsoever.
406 //    1: This value explicity requests the normal JSBSim
407 //       startup messages
408 //    2: This value asks for a message to be printed out when
409 //       a class is instantiated
410 //    4: When this value is set, a message is displayed when a
411 //       FGModel object executes its Run() method
412 //    8: When this value is set, various runtime state variables
413 //       are printed out periodically
414 //    16: When set various parameters are sanity checked and
415 //       a message is printed out when they go out of bounds
416
417 void FGMatrix33::Debug(int from)
418 {
419   if (debug_lvl <= 0) return;
420
421   if (debug_lvl & 1) { // Standard console startup message output
422     if (from == 0) { // Constructor
423
424     }
425   }
426   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
427     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
428     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
429   }
430   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
431   }
432   if (debug_lvl & 8 ) { // Runtime state variables
433   }
434   if (debug_lvl & 16) { // Sanity checking
435   }
436   if (debug_lvl & 64) {
437     if (from == 0) { // Constructor
438       cout << IdSrc << endl;
439       cout << IdHdr << endl;
440     }
441   }
442 }
443