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"
45 static const char *IdSrc = "$Id$";
46 static const char *IdHdr = ID_MATRIX33;
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 FGMatrix33::FGMatrix33(void)
56 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
57 data[6] = data[7] = data[8] = 0.0;
62 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
64 ostream& operator<<(ostream& os, const FGMatrix33& M)
66 for (unsigned int i=1; i<=M.Rows(); i++) {
67 for (unsigned int j=1; j<=M.Cols(); j++) {
68 if (i == M.Rows() && j == M.Cols())
77 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
79 istream& operator>>(istream& is, FGMatrix33& M)
81 for (unsigned int i=1; i<=M.Rows(); i++) {
82 for (unsigned int j=1; j<=M.Cols(); j++) {
89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 double FGMatrix33::Determinant(void) const {
92 return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
93 + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
94 - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
99 FGMatrix33 FGMatrix33::Inverse(void) const {
100 // Compute the inverse of a general matrix using Cramers rule.
101 // I guess googling for cramers rule gives tons of references
103 double rdet = 1.0/Determinant();
105 double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
106 double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
107 double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
108 double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
109 double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
110 double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
111 double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
112 double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
113 double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
115 return FGMatrix33( i11, i12, i13,
120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122 void FGMatrix33::InitMatrix(void)
124 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
125 data[6] = data[7] = data[8] = 0.0;
128 // *****************************************************************************
129 // binary operators ************************************************************
130 // *****************************************************************************
132 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
134 return FGMatrix33( Entry(1,1) - M(1,1),
142 Entry(3,3) - M(3,3) );
145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
147 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
149 data[0] -= M.data[0];
150 data[1] -= M.data[1];
151 data[2] -= M.data[2];
152 data[3] -= M.data[3];
153 data[4] -= M.data[4];
154 data[5] -= M.data[5];
155 data[6] -= M.data[6];
156 data[7] -= M.data[7];
157 data[8] -= M.data[8];
162 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
164 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
166 return FGMatrix33( Entry(1,1) + M(1,1),
174 Entry(3,3) + M(3,3) );
177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
179 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
181 Entry(1,1) += M(1,1);
182 Entry(1,2) += M(1,2);
183 Entry(1,3) += M(1,3);
184 Entry(2,1) += M(2,1);
185 Entry(2,2) += M(2,2);
186 Entry(2,3) += M(2,3);
187 Entry(3,1) += M(3,1);
188 Entry(3,2) += M(3,2);
189 Entry(3,3) += M(3,3);
194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
196 FGMatrix33 FGMatrix33::operator*(const double scalar) const
198 return FGMatrix33( scalar * Entry(1,1),
206 scalar * Entry(3,3) );
209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
211 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
213 return FGMatrix33( scalar * M(1,1),
224 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
226 FGMatrix33& FGMatrix33::operator*=(const double scalar)
228 Entry(1,1) *= scalar;
229 Entry(1,2) *= scalar;
230 Entry(1,3) *= scalar;
231 Entry(2,1) *= scalar;
232 Entry(2,2) *= scalar;
233 Entry(2,3) *= scalar;
234 Entry(3,1) *= scalar;
235 Entry(3,2) *= scalar;
236 Entry(3,3) *= scalar;
241 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
243 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
245 // FIXME: Make compiler friendlier
248 Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
249 Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
250 Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
251 Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
252 Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
253 Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
254 Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
255 Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
256 Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
261 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
263 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
265 // FIXME: Make compiler friendlier
268 a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
269 Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
270 Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
271 Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
273 a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
274 Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
275 Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
276 Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
278 a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
279 Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
280 Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
281 Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
286 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
288 FGMatrix33 FGMatrix33::operator/(const double scalar) const
293 double tmp = 1.0/scalar;
294 Quot(1,1) = Entry(1,1) * tmp;
295 Quot(1,2) = Entry(1,2) * tmp;
296 Quot(1,3) = Entry(1,3) * tmp;
297 Quot(2,1) = Entry(2,1) * tmp;
298 Quot(2,2) = Entry(2,2) * tmp;
299 Quot(2,3) = Entry(2,3) * tmp;
300 Quot(3,1) = Entry(3,1) * tmp;
301 Quot(3,2) = Entry(3,2) * tmp;
302 Quot(3,3) = Entry(3,3) * tmp;
305 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
313 FGMatrix33& FGMatrix33::operator/=(const double scalar)
316 double tmp = 1.0/scalar;
328 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
336 void FGMatrix33::T(void)
338 for (unsigned int i=1; i<=3; i++) {
339 for (unsigned int j=i+1; j<=3; j++) {
340 double tmp = Entry(i,j);
341 Entry(i,j) = Entry(j,i);
347 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
349 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
350 double tmp1 = v(1)*Entry(1,1);
351 double tmp2 = v(1)*Entry(2,1);
352 double tmp3 = v(1)*Entry(3,1);
354 tmp1 += v(2)*Entry(1,2);
355 tmp2 += v(2)*Entry(2,2);
356 tmp3 += v(2)*Entry(3,2);
358 tmp1 += v(3)*Entry(1,3);
359 tmp2 += v(3)*Entry(2,3);
360 tmp3 += v(3)*Entry(3,3);
362 return FGColumnVector3( tmp1, tmp2, tmp3 );
365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
366 // The bitmasked value choices are as follows:
367 // unset: In this case (the default) JSBSim would only print
368 // out the normally expected messages, essentially echoing
369 // the config files as they are read. If the environment
370 // variable is not set, debug_lvl is set to 1 internally
371 // 0: This requests JSBSim not to output any messages
373 // 1: This value explicity requests the normal JSBSim
375 // 2: This value asks for a message to be printed out when
376 // a class is instantiated
377 // 4: When this value is set, a message is displayed when a
378 // FGModel object executes its Run() method
379 // 8: When this value is set, various runtime state variables
380 // are printed out periodically
381 // 16: When set various parameters are sanity checked and
382 // a message is printed out when they go out of bounds
384 void FGMatrix33::Debug(int from)
386 if (debug_lvl <= 0) return;
388 if (debug_lvl & 1) { // Standard console startup message output
389 if (from == 0) { // Constructor
393 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
394 if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
395 if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
397 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
399 if (debug_lvl & 8 ) { // Runtime state variables
401 if (debug_lvl & 16) { // Sanity checking
403 if (debug_lvl & 64) {
404 if (from == 0) { // Constructor
405 cout << IdSrc << endl;
406 cout << IdHdr << endl;