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"
49 static const char *IdSrc = "$Id$";
50 static const char *IdHdr = ID_MATRIX33;
52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 FGMatrix33::FGMatrix33(void)
60 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
61 data[6] = data[7] = data[8] = 0.0;
66 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
68 ostream& operator<<(ostream& os, const FGMatrix33& M)
70 for (unsigned int i=1; i<=M.Rows(); i++) {
71 for (unsigned int j=1; j<=M.Cols(); j++) {
72 if (i == M.Rows() && j == M.Cols())
81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 istream& operator>>(istream& is, FGMatrix33& M)
85 for (unsigned int i=1; i<=M.Rows(); i++) {
86 for (unsigned int j=1; j<=M.Cols(); j++) {
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95 double FGMatrix33::Determinant(void) const {
96 return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
97 + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
98 - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 FGMatrix33 FGMatrix33::Inverse(void) const {
104 // Compute the inverse of a general matrix using Cramers rule.
105 // I guess googling for cramers rule gives tons of references
107 double rdet = 1.0/Determinant();
109 double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
110 double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
111 double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
112 double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
113 double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
114 double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
115 double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
116 double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
117 double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
119 return FGMatrix33( i11, i12, i13,
124 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
126 void FGMatrix33::InitMatrix(void)
128 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
129 data[6] = data[7] = data[8] = 0.0;
132 // *****************************************************************************
133 // binary operators ************************************************************
134 // *****************************************************************************
136 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
138 return FGMatrix33( Entry(1,1) - M(1,1),
146 Entry(3,3) - M(3,3) );
149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
151 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
153 data[0] -= M.data[0];
154 data[1] -= M.data[1];
155 data[2] -= M.data[2];
156 data[3] -= M.data[3];
157 data[4] -= M.data[4];
158 data[5] -= M.data[5];
159 data[6] -= M.data[6];
160 data[7] -= M.data[7];
161 data[8] -= M.data[8];
166 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
168 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
170 return FGMatrix33( Entry(1,1) + M(1,1),
178 Entry(3,3) + M(3,3) );
181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
185 Entry(1,1) += M(1,1);
186 Entry(1,2) += M(1,2);
187 Entry(1,3) += M(1,3);
188 Entry(2,1) += M(2,1);
189 Entry(2,2) += M(2,2);
190 Entry(2,3) += M(2,3);
191 Entry(3,1) += M(3,1);
192 Entry(3,2) += M(3,2);
193 Entry(3,3) += M(3,3);
198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
200 FGMatrix33 FGMatrix33::operator*(const double scalar) const
202 return FGMatrix33( scalar * Entry(1,1),
210 scalar * Entry(3,3) );
213 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
215 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
217 return FGMatrix33( scalar * M(1,1),
228 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
230 FGMatrix33& FGMatrix33::operator*=(const double scalar)
232 Entry(1,1) *= scalar;
233 Entry(1,2) *= scalar;
234 Entry(1,3) *= scalar;
235 Entry(2,1) *= scalar;
236 Entry(2,2) *= scalar;
237 Entry(2,3) *= scalar;
238 Entry(3,1) *= scalar;
239 Entry(3,2) *= scalar;
240 Entry(3,3) *= scalar;
245 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
247 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
249 // FIXME: Make compiler friendlier
252 Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
253 Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
254 Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
255 Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
256 Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
257 Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
258 Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
259 Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
260 Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
265 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
267 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
269 // FIXME: Make compiler friendlier
272 a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
273 Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
274 Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
275 Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
277 a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
278 Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
279 Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
280 Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
282 a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
283 Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
284 Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
285 Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
290 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
292 FGMatrix33 FGMatrix33::operator/(const double scalar) const
297 double tmp = 1.0/scalar;
298 Quot(1,1) = Entry(1,1) * tmp;
299 Quot(1,2) = Entry(1,2) * tmp;
300 Quot(1,3) = Entry(1,3) * tmp;
301 Quot(2,1) = Entry(2,1) * tmp;
302 Quot(2,2) = Entry(2,2) * tmp;
303 Quot(2,3) = Entry(2,3) * tmp;
304 Quot(3,1) = Entry(3,1) * tmp;
305 Quot(3,2) = Entry(3,2) * tmp;
306 Quot(3,3) = Entry(3,3) * tmp;
309 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
317 FGMatrix33& FGMatrix33::operator/=(const double scalar)
320 double tmp = 1.0/scalar;
332 mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
340 void FGMatrix33::T(void)
342 for (unsigned int i=1; i<=3; i++) {
343 for (unsigned int j=i+1; j<=3; j++) {
344 double tmp = Entry(i,j);
345 Entry(i,j) = Entry(j,i);
351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
353 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
354 double tmp1 = v(1)*Entry(1,1);
355 double tmp2 = v(1)*Entry(2,1);
356 double tmp3 = v(1)*Entry(3,1);
358 tmp1 += v(2)*Entry(1,2);
359 tmp2 += v(2)*Entry(2,2);
360 tmp3 += v(2)*Entry(3,2);
362 tmp1 += v(3)*Entry(1,3);
363 tmp2 += v(3)*Entry(2,3);
364 tmp3 += v(3)*Entry(3,3);
366 return FGColumnVector3( tmp1, tmp2, tmp3 );
369 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
370 // The bitmasked value choices are as follows:
371 // unset: In this case (the default) JSBSim would only print
372 // out the normally expected messages, essentially echoing
373 // the config files as they are read. If the environment
374 // variable is not set, debug_lvl is set to 1 internally
375 // 0: This requests JSBSim not to output any messages
377 // 1: This value explicity requests the normal JSBSim
379 // 2: This value asks for a message to be printed out when
380 // a class is instantiated
381 // 4: When this value is set, a message is displayed when a
382 // FGModel object executes its Run() method
383 // 8: When this value is set, various runtime state variables
384 // are printed out periodically
385 // 16: When set various parameters are sanity checked and
386 // a message is printed out when they go out of bounds
388 void FGMatrix33::Debug(int from)
390 if (debug_lvl <= 0) return;
392 if (debug_lvl & 1) { // Standard console startup message output
393 if (from == 0) { // Constructor
397 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
398 if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
399 if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
401 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
403 if (debug_lvl & 8 ) { // Runtime state variables
405 if (debug_lvl & 16) { // Sanity checking
407 if (debug_lvl & 64) {
408 if (from == 0) { // Constructor
409 cout << IdSrc << endl;
410 cout << IdHdr << endl;