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