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   rowCtr = colCtr = 1;
  55 }
  56
  57 /******************************************************************************/
  58
  59 FGMatrix::FGMatrix(const FGMatrix& M)
  60 {
  61   data  = NULL;
  62   rowCtr = colCtr = 1;
  63   *this = M;
  64 }
  65
  66 /******************************************************************************/
  67
  68 FGMatrix::~FGMatrix(void)
  69 {
  70   dealloc(data,rows);
  71   rowCtr = colCtr = 1;
  72   rows = cols = 0;
  73 }
  74
  75 /******************************************************************************/
  76
  77 ostream& operator<<(ostream& os, const FGMatrix& M)
  78 {
  79   for (unsigned int i=1; i<=M.Rows(); i++) {
  80     for (unsigned int j=1; j<=M.Cols(); j++) {
  81       if (i == M.Rows() && j == M.Cols())
  82         os << M.data[i][j];
  83       else
  84         os << M.data[i][j] << ", ";
  85     }
  86   }
  87   return os;
  88 }
  89
  90 /******************************************************************************/
  91
  92 FGMatrix& FGMatrix::operator<<(const float ff)
  93 {
  94   data[rowCtr][colCtr] = ff;
  95   if (++colCtr > Cols()) {
  96     colCtr = 1;
  97     if (++rowCtr > Rows())
  98       rowCtr = 1;
  99   }
 100   return *this;
 101 }
 102
 103 /******************************************************************************/
 104
 105 istream& operator>>(istream& is, FGMatrix& M)
 106 {
 107   for (unsigned int i=1; i<=M.Rows(); i++) {
 108     for (unsigned int j=1; j<=M.Cols(); j++) {
 109       is >> M.data[i][j];
 110     }
 111   }
 112   return is;
 113 }
 114
 115 /******************************************************************************/
 116
 117 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
 118 {
 119   if (&M != this) {
 120     if (data != NULL) dealloc(data,rows);
 121
 122     width  = M.width;
 123     prec   = M.prec;
 124     delim  = M.delim;
 125     origin = M.origin;
 126     rows   = M.rows;
 127     cols   = M.cols;
 128
 129     data = FGalloc(rows,cols);
 130     for (unsigned int i=0; i<=rows; i++) {
 131       for (unsigned int j=0; j<=cols; j++) {
 132         data[i][j] = M.data[i][j];
 133       }
 134     }
 135   }
 136   return *this;
 137 }
 138
 139 /******************************************************************************/
 140
 141 unsigned int FGMatrix::Rows(void) const
 142 {
 143   return rows;
 144 }
 145
 146 /******************************************************************************/
 147
 148 unsigned int FGMatrix::Cols(void) const
 149 {
 150   return cols;
 151 }
 152
 153 /******************************************************************************/
 154
 155 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
 156 {
 157   FGMatrix::delim  = delim;
 158   FGMatrix::width  = width;
 159   FGMatrix::prec   = prec;
 160   FGMatrix::origin = origin;
 161 }
 162
 163 /******************************************************************************/
 164
 165 void FGMatrix::InitMatrix(double value)
 166 {
 167   if (data) {
 168     for (unsigned int i=0;i<=rows;i++) {
 169       for (unsigned int j=0;j<=cols;j++) {
 170         operator()(i,j) = value;
 171       }
 172     }
 173   }
 174 }
 175
 176 /******************************************************************************/
 177
 178 void FGMatrix::InitMatrix(void)
 179 {
 180   this->InitMatrix(0);
 181 }
 182
 183 // *****************************************************************************
 184 // binary operators ************************************************************
 185 // *****************************************************************************
 186
 187 FGMatrix FGMatrix::operator-(const FGMatrix& M)
 188 {
 189   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 190     MatrixException mE;
 191     mE.Message = "Invalid row/column match in Matrix operator -";
 192     throw mE;
 193   }
 194
 195   FGMatrix Diff(Rows(), Cols());
 196
 197   for (unsigned int i=1; i<=Rows(); i++) {
 198     for (unsigned int j=1; j<=Cols(); j++) {
 199       Diff(i,j) = (*this)(i,j) - M(i,j);
 200     }
 201   }
 202   return Diff;
 203 }
 204
 205 /******************************************************************************/
 206
 207 void FGMatrix::operator-=(const FGMatrix &M)
 208 {
 209   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 210     MatrixException mE;
 211     mE.Message = "Invalid row/column match in Matrix operator -=";
 212     throw mE;
 213   }
 214
 215   for (unsigned int i=1; i<=Rows(); i++) {
 216     for (unsigned int j=1; j<=Cols(); j++) {
 217       (*this)(i,j) -= M(i,j);
 218     }
 219   }
 220 }
 221
 222 /******************************************************************************/
 223
 224 FGMatrix FGMatrix::operator+(const FGMatrix& M)
 225 {
 226   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 227     MatrixException mE;
 228     mE.Message = "Invalid row/column match in Matrix operator +";
 229     throw mE;
 230   }
 231
 232   FGMatrix Sum(Rows(), Cols());
 233
 234   for (unsigned int i=1; i<=Rows(); i++) {
 235     for (unsigned int j=1; j<=Cols(); j++) {
 236       Sum(i,j) = (*this)(i,j) + M(i,j);
 237     }
 238   }
 239   return Sum;
 240 }
 241
 242 /******************************************************************************/
 243
 244 void FGMatrix::operator+=(const FGMatrix &M)
 245 {
 246   if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
 247     MatrixException mE;
 248     mE.Message = "Invalid row/column match in Matrix operator +=";
 249     throw mE;
 250   }
 251
 252   for (unsigned int i=1; i<=Rows(); i++) {
 253     for (unsigned int j=1; j<=Cols(); j++) {
 254       (*this)(i,j)+=M(i,j);
 255     }
 256   }
 257 }
 258
 259 /******************************************************************************/
 260
 261 FGMatrix operator*(double scalar, FGMatrix &M)
 262 {
 263   FGMatrix Product(M.Rows(), M.Cols());
 264
 265   for (unsigned int i=1; i<=M.Rows(); i++) {
 266     for (unsigned int j=1; j<=M.Cols(); j++) {
 267       Product(i,j) = scalar*M(i,j);
 268     }
 269   }
 270   return Product;
 271 }
 272
 273 /******************************************************************************/
 274
 275 void FGMatrix::operator*=(const double scalar)
 276 {
 277   for (unsigned int i=1; i<=Rows(); i++) {
 278     for (unsigned int j=1; j<=Cols(); j++) {
 279       (*this)(i,j) *= scalar;
 280     }
 281   }
 282 }
 283
 284 /******************************************************************************/
 285
 286 FGMatrix FGMatrix::operator*(const FGMatrix& M)
 287 {
 288   if (Cols() != M.Rows()) {
 289     MatrixException mE;
 290     mE.Message = "Invalid row/column match in Matrix operator *";
 291     throw mE;
 292   }
 293
 294   FGMatrix Product(Rows(), M.Cols());
 295
 296   for (unsigned int i=1; i<=Rows(); i++) {
 297     for (unsigned int j=1; j<=M.Cols(); j++)  {
 298       Product(i,j) = 0;
 299       for (unsigned int k=1; k<=Cols(); k++) {
 300          Product(i,j) += (*this)(i,k) * M(k,j);
 301       }
 302     }
 303   }
 304   return Product;
 305 }
 306
 307 /******************************************************************************/
 308
 309 void FGMatrix::operator*=(const FGMatrix& M)
 310 {
 311   if (Cols() != M.Rows()) {
 312     MatrixException mE;
 313     mE.Message = "Invalid row/column match in Matrix operator *=";
 314     throw mE;
 315   }
 316
 317   double **prod;
 318
 319   prod = FGalloc(Rows(), M.Cols());
 320   for (unsigned int i=1; i<=Rows(); i++) {
 321     for (unsigned int j=1; j<=M.Cols(); j++) {
 322       prod[i][j] = 0;
 323       for (unsigned int k=1; k<=Cols(); k++) {
 324         prod[i][j] += (*this)(i,k) * M(k,j);
 325       }
 326     }
 327   }
 328   dealloc(data, Rows());
 329   data = prod;
 330   cols = M.cols;
 331 }
 332
 333 /******************************************************************************/
 334
 335 FGMatrix FGMatrix::operator/(const double scalar)
 336 {
 337   FGMatrix Quot(Rows(), Cols());
 338
 339   for (unsigned int i=1; i<=Rows(); i++) {
 340     for (unsigned int j=1; j<=Cols(); j++)  {
 341        Quot(i,j) = (*this)(i,j)/scalar;
 342     }
 343   }
 344   return Quot;
 345 }
 346
 347 /******************************************************************************/
 348
 349 void FGMatrix::operator/=(const double scalar)
 350 {
 351   for (unsigned int i=1; i<=Rows(); i++)  {
 352     for (unsigned int j=1; j<=Cols(); j++) {
 353       (*this)(i,j)/=scalar;
 354     }
 355   }
 356 }
 357
 358 /******************************************************************************/
 359
 360 void FGMatrix::T(void)
 361 {
 362   if (rows==cols)
 363     TransposeSquare();
 364   else
 365     TransposeNonSquare();
 366 }
 367
 368 /******************************************************************************/
 369
 370 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
 371 {
 372   FGColumnVector Product(Col.Rows());
 373
 374   if (Cols() != Col.Rows()) {
 375     MatrixException mE;
 376     mE.Message = "Invalid row/column match in Column Vector operator *";
 377     throw mE;
 378   }
 379
 380   for (unsigned int i=1;i<=Rows();i++) {
 381     Product(i) = 0.00;
 382     for (unsigned int j=1;j<=Cols();j++) {
 383       Product(i) += Col(j)*data[i][j];
 384     }
 385   }
 386   return Product;
 387 }
 388
 389 /******************************************************************************/
 390
 391 void FGMatrix::TransposeSquare(void)
 392 {
 393   for (unsigned int i=1; i<=rows; i++) {
 394     for (unsigned int j=i+1; j<=cols; j++) {
 395       double tmp = data[i][j];
 396       data[i][j] = data[j][i];
 397       data[j][i] = tmp;
 398     }
 399   }
 400 }
 401
 402 /******************************************************************************/
 403
 404 void FGMatrix::TransposeNonSquare(void)
 405 {
 406   double **tran;
 407
 408   tran = FGalloc(rows,cols);
 409
 410   for (unsigned int i=1; i<=rows; i++) {
 411     for (unsigned int j=1; j<=cols; j++) {
 412       tran[j][i] = data[i][j];
 413     }
 414   }
 415
 416   dealloc(data,rows);
 417
 418   data       = tran;
 419   unsigned int m = rows;
 420   rows       = cols;
 421   cols       = m;
 422 }
 423
 424 /******************************************************************************/
 425
 426 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
 427 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
 428 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
 429 FGColumnVector::~FGColumnVector() { }
 430
 431 /******************************************************************************/
 432
 433 double& FGColumnVector::operator()(int m) const
 434 {
 435   return FGMatrix::operator()(m,1);
 436 }
 437
 438 /******************************************************************************/
 439
 440 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
 441 {
 442   FGColumnVector Product(Col.Rows());
 443
 444   if (Mat.Cols() != Col.Rows()) {
 445     MatrixException mE;
 446     mE.Message = "Invalid row/column match in Column Vector operator *";
 447     throw mE;
 448   }
 449
 450   for (unsigned int i=1;i<=Mat.Rows();i++) {
 451     Product(i) = 0.00;
 452     for (unsigned int j=1;j<=Mat.Cols();j++) {
 453       Product(i) += Col(j)*Mat(i,j);
 454     }
 455   }
 456
 457   return Product;
 458 }
 459
 460 /******************************************************************************/
 461
 462 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
 463 {
 464   if (Rows() != C.Rows()) {
 465     MatrixException mE;
 466     mE.Message = "Invalid row/column match in Column Vector operator *";
 467     throw mE;
 468   }
 469
 470   FGColumnVector Sum(C.Rows());
 471
 472   for (unsigned int i=1; i<=C.Rows(); i++) {
 473     Sum(i) = C(i) + data[i][1];
 474   }
 475
 476   return Sum;
 477 }
 478
 479 /******************************************************************************/
 480
 481 FGColumnVector FGColumnVector::operator*(const double scalar)
 482 {
 483   FGColumnVector Product(Rows());
 484
 485   for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
 486
 487   return Product;
 488 }
 489
 490 /******************************************************************************/
 491
 492 FGColumnVector FGColumnVector::operator/(const double scalar)
 493 {
 494   FGColumnVector Quotient(Rows());
 495
 496   for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
 497
 498   return Quotient;
 499 }
 500
 501 /******************************************************************************/
 502
 503 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
 504 {
 505   FGColumnVector Product(C.Rows());
 506
 507   for (unsigned int i=1; i<=C.Rows(); i++) {
 508      Product(i) = scalar * C(i);
 509   }
 510
 511   return Product;
 512 }
 513
 514 /******************************************************************************/
 515 float FGColumnVector::Magnitude(void)
 516 {
 517   double num=0.0;
 518
 519   if ((data[1][1] == 0.00) &&
 520       (data[2][1] == 0.00) &&
 521       (data[3][1] == 0.00))
 522   {
 523     return 0.00;
 524   } else {
 525     for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
 526     return sqrt(num);
 527   }
 528 }
 529
 530 /******************************************************************************/
 531
 532 FGColumnVector FGColumnVector::Normalize(void)
 533 {
 534   return *this/Magnitude();
 535 }
 536