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