src/FDM/JSBSim/FGMatrix.cpp

   1 /*******************************************************************************
   2
   3 Module: FGMatrix.cpp
   4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
   5 Date started: 1998
   6 Purpose: FGMatrix 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 "FGMatrix.h"
  22
  23 /*******************************************************************************
  24 ************************************ CODE **************************************
  25 *******************************************************************************/
  26
  27 double** FGalloc(int rows, int cols)
  28 {
  29   double **A;
  30
  31   A = new double *[rows+1];
  32   if (!A)  return NULL;
  33
  34   for (int i=0; i <= rows; i++){
  35     A[i] = new double [cols+1];
  36     if (!A[i]) return NULL;
  37   }
  38   return A;
  39 }
  40
  41 /******************************************************************************/
  42
  43 void dealloc(double **A, int rows)
  44 {
  45   for(int i=0;i<= rows;i++) delete[] A[i];
  46   delete[] A;
  47 }
  48
  49 /******************************************************************************/
  50
  51 FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
  52 {
  53   data = FGalloc(rows,cols);
  54   InitMatrix();
  55   rowCtr = colCtr = 1;
  56 }
  57
  58 /******************************************************************************/
  59
  60 FGMatrix::FGMatrix(const FGMatrix& M)
  61 {
  62   data  = NULL;
  63   rowCtr = colCtr = 1;
  64   *this = M;
  65 }
  66
  67 /******************************************************************************/
  68
  69 FGMatrix::~FGMatrix(void)
  70 {
  71   dealloc(data,rows);
  72   rowCtr = colCtr = 1;
  73   rows = cols = 0;
  74 }
  75
  76 /******************************************************************************/
  77
  78 ostream& operator<<(ostream& os, const FGMatrix& M)
  79 {
  80   for (unsigned int i=1; i<=M.Rows(); i++) {
  81     for (unsigned int j=1; j<=M.Cols(); j++) {
  82       if (i == M.Rows() && j == M.Cols())
  83         os << M.data[i][j];
  84       else
  85         os << M.data[i][j] << ", ";
  86     }
  87   }
  88   return os;
  89 }
  90
  91 /******************************************************************************/
  92
  93 FGMatrix& FGMatrix::operator<<(const float ff)
  94 {
  95   data[rowCtr][colCtr] = ff;
  96   if (++colCtr > Cols()) {
  97     colCtr = 1;
  98     if (++rowCtr > Rows())
  99       rowCtr = 1;
 100   }
 101   return *this;
 102 }
 103
 104 /******************************************************************************/
 105
 106 istream& operator>>(istream& is, FGMatrix& M)
 107 {
 108   for (unsigned int i=1; i<=M.Rows(); i++) {
 109     for (unsigned int j=1; j<=M.Cols(); j++) {
 110       is >> M.data[i][j];
 111     }
 112   }
 113   return is;
 114 }
 115
 116 /******************************************************************************/
 117
 118 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
 119 {
 120   if (&M != this) {
 121     if (data != NULL) dealloc(data,rows);
 122
 123     width  = M.width;
 124     prec   = M.prec;
 125     delim  = M.delim;
 126     origin = M.origin;
 127     rows   = M.rows;
 128     cols   = M.cols;
 129
 130     data = FGalloc(rows,cols);
 131     for (unsigned int i=0; i<=rows; i++) {
 132       for (unsigned int j=0; j<=cols; j++) {
 133         data[i][j] = M.data[i][j];
 134       }
 135     }
 136   }
 137   return *this;
 138 }
 139
 140 /******************************************************************************/
 141
 142 unsigned int FGMatrix::Rows(void) const
 143 {
 144   return rows;
 145 }
 146
 147 /******************************************************************************/
 148
 149 unsigned int FGMatrix::Cols(void) const
 150 {
 151   return cols;
 152 }
 153
 154 /******************************************************************************/
 155
 156 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
 157 {
 158   FGMatrix::delim  = delim;
 159   FGMatrix::width  = width;
 160   FGMatrix::prec   = prec;
 161   FGMatrix::origin = origin;
 162 }
 163
 164 /******************************************************************************/
 165
 166 void FGMatrix::InitMatrix(double value)
 167 {
 168   if (data) {
 169     for (unsigned int i=0;i<=rows;i++) {
 170       for (unsigned int j=0;j<=cols;j++) {
 171         operator()(i,j) = value;
 172       }
 173     }
 174   }
 175 }
 176
 177 /******************************************************************************/
 178
 179 void FGMatrix::InitMatrix(void)
 180 {
 181   this->InitMatrix(0);
 182 }
 183
 184 // *****************************************************************************
 185 // binary operators ************************************************************
 186 // *****************************************************************************
 187
 188 FGMatrix FGMatrix::operator-(const FGMatrix& M)
 189 {
 190   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 191     MatrixException mE;
 192     mE.Message = "Invalid row/column match in Matrix operator -";
 193     throw mE;
 194   }
 195
 196   FGMatrix Diff(Rows(), Cols());
 197
 198   for (unsigned int i=1; i<=Rows(); i++) {
 199     for (unsigned int j=1; j<=Cols(); j++) {
 200       Diff(i,j) = data[i][j] - M(i,j);
 201     }
 202   }
 203   return Diff;
 204 }
 205
 206 /******************************************************************************/
 207
 208 void FGMatrix::operator-=(const FGMatrix &M)
 209 {
 210   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 211     MatrixException mE;
 212     mE.Message = "Invalid row/column match in Matrix operator -=";
 213     throw mE;
 214   }
 215
 216   for (unsigned int i=1; i<=Rows(); i++) {
 217     for (unsigned int j=1; j<=Cols(); j++) {
 218       data[i][j] -= M(i,j);
 219     }
 220   }
 221 }
 222
 223 /******************************************************************************/
 224
 225 FGMatrix FGMatrix::operator+(const FGMatrix& M)
 226 {
 227   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 228     MatrixException mE;
 229     mE.Message = "Invalid row/column match in Matrix operator +";
 230     throw mE;
 231   }
 232
 233   FGMatrix Sum(Rows(), Cols());
 234
 235   for (unsigned int i=1; i<=Rows(); i++) {
 236     for (unsigned int j=1; j<=Cols(); j++) {
 237       Sum(i,j) = data[i][j] + M(i,j);
 238     }
 239   }
 240   return Sum;
 241 }
 242
 243 /******************************************************************************/
 244
 245 void FGMatrix::operator+=(const FGMatrix &M)
 246 {
 247   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 248     MatrixException mE;
 249     mE.Message = "Invalid row/column match in Matrix operator +=";
 250     throw mE;
 251   }
 252
 253   for (unsigned int i=1; i<=Rows(); i++) {
 254     for (unsigned int j=1; j<=Cols(); j++) {
 255       data[i][j]+=M(i,j);
 256     }
 257   }
 258 }
 259
 260 /******************************************************************************/
 261
 262 FGMatrix operator*(double scalar, FGMatrix &M)
 263 {
 264   FGMatrix Product(M.Rows(), M.Cols());
 265
 266   for (unsigned int i=1; i<=M.Rows(); i++) {
 267     for (unsigned int j=1; j<=M.Cols(); j++) {
 268       Product(i,j) = scalar*M(i,j);
 269     }
 270   }
 271   return Product;
 272 }
 273
 274 /******************************************************************************/
 275
 276 void FGMatrix::operator*=(const double scalar)
 277 {
 278   for (unsigned int i=1; i<=Rows(); i++) {
 279     for (unsigned int j=1; j<=Cols(); j++) {
 280       data[i][j] *= scalar;
 281     }
 282   }
 283 }
 284
 285 /******************************************************************************/
 286
 287 FGMatrix FGMatrix::operator*(const FGMatrix& M)
 288 {
 289   if (Cols() != M.Rows()) {
 290     MatrixException mE;
 291     mE.Message = "Invalid row/column match in Matrix operator *";
 292     throw mE;
 293   }
 294
 295   FGMatrix Product(Rows(), M.Cols());
 296
 297   for (unsigned int i=1; i<=Rows(); i++) {
 298     for (unsigned int j=1; j<=M.Cols(); j++)  {
 299       Product(i,j) = 0;
 300       for (unsigned int k=1; k<=Cols(); k++) {
 301          Product(i,j) += data[i][k] * M(k,j);
 302       }
 303     }
 304   }
 305   return Product;
 306 }
 307
 308 /******************************************************************************/
 309
 310 void FGMatrix::operator*=(const FGMatrix& M)
 311 {
 312   if (Cols() != M.Rows()) {
 313     MatrixException mE;
 314     mE.Message = "Invalid row/column match in Matrix operator *=";
 315     throw mE;
 316   }
 317
 318   double **prod;
 319
 320   prod = FGalloc(Rows(), M.Cols());
 321   for (unsigned int i=1; i<=Rows(); i++) {
 322     for (unsigned int j=1; j<=M.Cols(); j++) {
 323       prod[i][j] = 0;
 324       for (unsigned int k=1; k<=Cols(); k++) {
 325         prod[i][j] += data[i][k] * M(k,j);
 326       }
 327     }
 328   }
 329   dealloc(data, Rows());
 330   data = prod;
 331   cols = M.cols;
 332 }
 333
 334 /******************************************************************************/
 335
 336 FGMatrix FGMatrix::operator/(const double scalar)
 337 {
 338   FGMatrix Quot(Rows(), Cols());
 339
 340   if(scalar != 0) {
 341     for (unsigned int i=1; i<=Rows(); i++) {
 342       for (unsigned int j=1; j<=Cols(); j++)  {
 343          Quot(i,j) = data[i][j]/scalar;
 344       }
 345     }
 346
 347   } else
 348     cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl;
 349   return Quot;
 350 }
 351
 352 /******************************************************************************/
 353
 354 void FGMatrix::operator/=(const double scalar)
 355 {
 356
 357   if(scalar != 0) {
 358     for (unsigned int i=1; i<=Rows(); i++)  {
 359       for (unsigned int j=1; j<=Cols(); j++) {
 360         data[i][j]/=scalar;
 361       }
 362     }
 363   } else
 364     cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl;
 365 }
 366
 367 /******************************************************************************/
 368
 369 void FGMatrix::T(void)
 370 {
 371   if (rows==cols)
 372     TransposeSquare();
 373   else
 374     TransposeNonSquare();
 375 }
 376
 377 /******************************************************************************/
 378
 379 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
 380 {
 381   FGColumnVector Product(Col.Rows());
 382
 383   if (Cols() != Col.Rows()) {
 384     MatrixException mE;
 385     mE.Message = "Invalid row/column match in Column Vector operator *";
 386     throw mE;
 387   }
 388
 389   for (unsigned int i=1;i<=Rows();i++) {
 390     Product(i) = 0.00;
 391     for (unsigned int j=1;j<=Cols();j++) {
 392       Product(i) += Col(j)*data[i][j];
 393     }
 394   }
 395   return Product;
 396 }
 397
 398 /******************************************************************************/
 399
 400 void FGMatrix::TransposeSquare(void)
 401 {
 402   for (unsigned int i=1; i<=rows; i++) {
 403     for (unsigned int j=i+1; j<=cols; 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 void FGMatrix::TransposeNonSquare(void)
 414 {
 415   double **tran;
 416
 417   tran = FGalloc(rows,cols);
 418
 419   for (unsigned int i=1; i<=rows; i++) {
 420     for (unsigned int j=1; j<=cols; j++) {
 421       tran[j][i] = data[i][j];
 422     }
 423   }
 424
 425   dealloc(data,rows);
 426
 427   data       = tran;
 428   unsigned int m = rows;
 429   rows       = cols;
 430   cols       = m;
 431 }
 432
 433 /******************************************************************************/
 434
 435 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
 436 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
 437 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
 438 FGColumnVector::~FGColumnVector() { }
 439
 440 /******************************************************************************/
 441
 442 double& FGColumnVector::operator()(int m) const
 443 {
 444   return FGMatrix::operator()(m,1);
 445 }
 446
 447 /******************************************************************************/
 448
 449 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
 450 {
 451   FGColumnVector Product(Col.Rows());
 452
 453   if (Mat.Cols() != Col.Rows()) {
 454     MatrixException mE;
 455     mE.Message = "Invalid row/column match in Column Vector operator *";
 456     throw mE;
 457   }
 458
 459   for (unsigned int i=1;i<=Mat.Rows();i++) {
 460     Product(i) = 0.00;
 461     for (unsigned int j=1;j<=Mat.Cols();j++) {
 462       Product(i) += Col(j)*Mat(i,j);
 463     }
 464   }
 465
 466   return Product;
 467 }
 468
 469 /******************************************************************************/
 470
 471 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
 472 {
 473   if (Rows() != C.Rows()) {
 474     MatrixException mE;
 475     mE.Message = "Invalid row/column match in Column Vector operator *";
 476     throw mE;
 477   }
 478
 479   FGColumnVector Sum(C.Rows());
 480
 481   for (unsigned int i=1; i<=C.Rows(); i++) {
 482     Sum(i) = C(i) + data[i][1];
 483   }
 484
 485   return Sum;
 486 }
 487
 488 /******************************************************************************/
 489
 490 FGColumnVector FGColumnVector::operator*(const double scalar)
 491 {
 492   FGColumnVector Product(Rows());
 493
 494   for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
 495
 496   return Product;
 497 }
 498
 499 /******************************************************************************/
 500
 501 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
 502 {
 503   if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
 504     MatrixException mE;
 505     mE.Message = "Invalid row/column match in Column Vector operator -";
 506     throw mE;
 507   }
 508
 509   FGColumnVector Diff(Rows());
 510
 511   for (unsigned int i=1; i<=Rows(); i++) {
 512     Diff(i) = data[i][1] - V(i);
 513   }
 514
 515   return Diff;
 516 }
 517
 518 /******************************************************************************/
 519
 520 FGColumnVector FGColumnVector::operator/(const double scalar)
 521 {
 522   FGColumnVector Quotient(Rows());
 523   if(scalar != 0) {
 524
 525
 526     for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
 527
 528   } else
 529     cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl;
 530   return Quotient;
 531
 532
 533 }
 534
 535 /******************************************************************************/
 536
 537 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
 538 {
 539   FGColumnVector Product(C.Rows());
 540
 541   for (unsigned int i=1; i<=C.Rows(); i++) {
 542      Product(i) = scalar * C(i);
 543   }
 544
 545   return Product;
 546 }
 547
 548 /******************************************************************************/
 549
 550 float FGColumnVector::Magnitude(void)
 551 {
 552   double num=0.0;
 553
 554   if ((data[1][1] == 0.00) &&
 555       (data[2][1] == 0.00) &&
 556       (data[3][1] == 0.00))
 557   {
 558     return 0.00;
 559   } else {
 560     for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
 561     return sqrt(num);
 562   }
 563 }
 564
 565 /******************************************************************************/
 566
 567 FGColumnVector FGColumnVector::Normalize(void)
 568 {
 569   double Mag = Magnitude();
 570
 571   if (Mag != 0) {
 572     for (unsigned int i=1; i<=Rows(); i++)
 573       for (unsigned int j=1; j<=Cols(); j++)
 574         data[i][j] = data[i][j]/Mag;
 575   }
 576
 577   return *this;
 578 }
 579
 580 /******************************************************************************/
 581
 582 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
 583 {
 584   if (Rows() != 3 || V.Rows() != 3) {
 585     MatrixException mE;
 586     mE.Message = "Invalid row count in vector cross product function";
 587     throw mE;
 588   }
 589
 590   FGColumnVector Product(3);
 591
 592   Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
 593   Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
 594   Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
 595
 596   return Product;
 597 }
 598
 599 /******************************************************************************/
 600
 601 FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
 602 {
 603   if (Rows() != 3 || V.Rows() != 3) {
 604     MatrixException mE;
 605     mE.Message = "Invalid row count in vector cross product function";
 606     throw mE;
 607   }
 608
 609   FGColumnVector Product(3);
 610
 611   Product(1) = data[1][1] * V(1);
 612   Product(2) = data[2][1] * V(2);
 613   Product(3) = data[3][1] * V(3);
 614
 615   return Product;
 616 }
 617
 618 /******************************************************************************/