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