src/FDM/JSBSim/FGMatrix33.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Module: FGMatrix33.cpp
   4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
   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 static const char *IdSrc = "$Id$";
  25 static const char *IdHdr = ID_MATRIX33;
  26
  27 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  28 CLASS IMPLEMENTATION
  29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  30
  31 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  32
  33 FGMatrix33::FGMatrix33(void)
  34 {
  35   InitMatrix();
  36   rowCtr = colCtr = 1;
  37
  38   Debug(0);
  39 }
  40
  41 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  42
  43 FGMatrix33::FGMatrix33(int r, int c)
  44 {
  45   InitMatrix();
  46   rowCtr = colCtr = 1;
  47
  48   Debug(0);
  49 }
  50
  51 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52
  53 FGMatrix33::FGMatrix33(const FGMatrix33& M)
  54 {
  55   rowCtr = colCtr = 1;
  56
  57   data[1][1] = M.data[1][1];
  58   data[1][2] = M.data[1][2];
  59   data[1][3] = M.data[1][3];
  60   data[2][1] = M.data[2][1];
  61   data[2][2] = M.data[2][2];
  62   data[2][3] = M.data[2][3];
  63   data[3][1] = M.data[3][1];
  64   data[3][2] = M.data[3][2];
  65   data[3][3] = M.data[3][3];
  66
  67   Debug(0);
  68 }
  69
  70 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  71
  72 FGMatrix33::~FGMatrix33(void)
  73 {
  74   rowCtr = colCtr = 1;
  75
  76   Debug(1);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 ostream& operator<<(ostream& os, const FGMatrix33& M)
  82 {
  83   for (unsigned int i=1; i<=M.Rows(); i++) {
  84     for (unsigned int j=1; j<=M.Cols(); j++) {
  85       if (i == M.Rows() && j == M.Cols())
  86         os << M.data[i][j];
  87       else
  88         os << M.data[i][j] << ", ";
  89     }
  90   }
  91   return os;
  92 }
  93
  94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  95
  96 FGMatrix33& FGMatrix33::operator<<(const double ff)
  97 {
  98   data[rowCtr][colCtr] = ff;
  99   if (++colCtr > Cols()) {
 100     colCtr = 1;
 101     if (++rowCtr > Rows())
 102       rowCtr = 1;
 103   }
 104   return *this;
 105 }
 106
 107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 108
 109 istream& operator>>(istream& is, FGMatrix33& M)
 110 {
 111   for (unsigned int i=1; i<=M.Rows(); i++) {
 112     for (unsigned int j=1; j<=M.Cols(); j++) {
 113       is >> M.data[i][j];
 114     }
 115   }
 116   return is;
 117 }
 118
 119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 120
 121 FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
 122 {
 123   if (&M != this) {
 124     data[1][1] = M.data[1][1];
 125     data[1][2] = M.data[1][2];
 126     data[1][3] = M.data[1][3];
 127     data[2][1] = M.data[2][1];
 128     data[2][2] = M.data[2][2];
 129     data[2][3] = M.data[2][3];
 130     data[3][1] = M.data[3][1];
 131     data[3][2] = M.data[3][2];
 132     data[3][3] = M.data[3][3];
 133   }
 134   return *this;
 135 }
 136
 137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 138
 139 void FGMatrix33::InitMatrix(double value)
 140 {
 141   if (data) {
 142     data[1][1] = value;
 143     data[1][2] = value;
 144     data[1][3] = value;
 145     data[2][1] = value;
 146     data[2][2] = value;
 147     data[2][3] = value;
 148     data[3][1] = value;
 149     data[3][2] = value;
 150     data[3][3] = value;
 151   }
 152 }
 153
 154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 155
 156 void FGMatrix33::InitMatrix(void)
 157 {
 158   this->InitMatrix(0);
 159 }
 160
 161 // *****************************************************************************
 162 // binary operators ************************************************************
 163 // *****************************************************************************
 164
 165 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
 166 {
 167   FGMatrix33 Diff;
 168
 169   Diff(1,1) = data[1][1] - M(1,1);
 170   Diff(1,2) = data[1][2] - M(1,2);
 171   Diff(1,3) = data[1][3] - M(1,3);
 172   Diff(2,1) = data[2][1] - M(2,1);
 173   Diff(2,2) = data[2][2] - M(2,2);
 174   Diff(2,3) = data[2][3] - M(2,3);
 175   Diff(3,1) = data[3][1] - M(3,1);
 176   Diff(3,2) = data[3][2] - M(3,2);
 177   Diff(3,3) = data[3][3] - M(3,3);
 178
 179   return Diff;
 180 }
 181
 182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 183
 184 void FGMatrix33::operator-=(const FGMatrix33 &M)
 185 {
 186   data[1][1] -= M(1,1);
 187   data[1][2] -= M(1,2);
 188   data[1][3] -= M(1,3);
 189   data[2][1] -= M(2,1);
 190   data[2][2] -= M(2,2);
 191   data[2][3] -= M(2,3);
 192   data[3][1] -= M(3,1);
 193   data[3][2] -= M(3,2);
 194   data[3][3] -= M(3,3);
 195 }
 196
 197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 198
 199 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
 200 {
 201   FGMatrix33 Sum;
 202
 203   Sum(1,1) = data[1][1] + M(1,1);
 204   Sum(1,2) = data[1][2] + M(1,2);
 205   Sum(1,3) = data[1][3] + M(1,3);
 206   Sum(2,1) = data[2][1] + M(2,1);
 207   Sum(2,2) = data[2][2] + M(2,2);
 208   Sum(2,3) = data[2][3] + M(2,3);
 209   Sum(3,1) = data[3][1] + M(3,1);
 210   Sum(3,2) = data[3][2] + M(3,2);
 211   Sum(3,3) = data[3][3] + M(3,3);
 212
 213   return Sum;
 214 }
 215
 216 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 217
 218 void FGMatrix33::operator+=(const FGMatrix33 &M)
 219 {
 220   data[1][1] += M(1,1);
 221   data[1][2] += M(1,2);
 222   data[1][3] += M(1,3);
 223   data[2][1] += M(2,1);
 224   data[2][2] += M(2,2);
 225   data[2][3] += M(2,3);
 226   data[3][1] += M(3,1);
 227   data[3][2] += M(3,2);
 228   data[3][3] += M(3,3);
 229 }
 230
 231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 232
 233 FGMatrix33 FGMatrix33::operator*(const double scalar)
 234 {
 235   FGMatrix33 Product;
 236
 237   Product(1,1) = data[1][1] * scalar;
 238   Product(1,2) = data[1][2] * scalar;
 239   Product(1,3) = data[1][3] * scalar;
 240   Product(2,1) = data[2][1] * scalar;
 241   Product(2,2) = data[2][2] * scalar;
 242   Product(2,3) = data[2][3] * scalar;
 243   Product(3,1) = data[3][1] * scalar;
 244   Product(3,2) = data[3][2] * scalar;
 245   Product(3,3) = data[3][3] * scalar;
 246
 247   return Product;
 248 }
 249
 250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 251
 252 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 253 {
 254   FGMatrix33 Product;
 255
 256   Product(1,1) = M(1,1) * scalar;
 257   Product(1,2) = M(1,2) * scalar;
 258   Product(1,3) = M(1,3) * scalar;
 259   Product(2,1) = M(2,1) * scalar;
 260   Product(2,2) = M(2,2) * scalar;
 261   Product(2,3) = M(2,3) * scalar;
 262   Product(3,1) = M(3,1) * scalar;
 263   Product(3,2) = M(3,2) * scalar;
 264   Product(3,3) = M(3,3) * scalar;
 265
 266   return Product;
 267 }
 268
 269 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 270
 271 void FGMatrix33::operator*=(const double scalar)
 272 {
 273   data[1][1] *= scalar;
 274   data[1][2] *= scalar;
 275   data[1][3] *= scalar;
 276   data[2][1] *= scalar;
 277   data[2][2] *= scalar;
 278   data[2][3] *= scalar;
 279   data[3][1] *= scalar;
 280   data[3][2] *= scalar;
 281   data[3][3] *= scalar;
 282 }
 283
 284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 285
 286 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
 287 {
 288   FGMatrix33 Product;
 289
 290   Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
 291   Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
 292   Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
 293   Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
 294   Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
 295   Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
 296   Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
 297   Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
 298   Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
 299
 300   return Product;
 301 }
 302
 303 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 304
 305 void FGMatrix33::operator*=(const FGMatrix33& M)
 306 {
 307   double a,b,c;
 308
 309   a = data[1][1]; b=data[1][2]; c=data[1][3];
 310   data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
 311   data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
 312   data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
 313
 314   a = data[2][1]; b=data[2][2]; c=data[2][3];
 315   data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
 316   data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
 317   data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
 318
 319   a = data[3][1]; b=data[3][2]; c=data[3][3];
 320   data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
 321   data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
 322   data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
 323 }
 324
 325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 326
 327 FGMatrix33 FGMatrix33::operator/(const double scalar)
 328 {
 329   FGMatrix33 Quot;
 330
 331   if ( scalar != 0 ) {
 332           double tmp = 1.0/scalar;
 333     Quot(1,1) = data[1][1] * tmp;
 334     Quot(1,2) = data[1][2] * tmp;
 335     Quot(1,3) = data[1][3] * tmp;
 336     Quot(2,1) = data[2][1] * tmp;
 337     Quot(2,2) = data[2][2] * tmp;
 338     Quot(2,3) = data[2][3] * tmp;
 339     Quot(3,1) = data[3][1] * tmp;
 340     Quot(3,2) = data[3][2] * tmp;
 341     Quot(3,3) = data[3][3] * tmp;
 342   } else {
 343     MatrixException mE;
 344     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 345     throw mE;
 346   }
 347   return Quot;
 348 }
 349
 350 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 351
 352 void FGMatrix33::operator/=(const double scalar)
 353 {
 354   if ( scalar != 0 ) {
 355     double tmp = 1.0/scalar;
 356     data[1][1] *= tmp;
 357     data[1][2] *= tmp;
 358     data[1][3] *= tmp;
 359     data[2][1] *= tmp;
 360     data[2][2] *= tmp;
 361     data[2][3] *= tmp;
 362     data[3][1] *= tmp;
 363     data[3][2] *= tmp;
 364     data[3][3] *= tmp;
 365   } else {
 366     MatrixException mE;
 367     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 368     throw mE;
 369   }
 370 }
 371
 372 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 373
 374 void FGMatrix33::T(void)
 375 {
 376   for (unsigned int i=1; i<=3; i++) {
 377     for (unsigned int j=i+1; j<=3; j++) {
 378       double tmp = data[i][j];
 379       data[i][j] = data[j][i];
 380       data[j][i] = tmp;
 381     }
 382   }
 383 }
 384
 385 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 386
 387 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
 388 {
 389   FGColumnVector3 Product;
 390
 391   Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
 392   Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
 393   Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
 394
 395   return Product;
 396 }
 397
 398 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 399 //    The bitmasked value choices are as follows:
 400 //    unset: In this case (the default) JSBSim would only print
 401 //       out the normally expected messages, essentially echoing
 402 //       the config files as they are read. If the environment
 403 //       variable is not set, debug_lvl is set to 1 internally
 404 //    0: This requests JSBSim not to output any messages
 405 //       whatsoever.
 406 //    1: This value explicity requests the normal JSBSim
 407 //       startup messages
 408 //    2: This value asks for a message to be printed out when
 409 //       a class is instantiated
 410 //    4: When this value is set, a message is displayed when a
 411 //       FGModel object executes its Run() method
 412 //    8: When this value is set, various runtime state variables
 413 //       are printed out periodically
 414 //    16: When set various parameters are sanity checked and
 415 //       a message is printed out when they go out of bounds
 416
 417 void FGMatrix33::Debug(int from)
 418 {
 419   if (debug_lvl <= 0) return;
 420
 421   if (debug_lvl & 1) { // Standard console startup message output
 422     if (from == 0) { // Constructor
 423
 424     }
 425   }
 426   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 427     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 428     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 429   }
 430   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 431   }
 432   if (debug_lvl & 8 ) { // Runtime state variables
 433   }
 434   if (debug_lvl & 16) { // Sanity checking
 435   }
 436   if (debug_lvl & 64) {
 437     if (from == 0) { // Constructor
 438       cout << IdSrc << endl;
 439       cout << IdHdr << endl;
 440     }
 441   }
 442 }
 443