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