FG_HAVE_STD_INCLUDES -> SG_HAVE_STD_INCLUDES

[flightgear.git] / src / FDM / JSBSim / FGMatrix.cpp

diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp
index c7f40c4b7f659163d3618bdf2b7d80f7e7d2e822..5660020fdb2ca6414acdcad581395b7fbb23c06b 100644 (file)
--- a/src/FDM/JSBSim/FGMatrix.cpp
+++ b/src/FDM/JSBSim/FGMatrix.cpp
@@ -1,4 +1,4 @@
-/*******************************************************************************
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 Module: FGMatrix.cpp
 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
@@ -14,15 +14,18 @@ HISTORY
 ??/??/??   TP   Created
 03/16/2000 JSB  Added exception throwing
 
-********************************************************************************
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 INCLUDES
-*******************************************************************************/
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGMatrix.h"
 
-/*******************************************************************************
-************************************ CODE **************************************
-*******************************************************************************/
+static const char *IdSrc = "$Header$";
+static const char *IdHdr = ID_MATRIX;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 double** FGalloc(int rows, int cols)
 {
@@ -38,7 +41,7 @@ double** FGalloc(int rows, int cols)
   return A;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void dealloc(double **A, int rows)
 {
@@ -46,7 +49,7 @@ void dealloc(double **A, int rows)
   delete[] A;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
 {
@@ -55,7 +58,7 @@ FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c
   rowCtr = colCtr = 1;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix::FGMatrix(const FGMatrix& M)
 {
@@ -64,7 +67,7 @@ FGMatrix::FGMatrix(const FGMatrix& M)
   *this = M;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix::~FGMatrix(void)
 {
@@ -73,7 +76,7 @@ FGMatrix::~FGMatrix(void)
   rows = cols = 0;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 ostream& operator<<(ostream& os, const FGMatrix& M)
 {
@@ -88,7 +91,7 @@ ostream& operator<<(ostream& os, const FGMatrix& M)
   return os;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix& FGMatrix::operator<<(const float ff)
 {
@@ -101,7 +104,7 @@ FGMatrix& FGMatrix::operator<<(const float ff)
   return *this;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 istream& operator>>(istream& is, FGMatrix& M)
 {
@@ -113,7 +116,7 @@ istream& operator>>(istream& is, FGMatrix& M)
   return is;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix& FGMatrix::operator=(const FGMatrix& M)
 {
@@ -137,21 +140,21 @@ FGMatrix& FGMatrix::operator=(const FGMatrix& M)
   return *this;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 unsigned int FGMatrix::Rows(void) const
 {
   return rows;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 unsigned int FGMatrix::Cols(void) const
 {
   return cols;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
 {
@@ -161,7 +164,7 @@ void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
   FGMatrix::origin = origin;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::InitMatrix(double value)
 {
@@ -174,7 +177,7 @@ void FGMatrix::InitMatrix(double value)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::InitMatrix(void)
 {
@@ -203,7 +206,7 @@ FGMatrix FGMatrix::operator-(const FGMatrix& M)
   return Diff;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::operator-=(const FGMatrix &M)
 {
@@ -220,7 +223,7 @@ void FGMatrix::operator-=(const FGMatrix &M)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix FGMatrix::operator+(const FGMatrix& M)
 {
@@ -240,7 +243,7 @@ FGMatrix FGMatrix::operator+(const FGMatrix& M)
   return Sum;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::operator+=(const FGMatrix &M)
 {
@@ -257,7 +260,7 @@ void FGMatrix::operator+=(const FGMatrix &M)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix operator*(double scalar, FGMatrix &M)
 {
@@ -271,7 +274,7 @@ FGMatrix operator*(double scalar, FGMatrix &M)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::operator*=(const double scalar)
 {
@@ -282,7 +285,7 @@ void FGMatrix::operator*=(const double scalar)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix FGMatrix::operator*(const FGMatrix& M)
 {
@@ -305,7 +308,7 @@ FGMatrix FGMatrix::operator*(const FGMatrix& M)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::operator*=(const FGMatrix& M)
 {
@@ -331,32 +334,40 @@ void FGMatrix::operator*=(const FGMatrix& M)
   cols = M.cols;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGMatrix FGMatrix::operator/(const double scalar)
 {
   FGMatrix Quot(Rows(), Cols());
 
-  for (unsigned int i=1; i<=Rows(); i++) {
-    for (unsigned int j=1; j<=Cols(); j++)  {
-       Quot(i,j) = data[i][j]/scalar;
+  if(scalar != 0) {
+    for (unsigned int i=1; i<=Rows(); i++) {
+      for (unsigned int j=1; j<=Cols(); j++)  {
+         Quot(i,j) = data[i][j]/scalar;
+      }
     }
-  }
-  return Quot;
+    
+  } else
+    cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl; 
+  return Quot;  
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::operator/=(const double scalar)
 {
-  for (unsigned int i=1; i<=Rows(); i++)  {
-    for (unsigned int j=1; j<=Cols(); j++) {
-      data[i][j]/=scalar;
+  
+  if(scalar != 0) {
+    for (unsigned int i=1; i<=Rows(); i++)  {
+      for (unsigned int j=1; j<=Cols(); j++) {
+        data[i][j]/=scalar;
+      }
     }
-  }
+  } else 
+    cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl; 
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::T(void)
 {
@@ -366,7 +377,7 @@ void FGMatrix::T(void)
     TransposeNonSquare();
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
 {
@@ -387,7 +398,7 @@ FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::TransposeSquare(void)
 {
@@ -400,7 +411,7 @@ void FGMatrix::TransposeSquare(void)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGMatrix::TransposeNonSquare(void)
 {
@@ -422,21 +433,21 @@ void FGMatrix::TransposeNonSquare(void)
   cols       = m;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
 FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
 FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
 FGColumnVector::~FGColumnVector() { }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 double& FGColumnVector::operator()(int m) const
 {
   return FGMatrix::operator()(m,1);
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
 {
@@ -458,7 +469,7 @@ FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
 {
@@ -477,7 +488,7 @@ FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
   return Sum;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::operator*(const double scalar)
 {
@@ -488,7 +499,7 @@ FGColumnVector FGColumnVector::operator*(const double scalar)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
 {
@@ -507,18 +518,24 @@ FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
   return Diff;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::operator/(const double scalar)
 {
   FGColumnVector Quotient(Rows());
+  if(scalar != 0) {
+    
 
-  for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
+    for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
 
+  } else 
+    cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl; 
   return Quotient;
+  
+    
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector operator*(const double scalar, const FGColumnVector& C)
 {
@@ -531,7 +548,7 @@ FGColumnVector operator*(const double scalar, const FGColumnVector& C)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 float FGColumnVector::Magnitude(void)
 {
@@ -548,20 +565,22 @@ float FGColumnVector::Magnitude(void)
   }
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::Normalize(void)
 {
   double Mag = Magnitude();
 
-  for (unsigned int i=1; i<=Rows(); i++)
-    for (unsigned int j=1; j<=Cols(); j++)
-      data[i][j] = data[i][j]/Mag;
+  if (Mag != 0) {
+    for (unsigned int i=1; i<=Rows(); i++)
+      for (unsigned int j=1; j<=Cols(); j++)
+        data[i][j] = data[i][j]/Mag;
+  }    
 
   return *this;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
 {
@@ -580,5 +599,23 @@ FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
   return Product;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V)
+{
+  if (Rows() != 3 || V.Rows() != 3) {
+    MatrixException mE;
+    mE.Message = "Invalid row count in vector cross product function";
+    throw mE;
+  }
+
+  FGColumnVector Product(3);
+
+  Product(1) = data[1][1] * V(1);
+  Product(2) = data[2][1] * V(2);
+  Product(3) = data[3][1] * V(3);
+
+  return Product;
+}
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%