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   }
 348   return Quot;
 349 }
 350
 351 /******************************************************************************/
 352
 353 void FGMatrix::operator/=(const double scalar)
 354 {
 355
 356   if(scalar != 0) {
 357     for (unsigned int i=1; i<=Rows(); i++)  {
 358       for (unsigned int j=1; j<=Cols(); j++) {
 359         data[i][j]/=scalar;
 360       }
 361     }
 362   }
 363 }
 364
 365 /******************************************************************************/
 366
 367 void FGMatrix::T(void)
 368 {
 369   if (rows==cols)
 370     TransposeSquare();
 371   else
 372     TransposeNonSquare();
 373 }
 374
 375 /******************************************************************************/
 376
 377 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
 378 {
 379   FGColumnVector Product(Col.Rows());
 380
 381   if (Cols() != Col.Rows()) {
 382     MatrixException mE;
 383     mE.Message = "Invalid row/column match in Column Vector operator *";
 384     throw mE;
 385   }
 386
 387   for (unsigned int i=1;i<=Rows();i++) {
 388     Product(i) = 0.00;
 389     for (unsigned int j=1;j<=Cols();j++) {
 390       Product(i) += Col(j)*data[i][j];
 391     }
 392   }
 393   return Product;
 394 }
 395
 396 /******************************************************************************/
 397
 398 void FGMatrix::TransposeSquare(void)
 399 {
 400   for (unsigned int i=1; i<=rows; i++) {
 401     for (unsigned int j=i+1; j<=cols; j++) {
 402       double tmp = data[i][j];
 403       data[i][j] = data[j][i];
 404       data[j][i] = tmp;
 405     }
 406   }
 407 }
 408
 409 /******************************************************************************/
 410
 411 void FGMatrix::TransposeNonSquare(void)
 412 {
 413   double **tran;
 414
 415   tran = FGalloc(rows,cols);
 416
 417   for (unsigned int i=1; i<=rows; i++) {
 418     for (unsigned int j=1; j<=cols; j++) {
 419       tran[j][i] = data[i][j];
 420     }
 421   }
 422
 423   dealloc(data,rows);
 424
 425   data       = tran;
 426   unsigned int m = rows;
 427   rows       = cols;
 428   cols       = m;
 429 }
 430
 431 /******************************************************************************/
 432
 433 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
 434 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
 435 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
 436 FGColumnVector::~FGColumnVector() { }
 437
 438 /******************************************************************************/
 439
 440 double& FGColumnVector::operator()(int m) const
 441 {
 442   return FGMatrix::operator()(m,1);
 443 }
 444
 445 /******************************************************************************/
 446
 447 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
 448 {
 449   FGColumnVector Product(Col.Rows());
 450
 451   if (Mat.Cols() != Col.Rows()) {
 452     MatrixException mE;
 453     mE.Message = "Invalid row/column match in Column Vector operator *";
 454     throw mE;
 455   }
 456
 457   for (unsigned int i=1;i<=Mat.Rows();i++) {
 458     Product(i) = 0.00;
 459     for (unsigned int j=1;j<=Mat.Cols();j++) {
 460       Product(i) += Col(j)*Mat(i,j);
 461     }
 462   }
 463
 464   return Product;
 465 }
 466
 467 /******************************************************************************/
 468
 469 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
 470 {
 471   if (Rows() != C.Rows()) {
 472     MatrixException mE;
 473     mE.Message = "Invalid row/column match in Column Vector operator *";
 474     throw mE;
 475   }
 476
 477   FGColumnVector Sum(C.Rows());
 478
 479   for (unsigned int i=1; i<=C.Rows(); i++) {
 480     Sum(i) = C(i) + data[i][1];
 481   }
 482
 483   return Sum;
 484 }
 485
 486 /******************************************************************************/
 487
 488 FGColumnVector FGColumnVector::operator*(const double scalar)
 489 {
 490   FGColumnVector Product(Rows());
 491
 492   for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
 493
 494   return Product;
 495 }
 496
 497 /******************************************************************************/
 498
 499 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
 500 {
 501   if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
 502     MatrixException mE;
 503     mE.Message = "Invalid row/column match in Column Vector operator -";
 504     throw mE;
 505   }
 506
 507   FGColumnVector Diff(Rows());
 508
 509   for (unsigned int i=1; i<=Rows(); i++) {
 510     Diff(i) = data[i][1] - V(i);
 511   }
 512
 513   return Diff;
 514 }
 515
 516 /******************************************************************************/
 517
 518 FGColumnVector FGColumnVector::operator/(const double scalar)
 519 {
 520   FGColumnVector Quotient(Rows());
 521   if(scalar != 0) {
 522     for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
 523   }
 524   return Quotient;
 525 }
 526
 527 /******************************************************************************/
 528
 529 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
 530 {
 531   FGColumnVector Product(C.Rows());
 532
 533   for (unsigned int i=1; i<=C.Rows(); i++) {
 534      Product(i) = scalar * C(i);
 535   }
 536
 537   return Product;
 538 }
 539
 540 /******************************************************************************/
 541
 542 float FGColumnVector::Magnitude(void)
 543 {
 544   double num=0.0;
 545
 546   if ((data[1][1] == 0.00) &&
 547       (data[2][1] == 0.00) &&
 548       (data[3][1] == 0.00))
 549   {
 550     return 0.00;
 551   } else {
 552     for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
 553     return sqrt(num);
 554   }
 555 }
 556
 557 /******************************************************************************/
 558
 559 FGColumnVector FGColumnVector::Normalize(void)
 560 {
 561   double Mag = Magnitude();
 562
 563   if (Mag != 0) {
 564     for (unsigned int i=1; i<=Rows(); i++)
 565       for (unsigned int j=1; j<=Cols(); j++)
 566         data[i][j] = data[i][j]/Mag;
 567   }
 568
 569   return *this;
 570 }
 571
 572 /******************************************************************************/
 573
 574 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
 575 {
 576   if (Rows() != 3 || V.Rows() != 3) {
 577     MatrixException mE;
 578     mE.Message = "Invalid row count in vector cross product function";
 579     throw mE;
 580   }
 581
 582   FGColumnVector Product(3);
 583
 584   Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
 585   Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
 586   Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
 587
 588   return Product;
 589 }
 590
 591 /******************************************************************************/
 592
 593 FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
 594 {
 595   if (Rows() != 3 || V.Rows() != 3) {
 596     MatrixException mE;
 597     mE.Message = "Invalid row count in vector cross product function";
 598     throw mE;
 599   }
 600
 601   FGColumnVector Product(3);
 602
 603   Product(1) = data[1][1] * V(1);
 604   Product(2) = data[2][1] * V(2);
 605   Product(3) = data[3][1] * V(3);
 606
 607   return Product;
 608 }
 609
 610 /******************************************************************************/