src/FDM/JSBsim/FGMatrix.cpp

   1 /*******************************************************************************
   2
   3 Module: FGMatrix.cpp
   4 Author: Tony Peden [formatted here by JSB]
   5 Date started: ??
   6 Purpose: FGMatrix class
   7 Called by: Various
   8
   9 FUNCTIONAL DESCRIPTION
  10 --------------------------------------------------------------------------------
  11
  12
  13 ARGUMENTS
  14 --------------------------------------------------------------------------------
  15
  16
  17 HISTORY
  18 --------------------------------------------------------------------------------
  19 ??/??/??   TP   Created
  20
  21 ********************************************************************************
  22 INCLUDES
  23 *******************************************************************************/
  24
  25 #include <stdlib.h>
  26 #include "FGMatrix.h"
  27 #include <iostream.h>
  28 #include <iomanip.h>
  29 #include <fstream.h>
  30
  31 /*******************************************************************************
  32 DEFINES
  33 *******************************************************************************/
  34
  35 #pragma warn -use
  36
  37 /*******************************************************************************
  38 CONSTANTS
  39 *******************************************************************************/
  40
  41
  42 /*******************************************************************************
  43 TYPEDEFS
  44 *******************************************************************************/
  45
  46
  47 /*******************************************************************************
  48 GLOBALS
  49 *******************************************************************************/
  50
  51
  52 /*******************************************************************************
  53 ************************************ CODE **************************************
  54 *******************************************************************************/
  55
  56 double** alloc(int rows,int cols)
  57 {
  58   double **A;
  59
  60   A = new double *[rows+1];
  61   if (!A)       return NULL;
  62
  63   for (int i=0;i<=rows;i++){
  64     A[i]=new double[cols+1];
  65     if (!A[i]) return NULL;
  66   }
  67   return A;
  68 }
  69
  70
  71 void dealloc(double **A, int rows, int cols)
  72 {
  73   for(int i=0;i<=rows;i++){
  74     delete[] A[i];
  75   }
  76
  77   delete[] A;
  78 }
  79
  80
  81 FGMatrix::FGMatrix(unsigned rows, unsigned cols)
  82 {
  83   this->rows=rows;
  84   this->cols=cols;
  85   keep=false;
  86   data=alloc(rows,cols);
  87 }
  88
  89
  90 FGMatrix::FGMatrix(const FGMatrix& A)
  91 {
  92   data=NULL;
  93   *this=A;
  94 }
  95
  96
  97 FGMatrix::~FGMatrix(void)
  98 {
  99   if (keep == false) {
 100     dealloc(data,rows,cols);
 101     rows=cols=0;
 102   }
 103 }
 104
 105
 106 FGMatrix& FGMatrix::operator=(const FGMatrix& A)
 107 {
 108   if (&A != this) {
 109     if (data != NULL) dealloc(data,rows,cols);
 110
 111     width  = A.width;
 112     prec   = A.prec;
 113     delim  = A.delim;
 114     origin = A.origin;
 115     rows   = A.rows;
 116     cols   = A.cols;
 117     keep   = false;
 118
 119     if (A.keep  == true) {
 120       data = A.data;
 121     } else {
 122       data = alloc(rows,cols);
 123       for (unsigned int i=0; i<=rows; i++) {
 124               for (unsigned int j=0; j<=cols; j++) {
 125                       data[i][j] = A.data[i][j];
 126               }
 127       }
 128     }
 129   }
 130   return *this;
 131 }
 132
 133
 134 double& FGMatrix::operator()(unsigned row, unsigned col)
 135 {
 136   return data[row][col];
 137 }
 138
 139
 140 unsigned FGMatrix::Rows(void) const
 141 {
 142   return rows;
 143 }
 144
 145
 146 unsigned FGMatrix::Cols(void) const
 147 {
 148   return cols;
 149 }
 150
 151
 152 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
 153 {
 154   FGMatrix::delim=delim;
 155   FGMatrix::width=width;
 156   FGMatrix::prec=prec;
 157   FGMatrix::origin=origin;
 158 }
 159
 160
 161 void FGMatrix::InitMatrix(double value)
 162 {
 163   if (data) {
 164     for (unsigned int i=0;i<=rows;i++) {
 165                         for (unsigned int j=0;j<=cols;j++) {
 166                 operator()(i,j) = value;
 167                         }
 168                 }
 169         }
 170 }
 171
 172
 173 void FGMatrix::InitMatrix(void)
 174 {
 175         this->InitMatrix(0);
 176 }
 177
 178
 179 FGMatrix operator-(FGMatrix& A, FGMatrix& B)
 180 {
 181         if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
 182                 cout << endl << "FGMatrix::operator-" << endl << '\t';
 183                 cout << "Subtraction not defined for matrices of different sizes";
 184     cout << endl;
 185                 exit(1);
 186         }
 187
 188   FGMatrix Diff(A.Rows(),A.Cols());
 189   Diff.keep=true;
 190   for (unsigned int i=1;i<=A.Rows();i++) {
 191                 for (unsigned int j=1;j<=A.Cols();j++) {
 192                         Diff(i,j)=A(i,j)-B(i,j);
 193                 }
 194         }
 195         return Diff;
 196 }
 197
 198
 199 void operator-=(FGMatrix &A,FGMatrix &B)
 200 {
 201         if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
 202                 cout << endl << "FGMatrix::operator-" << endl << '\t';
 203                 cout << "Subtraction not defined for matrices of different sizes";
 204     cout << endl;
 205                 exit(1);
 206         }
 207
 208   for (unsigned int i=1;i<=A.Rows();i++) {
 209                 for (unsigned int j=1;j<=A.Cols();j++) {
 210                         A(i,j)-=B(i,j);
 211                 }
 212         }
 213 }
 214
 215
 216 FGMatrix operator+(FGMatrix& A, FGMatrix& B)
 217 {
 218         if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
 219                 cout << endl << "FGMatrix::operator+" << endl << '\t';
 220                 cout << "Addition not defined for matrices of different sizes";
 221     cout << endl;
 222                 exit(1);
 223         }
 224
 225   FGMatrix Sum(A.Rows(),A.Cols());
 226   Sum.keep = true;
 227         for (unsigned int i=1;i<=A.Rows();i++) {
 228                 for (unsigned int j=1;j<=A.Cols();j++) {
 229                         Sum(i,j)=A(i,j)+B(i,j);
 230                 }
 231         }
 232         return Sum;
 233 }
 234
 235
 236 void operator+=(FGMatrix &A,FGMatrix &B)
 237 {
 238         if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
 239                 cout << endl << "FGMatrix::operator+" << endl << '\t';
 240                 cout << "Addition not defined for matrices of different sizes";
 241     cout << endl;
 242                 exit(1);
 243         }
 244   for (unsigned int i=1;i<=A.Rows();i++) {
 245                 for (unsigned int j=1;j<=A.Cols();j++) {
 246                         A(i,j)+=B(i,j);
 247                 }
 248         }
 249 }
 250
 251
 252 FGMatrix operator*(double scalar,FGMatrix &A)
 253 {
 254         FGMatrix Product(A.Rows(),A.Cols());
 255   Product.keep = true;
 256         for (unsigned int i=1;i<=A.Rows();i++) {
 257                 for (unsigned int j=1;j<=A.Cols();j++) {
 258         Product(i,j) = scalar*A(i,j);
 259     }
 260         }
 261         return Product;
 262 }
 263
 264
 265 void operator*=(FGMatrix &A,double scalar)
 266 {
 267         for (unsigned int i=1;i<=A.Rows();i++) {
 268                 for (unsigned int j=1;j<=A.Cols();j++) {
 269         A(i,j)*=scalar;
 270     }
 271   }
 272 }
 273
 274
 275 FGMatrix operator*(FGMatrix &Left, FGMatrix &Right)
 276 {
 277         if (Left.Cols() != Right.Rows()) {
 278                 cout << endl << "FGMatrix::operator*" << endl << '\t';
 279                 cout << "The number of rows in the right matrix must match the number";
 280                 cout << endl << '\t' << "of columns in the left." << endl;
 281                 cout << '\t' << "Multiplication not defined." << endl;
 282                 exit(1);
 283         }
 284
 285         FGMatrix Product(Left.Rows(),Right.Cols());
 286         Product.keep = true;
 287         for (unsigned int i=1;i<=Left.Rows();i++) {
 288                 for (unsigned int j=1;j<=Right.Cols();j++)      {
 289         Product(i,j) = 0;
 290       for (unsigned int k=1;k<=Left.Cols();k++) {
 291                         Product(i,j)+=Left(i,k)*Right(k,j);
 292       }
 293                 }
 294         }
 295         return Product;
 296 }
 297
 298
 299 void operator*=(FGMatrix &Left,FGMatrix &Right)
 300 {
 301         if (Left.Cols() != Right.Rows()) {
 302                 cout << endl << "FGMatrix::operator*" << endl << '\t';
 303                 cout << "The number of rows in the right matrix must match the number";
 304                 cout << endl << '\t' << "of columns in the left." << endl;
 305                 cout << '\t' << "Multiplication not defined." << endl;
 306                 exit(1);
 307         }
 308
 309         double **prod;
 310
 311                 prod=alloc(Left.Rows(),Right.Cols());
 312                 for (unsigned int i=1;i<=Left.Rows();i++) {
 313                         for (unsigned int j=1;j<=Right.Cols();j++) {
 314         prod[i][j] = 0;
 315                                 for (unsigned int k=1;k<=Left.Cols();k++) {
 316                 prod[i][j]+=Left(i,k)*Right(k,j);
 317                                 }
 318                         }
 319                 }
 320                 dealloc(Left.data,Left.Rows(),Left.Cols());
 321                 Left.data=prod;
 322                 Left.cols=Right.cols;
 323 }
 324
 325
 326 FGMatrix operator/(FGMatrix& A, double scalar)
 327 {
 328         FGMatrix Quot(A.Rows(),A.Cols());
 329         A.keep = true;
 330         for (unsigned int i=1;i<=A.Rows();i++) {
 331                 for (unsigned int j=1;j<=A.Cols();j++)  {
 332                 Quot(i,j)=A(i,j)/scalar;
 333                 }
 334         }
 335         return Quot;
 336 }
 337
 338
 339 void operator/=(FGMatrix &A,double scalar)
 340 {
 341         for (unsigned int i=1;i<=A.Rows();i++)  {
 342                 for (unsigned int j=1;j<=A.Cols();j++) {
 343                         A(i,j)/=scalar;
 344                 }
 345         }
 346 }
 347
 348
 349 void FGMatrix::T(void)
 350 {
 351         if (rows==cols)
 352                 TransposeSquare();
 353         else
 354                 TransposeNonSquare();
 355 }
 356
 357
 358 void FGMatrix::TransposeSquare(void)
 359 {
 360         for (unsigned int i=1;i<=rows;i++) {
 361                 for (unsigned int j=i+1;j<=cols;j++) {
 362                         double tmp=data[i][j];
 363                         data[i][j]=data[j][i];
 364                         data[j][i]=tmp;
 365                 }
 366         }
 367 }
 368
 369
 370 void FGMatrix::TransposeNonSquare(void)
 371 {
 372         double **tran;
 373
 374         tran=alloc(rows,cols);
 375         for (unsigned int i=1;i<=rows;i++) {
 376                 for (unsigned int j=1;j<=cols;j++) {
 377                         tran[j][i]=data[i][j];
 378                 }
 379         }
 380         dealloc(data,rows,cols);
 381
 382         data=tran;
 383         unsigned m=rows;
 384         rows=cols;
 385         cols=m;
 386 }
 387
 388
 389 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
 390 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
 391 FGColumnVector::FGColumnVector(FGColumnVector& b):FGMatrix(b) { }
 392 FGColumnVector::~FGColumnVector() { }
 393 double& FGColumnVector::operator()(int m)
 394 {
 395         return FGMatrix::operator()(m,1);
 396 }
 397