src/FDM/JSBSim/math/FGMatrix33.cpp

   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
  43 #include <iostream>
  44
  45 using namespace std;
  46
  47 namespace JSBSim {
  48
  49 static const char *IdSrc = "$Id$";
  50 static const char *IdHdr = ID_MATRIX33;
  51
  52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 CLASS IMPLEMENTATION
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  57
  58 FGMatrix33::FGMatrix33(void)
  59 {
  60   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
  61     data[6] = data[7] = data[8] = 0.0;
  62
  63   // Debug(0);
  64 }
  65
  66 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  67
  68 ostream& operator<<(ostream& os, const FGMatrix33& M)
  69 {
  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())
  73         os << M(i,j);
  74       else
  75         os << M(i,j) << ", ";
  76     }
  77   }
  78   return os;
  79 }
  80
  81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82
  83 istream& operator>>(istream& is, FGMatrix33& M)
  84 {
  85   for (unsigned int i=1; i<=M.Rows(); i++) {
  86     for (unsigned int j=1; j<=M.Cols(); j++) {
  87       is >> M(i,j);
  88     }
  89   }
  90   return is;
  91 }
  92
  93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  94
  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);
  99 }
 100
 101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 102
 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
 106   // for this. :)
 107   double rdet = 1.0/Determinant();
 108
 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));
 118
 119   return FGMatrix33( i11, i12, i13,
 120                      i21, i22, i23,
 121                      i31, i32, i33 );
 122 }
 123
 124 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 125
 126 void FGMatrix33::InitMatrix(void)
 127 {
 128   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 129     data[6] = data[7] = data[8] = 0.0;
 130 }
 131
 132 // *****************************************************************************
 133 // binary operators ************************************************************
 134 // *****************************************************************************
 135
 136 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 137 {
 138   return FGMatrix33( Entry(1,1) - M(1,1),
 139                      Entry(1,2) - M(1,2),
 140                      Entry(1,3) - M(1,3),
 141                      Entry(2,1) - M(2,1),
 142                      Entry(2,2) - M(2,2),
 143                      Entry(2,3) - M(2,3),
 144                      Entry(3,1) - M(3,1),
 145                      Entry(3,2) - M(3,2),
 146                      Entry(3,3) - M(3,3) );
 147 }
 148
 149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 150
 151 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 152 {
 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];
 162
 163   return *this;
 164 }
 165
 166 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 167
 168 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 169 {
 170   return FGMatrix33( Entry(1,1) + M(1,1),
 171                      Entry(1,2) + M(1,2),
 172                      Entry(1,3) + M(1,3),
 173                      Entry(2,1) + M(2,1),
 174                      Entry(2,2) + M(2,2),
 175                      Entry(2,3) + M(2,3),
 176                      Entry(3,1) + M(3,1),
 177                      Entry(3,2) + M(3,2),
 178                      Entry(3,3) + M(3,3) );
 179 }
 180
 181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 182
 183 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 184 {
 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);
 194
 195   return *this;
 196 }
 197
 198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 199
 200 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 201 {
 202   return FGMatrix33( scalar * Entry(1,1),
 203                      scalar * Entry(1,2),
 204                      scalar * Entry(1,3),
 205                      scalar * Entry(2,1),
 206                      scalar * Entry(2,2),
 207                      scalar * Entry(2,3),
 208                      scalar * Entry(3,1),
 209                      scalar * Entry(3,2),
 210                      scalar * Entry(3,3) );
 211 }
 212
 213 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 214
 215 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 216 {
 217   return FGMatrix33( scalar * M(1,1),
 218                      scalar * M(1,2),
 219                      scalar * M(1,3),
 220                      scalar * M(2,1),
 221                      scalar * M(2,2),
 222                      scalar * M(2,3),
 223                      scalar * M(3,1),
 224                      scalar * M(3,2),
 225                      scalar * M(3,3) );
 226 }
 227
 228 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 229
 230 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 231 {
 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;
 241
 242   return *this;
 243 }
 244
 245 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 246
 247 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 248 {
 249   // FIXME: Make compiler friendlier
 250   FGMatrix33 Product;
 251
 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);
 261
 262   return Product;
 263 }
 264
 265 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 266
 267 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 268 {
 269   // FIXME: Make compiler friendlier
 270   double a,b,c;
 271
 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);
 276
 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);
 281
 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);
 286
 287   return *this;
 288 }
 289
 290 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 291
 292 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 293 {
 294   FGMatrix33 Quot;
 295
 296   if ( scalar != 0 ) {
 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;
 307   } else {
 308     MatrixException mE;
 309     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 310     throw mE;
 311   }
 312   return Quot;
 313 }
 314
 315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 316
 317 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 318 {
 319   if ( scalar != 0 ) {
 320     double tmp = 1.0/scalar;
 321     Entry(1,1) *= tmp;
 322     Entry(1,2) *= tmp;
 323     Entry(1,3) *= tmp;
 324     Entry(2,1) *= tmp;
 325     Entry(2,2) *= tmp;
 326     Entry(2,3) *= tmp;
 327     Entry(3,1) *= tmp;
 328     Entry(3,2) *= tmp;
 329     Entry(3,3) *= tmp;
 330   } else {
 331     MatrixException mE;
 332     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 333     throw mE;
 334   }
 335   return *this;
 336 }
 337
 338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 339
 340 void FGMatrix33::T(void)
 341 {
 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);
 346       Entry(j,i) = tmp;
 347     }
 348   }
 349 }
 350
 351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 352
 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);
 357
 358   tmp1 += v(2)*Entry(1,2);
 359   tmp2 += v(2)*Entry(2,2);
 360   tmp3 += v(2)*Entry(3,2);
 361
 362   tmp1 += v(3)*Entry(1,3);
 363   tmp2 += v(3)*Entry(2,3);
 364   tmp3 += v(3)*Entry(3,3);
 365
 366   return FGColumnVector3( tmp1, tmp2, tmp3 );
 367 }
 368
 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
 376 //       whatsoever.
 377 //    1: This value explicity requests the normal JSBSim
 378 //       startup messages
 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
 387
 388 void FGMatrix33::Debug(int from)
 389 {
 390   if (debug_lvl <= 0) return;
 391
 392   if (debug_lvl & 1) { // Standard console startup message output
 393     if (from == 0) { // Constructor
 394
 395     }
 396   }
 397   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 398     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 399     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 400   }
 401   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 402   }
 403   if (debug_lvl & 8 ) { // Runtime state variables
 404   }
 405   if (debug_lvl & 16) { // Sanity checking
 406   }
 407   if (debug_lvl & 64) {
 408     if (from == 0) { // Constructor
 409       cout << IdSrc << endl;
 410       cout << IdHdr << endl;
 411     }
 412   }
 413 }
 414 }