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