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