Branch data Line data Source code
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 Software
13 : : Foundation; either version 2 of the License, or (at your option) any later
14 : : 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 with
22 : : this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 : : Place - Suite 330, Boston, MA 02111-1307, USA.
24 : :
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.
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 <sstream>
43 : : #include <iomanip>
44 : :
45 : : #include <iostream>
46 : :
47 : : using namespace std;
48 : :
49 : : namespace JSBSim {
50 : :
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;
53 : :
54 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 : : CLASS IMPLEMENTATION
56 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
57 : :
58 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
59 : :
60 : 1836293 : FGMatrix33::FGMatrix33(void)
61 : : {
62 : : data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
63 : 1836293 : data[6] = data[7] = data[8] = 0.0;
64 : :
65 : : // Debug(0);
66 : 1836293 : }
67 : :
68 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
69 : :
70 : 4909 : string FGMatrix33::Dump(const string& delimiter) const
71 : : {
72 : 4909 : ostringstream buffer;
73 : 4909 : buffer << setw(12) << setprecision(10) << data[0] << delimiter;
74 : 4909 : buffer << setw(12) << setprecision(10) << data[3] << delimiter;
75 : 4909 : buffer << setw(12) << setprecision(10) << data[6] << delimiter;
76 : 4909 : buffer << setw(12) << setprecision(10) << data[1] << delimiter;
77 : 4909 : buffer << setw(12) << setprecision(10) << data[4] << delimiter;
78 : 4909 : buffer << setw(12) << setprecision(10) << data[7] << delimiter;
79 : 4909 : buffer << setw(12) << setprecision(10) << data[2] << delimiter;
80 : 4909 : buffer << setw(12) << setprecision(10) << data[5] << delimiter;
81 : 19636 : buffer << setw(12) << setprecision(10) << data[8];
82 : 4909 : return buffer.str();
83 : : }
84 : :
85 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
86 : :
87 : 0 : string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
88 : : {
89 : 0 : ostringstream buffer;
90 : :
91 : 0 : buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
92 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
93 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
94 : :
95 : 0 : buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
96 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
97 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
98 : :
99 : 0 : buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
100 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
101 : 0 : buffer << right << fixed << setw(9) << setprecision(6) << data[8];
102 : :
103 : 0 : buffer << setw(0) << left;
104 : :
105 : 0 : return buffer.str();
106 : : }
107 : :
108 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 : :
110 : 54006 : FGQuaternion FGMatrix33::GetQuaternion(void)
111 : : {
112 : 54006 : FGQuaternion Q;
113 : :
114 : : double tempQ[4];
115 : : int idx;
116 : :
117 : 54006 : tempQ[0] = 1.0 + data[0] + data[4] + data[8];
118 : 54006 : tempQ[1] = 1.0 + data[0] - data[4] - data[8];
119 : 54006 : tempQ[2] = 1.0 - data[0] + data[4] - data[8];
120 : 54006 : tempQ[3] = 1.0 - data[0] - data[4] + data[8];
121 : :
122 : : // Find largest of the above
123 : 54006 : idx = 0;
124 [ + + ][ + + ]: 216024 : for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
125 : :
126 [ + + + + : 54006 : switch(idx) {
- ]
127 : : case 0:
128 : 1 : Q(1) = 0.50*sqrt(tempQ[0]);
129 : 3 : Q(2) = 0.25*(data[7] - data[5])/Q(1);
130 : 3 : Q(3) = 0.25*(data[2] - data[6])/Q(1);
131 : 3 : Q(4) = 0.25*(data[3] - data[1])/Q(1);
132 : 1 : break;
133 : : case 1:
134 : 4013 : Q(2) = 0.50*sqrt(tempQ[1]);
135 : 12039 : Q(1) = 0.25*(data[7] - data[5])/Q(2);
136 : 12039 : Q(3) = 0.25*(data[3] + data[1])/Q(2);
137 : 12039 : Q(4) = 0.25*(data[2] + data[6])/Q(2);
138 : 4013 : break;
139 : : case 2:
140 : 49293 : Q(3) = 0.50*sqrt(tempQ[2]);
141 : 147879 : Q(1) = 0.25*(data[2] - data[6])/Q(3);
142 : 147879 : Q(2) = 0.25*(data[3] + data[1])/Q(3);
143 : 147879 : Q(4) = 0.25*(data[7] + data[5])/Q(3);
144 : 49293 : break;
145 : : case 3:
146 : 699 : Q(4) = 0.50*sqrt(tempQ[3]);
147 : 2097 : Q(1) = 0.25*(data[3] - data[1])/Q(4);
148 : 2097 : Q(2) = 0.25*(data[6] + data[2])/Q(4);
149 : 54006 : Q(3) = 0.25*(data[7] + data[5])/Q(4);
150 : : break;
151 : : default:
152 : : //error
153 : : break;
154 : : }
155 : :
156 : : return (Q);
157 : : }
158 : :
159 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
160 : :
161 : 0 : ostream& operator<<(ostream& os, const FGMatrix33& M)
162 : : {
163 [ # # ]: 0 : for (unsigned int i=1; i<=M.Rows(); i++) {
164 [ # # ]: 0 : for (unsigned int j=1; j<=M.Cols(); j++) {
165 [ # # ][ # # ]: 0 : if (i == M.Rows() && j == M.Cols())
[ # # ]
166 : : os << M(i,j);
167 : : else
168 : 0 : os << M(i,j) << ", ";
169 : : }
170 : : }
171 : 0 : return os;
172 : : }
173 : :
174 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
175 : :
176 : 0 : istream& operator>>(istream& is, FGMatrix33& M)
177 : : {
178 [ # # ]: 0 : for (unsigned int i=1; i<=M.Rows(); i++) {
179 [ # # ]: 0 : for (unsigned int j=1; j<=M.Cols(); j++) {
180 : : is >> M(i,j);
181 : : }
182 : : }
183 : 0 : return is;
184 : : }
185 : :
186 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
187 : :
188 : 0 : 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 : 0 : - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
192 : : }
193 : :
194 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
195 : :
196 : 0 : 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
199 : : // for this. :)
200 : :
201 [ # # ]: 0 : if (Determinant() != 0.0) {
202 : 0 : double rdet = 1.0/Determinant();
203 : :
204 : 0 : double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
205 : 0 : double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
206 : 0 : double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
207 : 0 : double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
208 : 0 : double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
209 : 0 : double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
210 : 0 : double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
211 : 0 : double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
212 : 0 : double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
213 : :
214 : : return FGMatrix33( i11, i12, i13,
215 : : i21, i22, i23,
216 : 0 : i31, i32, i33 );
217 : : } else {
218 : : return FGMatrix33( 0, 0, 0,
219 : : 0, 0, 0,
220 : 0 : 0, 0, 0 );
221 : : }
222 : : }
223 : :
224 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
225 : :
226 : 43 : void FGMatrix33::InitMatrix(void)
227 : : {
228 : : data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
229 : 43 : data[6] = data[7] = data[8] = 0.0;
230 : 43 : }
231 : :
232 : : // *****************************************************************************
233 : : // binary operators ************************************************************
234 : : // *****************************************************************************
235 : :
236 : 0 : FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
237 : : {
238 : : return FGMatrix33( data[0] - M.data[0],
239 : : data[3] - M.data[3],
240 : : data[6] - M.data[6],
241 : : data[1] - M.data[1],
242 : : data[4] - M.data[4],
243 : : data[7] - M.data[7],
244 : : data[2] - M.data[2],
245 : : data[5] - M.data[5],
246 : 0 : data[8] - M.data[8] );
247 : : }
248 : :
249 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
250 : :
251 : 0 : FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
252 : : {
253 : 0 : data[0] -= M.data[0];
254 : 0 : data[1] -= M.data[1];
255 : 0 : data[2] -= M.data[2];
256 : 0 : data[3] -= M.data[3];
257 : 0 : data[4] -= M.data[4];
258 : 0 : data[5] -= M.data[5];
259 : 0 : data[6] -= M.data[6];
260 : 0 : data[7] -= M.data[7];
261 : 0 : data[8] -= M.data[8];
262 : :
263 : 0 : return *this;
264 : : }
265 : :
266 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
267 : :
268 : 0 : FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
269 : : {
270 : : return FGMatrix33( data[0] + M.data[0],
271 : : data[3] + M.data[3],
272 : : data[6] + M.data[6],
273 : : data[1] + M.data[1],
274 : : data[4] + M.data[4],
275 : : data[7] + M.data[7],
276 : : data[2] + M.data[2],
277 : : data[5] + M.data[5],
278 : 0 : data[8] + M.data[8] );
279 : : }
280 : :
281 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
282 : :
283 : 2592294 : FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
284 : : {
285 : 2592294 : data[0] += M.data[0];
286 : 2592294 : data[3] += M.data[3];
287 : 2592294 : data[6] += M.data[6];
288 : 2592294 : data[1] += M.data[1];
289 : 2592294 : data[4] += M.data[4];
290 : 2592294 : data[7] += M.data[7];
291 : 2592294 : data[2] += M.data[2];
292 : 2592294 : data[5] += M.data[5];
293 : 2592294 : data[8] += M.data[8];
294 : :
295 : 2592294 : return *this;
296 : : }
297 : :
298 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
299 : :
300 : 0 : FGMatrix33 FGMatrix33::operator*(const double scalar) const
301 : : {
302 : : return FGMatrix33( scalar * data[0],
303 : : scalar * data[3],
304 : : scalar * data[6],
305 : : scalar * data[1],
306 : : scalar * data[4],
307 : : scalar * data[7],
308 : : scalar * data[2],
309 : : scalar * data[5],
310 : 0 : scalar * data[8] );
311 : : }
312 : :
313 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
314 : : /*
315 : : FGMatrix33 operator*(double scalar, FGMatrix33 &M)
316 : : {
317 : : return FGMatrix33( scalar * M(1,1),
318 : : scalar * M(1,2),
319 : : scalar * M(1,3),
320 : : scalar * M(2,1),
321 : : scalar * M(2,2),
322 : : scalar * M(2,3),
323 : : scalar * M(3,1),
324 : : scalar * M(3,2),
325 : : scalar * M(3,3) );
326 : : }
327 : : */
328 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
329 : :
330 : 0 : FGMatrix33& FGMatrix33::operator*=(const double scalar)
331 : : {
332 : 0 : data[0] *= scalar;
333 : 0 : data[3] *= scalar;
334 : 0 : data[6] *= scalar;
335 : 0 : data[1] *= scalar;
336 : 0 : data[4] *= scalar;
337 : 0 : data[7] *= scalar;
338 : 0 : data[2] *= scalar;
339 : 0 : data[5] *= scalar;
340 : 0 : data[8] *= scalar;
341 : :
342 : 0 : return *this;
343 : : }
344 : :
345 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
346 : :
347 : 324037 : FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
348 : : {
349 : 324037 : FGMatrix33 Product;
350 : :
351 : 324037 : Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
352 : 324037 : Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
353 : 324037 : Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
354 : 324037 : Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
355 : 324037 : Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
356 : 324037 : Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
357 : 324037 : Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
358 : 324037 : Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
359 : 324037 : Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
360 : :
361 : : return Product;
362 : : }
363 : :
364 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
365 : :
366 : 0 : FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
367 : : {
368 : : // FIXME: Make compiler friendlier
369 : : double a,b,c;
370 : :
371 : 0 : a = data[0]; b=data[3]; c=data[6];
372 : 0 : data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
373 : 0 : data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
374 : 0 : data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
375 : :
376 : 0 : a = data[1]; b=data[4]; c=data[7];
377 : 0 : data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
378 : 0 : data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
379 : 0 : data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
380 : :
381 : 0 : a = data[2]; b=data[5]; c=data[8];
382 : 0 : data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
383 : 0 : data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
384 : 0 : data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
385 : :
386 : 0 : return *this;
387 : : }
388 : :
389 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
390 : :
391 : 0 : FGMatrix33 FGMatrix33::operator/(const double scalar) const
392 : : {
393 : 0 : FGMatrix33 Quot;
394 : :
395 [ # # ]: 0 : if ( scalar != 0 ) {
396 : 0 : double tmp = 1.0/scalar;
397 : 0 : Quot.data[0] = data[0] * tmp;
398 : 0 : Quot.data[3] = data[3] * tmp;
399 : 0 : Quot.data[6] = data[6] * tmp;
400 : 0 : Quot.data[1] = data[1] * tmp;
401 : 0 : Quot.data[4] = data[4] * tmp;
402 : 0 : Quot.data[7] = data[7] * tmp;
403 : 0 : Quot.data[2] = data[2] * tmp;
404 : 0 : Quot.data[5] = data[5] * tmp;
405 : 0 : Quot.data[8] = data[8] * tmp;
406 : : } else {
407 : : MatrixException mE;
408 : : mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
409 : 0 : throw mE;
410 : : }
411 : 0 : return Quot;
412 : : }
413 : :
414 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
415 : :
416 : 0 : FGMatrix33& FGMatrix33::operator/=(const double scalar)
417 : : {
418 [ # # ]: 0 : if ( scalar != 0 ) {
419 : 0 : double tmp = 1.0/scalar;
420 : 0 : data[0] *= tmp;
421 : 0 : data[3] *= tmp;
422 : 0 : data[6] *= tmp;
423 : 0 : data[1] *= tmp;
424 : 0 : data[4] *= tmp;
425 : 0 : data[7] *= tmp;
426 : 0 : data[2] *= tmp;
427 : 0 : data[5] *= tmp;
428 : 0 : data[8] *= tmp;
429 : : } else {
430 : : MatrixException mE;
431 : : mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
432 : 0 : throw mE;
433 : : }
434 : 0 : return *this;
435 : : }
436 : :
437 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
438 : :
439 : 162022 : void FGMatrix33::T(void)
440 : : {
441 : : double tmp;
442 : :
443 : 162022 : tmp = data[3];
444 : 162022 : data[3] = data[1];
445 : 162022 : data[1] = tmp;
446 : :
447 : 162022 : tmp = data[6];
448 : 162022 : data[6] = data[2];
449 : 162022 : data[2] = tmp;
450 : :
451 : 162022 : tmp = data[7];
452 : 162022 : data[7] = data[5];
453 : 162022 : data[5] = tmp;
454 : 162022 : }
455 : :
456 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
457 : :
458 : 1514508 : FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
459 : : {
460 : 1514508 : double v1 = v(1);
461 : 1514508 : double v2 = v(2);
462 : 1514508 : double v3 = v(3);
463 : :
464 : 1514508 : double tmp1 = v1*data[0]; //[(col-1)*eRows+row-1]
465 : 1514508 : double tmp2 = v1*data[1];
466 : 1514508 : double tmp3 = v1*data[2];
467 : :
468 : 1514508 : tmp1 += v2*data[3];
469 : 1514508 : tmp2 += v2*data[4];
470 : 1514508 : tmp3 += v2*data[5];
471 : :
472 : 1514508 : tmp1 += v3*data[6];
473 : 1514508 : tmp2 += v3*data[7];
474 : 1514508 : tmp3 += v3*data[8];
475 : :
476 : 1514508 : return FGColumnVector3( tmp1, tmp2, tmp3 );
477 : : }
478 : :
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
486 : : // whatsoever.
487 : : // 1: This value explicity requests the normal JSBSim
488 : : // startup messages
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
497 : :
498 : 0 : void FGMatrix33::Debug(int from)
499 : : {
500 [ # # ]: 0 : if (debug_lvl <= 0) return;
501 : :
502 : 0 : if (debug_lvl & 1) { // Standard console startup message output
503 : : if (from == 0) { // Constructor
504 : :
505 : : }
506 : : }
507 [ # # ]: 0 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
508 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
509 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
510 : : }
511 : 0 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
512 : : }
513 : 0 : if (debug_lvl & 8 ) { // Runtime state variables
514 : : }
515 : 0 : if (debug_lvl & 16) { // Sanity checking
516 : : }
517 [ # # ]: 0 : if (debug_lvl & 64) {
518 [ # # ]: 0 : if (from == 0) { // Constructor
519 : 0 : cout << IdSrc << endl;
520 : 0 : cout << IdHdr << endl;
521 : : }
522 : : }
523 : : }
524 [ + + ][ + - ]: 12 : }
|
