src/FDM/JSBSim/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 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   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
  38     data[6] = data[7] = data[8] = 0.0;
  39
  40   Debug(0);
  41 }
  42
  43 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  44
  45 ostream& operator<<(ostream& os, const FGMatrix33& M)
  46 {
  47   for (unsigned int i=1; i<=M.Rows(); i++) {
  48     for (unsigned int j=1; j<=M.Cols(); j++) {
  49       if (i == M.Rows() && j == M.Cols())
  50         os << M(i,j);
  51       else
  52         os << M(i,j) << ", ";
  53     }
  54   }
  55   return os;
  56 }
  57
  58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  59
  60 istream& operator>>(istream& is, FGMatrix33& M)
  61 {
  62   for (unsigned int i=1; i<=M.Rows(); i++) {
  63     for (unsigned int j=1; j<=M.Cols(); j++) {
  64       is >> M(i,j);
  65     }
  66   }
  67   return is;
  68 }
  69
  70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  71
  72 double FGMatrix33::Determinant(void) const {
  73   return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
  74     + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
  75     - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
  76 }
  77
  78 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  79
  80 FGMatrix33 FGMatrix33::Inverse(void) const {
  81   // Compute the inverse of a general matrix using Cramers rule.
  82   // I guess googling for cramers rule gives tons of references
  83   // for this. :)
  84   double rdet = 1.0/Determinant();
  85
  86   double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
  87   double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
  88   double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
  89   double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
  90   double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
  91   double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
  92   double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
  93   double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
  94   double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
  95
  96   return FGMatrix33( i11, i12, i13,
  97                      i21, i22, i23,
  98                      i31, i32, i33 );
  99 }
 100
 101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 102
 103 void FGMatrix33::InitMatrix(void)
 104 {
 105   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 106     data[6] = data[7] = data[8] = 0.0;
 107 }
 108
 109 // *****************************************************************************
 110 // binary operators ************************************************************
 111 // *****************************************************************************
 112
 113 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 114 {
 115   return FGMatrix33( Entry(1,1) - M(1,1),
 116                      Entry(1,2) - M(1,2),
 117                      Entry(1,3) - M(1,3),
 118                      Entry(2,1) - M(2,1),
 119                      Entry(2,2) - M(2,2),
 120                      Entry(2,3) - M(2,3),
 121                      Entry(3,1) - M(3,1),
 122                      Entry(3,2) - M(3,2),
 123                      Entry(3,3) - M(3,3) );
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 129 {
 130   data[0] -= M.data[0];
 131   data[1] -= M.data[1];
 132   data[2] -= M.data[2];
 133   data[3] -= M.data[3];
 134   data[4] -= M.data[4];
 135   data[5] -= M.data[5];
 136   data[6] -= M.data[6];
 137   data[7] -= M.data[7];
 138   data[8] -= M.data[8];
 139
 140   return *this;
 141 }
 142
 143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 144
 145 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 146 {
 147   return FGMatrix33( Entry(1,1) + M(1,1),
 148                      Entry(1,2) + M(1,2),
 149                      Entry(1,3) + M(1,3),
 150                      Entry(2,1) + M(2,1),
 151                      Entry(2,2) + M(2,2),
 152                      Entry(2,3) + M(2,3),
 153                      Entry(3,1) + M(3,1),
 154                      Entry(3,2) + M(3,2),
 155                      Entry(3,3) + M(3,3) );
 156 }
 157
 158 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 159
 160 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 161 {
 162   Entry(1,1) += M(1,1);
 163   Entry(1,2) += M(1,2);
 164   Entry(1,3) += M(1,3);
 165   Entry(2,1) += M(2,1);
 166   Entry(2,2) += M(2,2);
 167   Entry(2,3) += M(2,3);
 168   Entry(3,1) += M(3,1);
 169   Entry(3,2) += M(3,2);
 170   Entry(3,3) += M(3,3);
 171
 172   return *this;
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176
 177 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 178 {
 179   return FGMatrix33( scalar * Entry(1,1),
 180                      scalar * Entry(1,2),
 181                      scalar * Entry(1,3),
 182                      scalar * Entry(2,1),
 183                      scalar * Entry(2,2),
 184                      scalar * Entry(2,3),
 185                      scalar * Entry(3,1),
 186                      scalar * Entry(3,2),
 187                      scalar * Entry(3,3) );
 188 }
 189
 190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 191
 192 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 193 {
 194   return FGMatrix33( scalar * M(1,1),
 195                      scalar * M(1,2),
 196                      scalar * M(1,3),
 197                      scalar * M(2,1),
 198                      scalar * M(2,2),
 199                      scalar * M(2,3),
 200                      scalar * M(3,1),
 201                      scalar * M(3,2),
 202                      scalar * M(3,3) );
 203 }
 204
 205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 206
 207 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 208 {
 209   Entry(1,1) *= scalar;
 210   Entry(1,2) *= scalar;
 211   Entry(1,3) *= scalar;
 212   Entry(2,1) *= scalar;
 213   Entry(2,2) *= scalar;
 214   Entry(2,3) *= scalar;
 215   Entry(3,1) *= scalar;
 216   Entry(3,2) *= scalar;
 217   Entry(3,3) *= scalar;
 218
 219   return *this;
 220 }
 221
 222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 223
 224 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 225 {
 226   // FIXME: Make compiler friendlier
 227   FGMatrix33 Product;
 228
 229   Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
 230   Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
 231   Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
 232   Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
 233   Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
 234   Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
 235   Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
 236   Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
 237   Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
 238
 239   return Product;
 240 }
 241
 242 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 243
 244 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 245 {
 246   // FIXME: Make compiler friendlier
 247   double a,b,c;
 248
 249   a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
 250   Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 251   Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 252   Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 253
 254   a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
 255   Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 256   Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 257   Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 258
 259   a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
 260   Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 261   Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 262   Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 263
 264   return *this;
 265 }
 266
 267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 268
 269 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 270 {
 271   FGMatrix33 Quot;
 272
 273   if ( scalar != 0 ) {
 274     double tmp = 1.0/scalar;
 275     Quot(1,1) = Entry(1,1) * tmp;
 276     Quot(1,2) = Entry(1,2) * tmp;
 277     Quot(1,3) = Entry(1,3) * tmp;
 278     Quot(2,1) = Entry(2,1) * tmp;
 279     Quot(2,2) = Entry(2,2) * tmp;
 280     Quot(2,3) = Entry(2,3) * tmp;
 281     Quot(3,1) = Entry(3,1) * tmp;
 282     Quot(3,2) = Entry(3,2) * tmp;
 283     Quot(3,3) = Entry(3,3) * tmp;
 284   } else {
 285     MatrixException mE;
 286     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 287     throw mE;
 288   }
 289   return Quot;
 290 }
 291
 292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 293
 294 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 295 {
 296   if ( scalar != 0 ) {
 297     double tmp = 1.0/scalar;
 298     Entry(1,1) *= tmp;
 299     Entry(1,2) *= tmp;
 300     Entry(1,3) *= tmp;
 301     Entry(2,1) *= tmp;
 302     Entry(2,2) *= tmp;
 303     Entry(2,3) *= tmp;
 304     Entry(3,1) *= tmp;
 305     Entry(3,2) *= tmp;
 306     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 *this;
 313 }
 314
 315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 316
 317 void FGMatrix33::T(void)
 318 {
 319   for (unsigned int i=1; i<=3; i++) {
 320     for (unsigned int j=i+1; j<=3; j++) {
 321       double tmp = Entry(i,j);
 322       Entry(i,j) = Entry(j,i);
 323       Entry(j,i) = tmp;
 324     }
 325   }
 326 }
 327
 328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 329
 330 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
 331   double tmp1 = v(1)*Entry(1,1);
 332   double tmp2 = v(1)*Entry(2,1);
 333   double tmp3 = v(1)*Entry(3,1);
 334
 335   tmp1 += v(2)*Entry(1,2);
 336   tmp2 += v(2)*Entry(2,2);
 337   tmp3 += v(2)*Entry(3,2);
 338
 339   tmp1 += v(3)*Entry(1,3);
 340   tmp2 += v(3)*Entry(2,3);
 341   tmp3 += v(3)*Entry(3,3);
 342
 343   return FGColumnVector3( tmp1, tmp2, tmp3 );
 344 }
 345
 346 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 347 //    The bitmasked value choices are as follows:
 348 //    unset: In this case (the default) JSBSim would only print
 349 //       out the normally expected messages, essentially echoing
 350 //       the config files as they are read. If the environment
 351 //       variable is not set, debug_lvl is set to 1 internally
 352 //    0: This requests JSBSim not to output any messages
 353 //       whatsoever.
 354 //    1: This value explicity requests the normal JSBSim
 355 //       startup messages
 356 //    2: This value asks for a message to be printed out when
 357 //       a class is instantiated
 358 //    4: When this value is set, a message is displayed when a
 359 //       FGModel object executes its Run() method
 360 //    8: When this value is set, various runtime state variables
 361 //       are printed out periodically
 362 //    16: When set various parameters are sanity checked and
 363 //       a message is printed out when they go out of bounds
 364
 365 void FGMatrix33::Debug(int from)
 366 {
 367   if (debug_lvl <= 0) return;
 368
 369   if (debug_lvl & 1) { // Standard console startup message output
 370     if (from == 0) { // Constructor
 371
 372     }
 373   }
 374   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 375     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 376     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 377   }
 378   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 379   }
 380   if (debug_lvl & 8 ) { // Runtime state variables
 381   }
 382   if (debug_lvl & 16) { // Sanity checking
 383   }
 384   if (debug_lvl & 64) {
 385     if (from == 0) { // Constructor
 386       cout << IdSrc << endl;
 387       cout << IdHdr << endl;
 388     }
 389   }
 390 }
 391 }