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