builddir -> srcdir so builds can be done outside the master source directory.

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

diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp
index 549a83c99456d850ee9ddb313c6409846a856ad9..5b4f5da10ba5937b5f0495d92a53517c19144544 100644 (file)
--- a/src/FDM/JSBSim/FGMatrix.cpp
+++ b/src/FDM/JSBSim/FGMatrix.cpp
@@ -197,7 +197,7 @@ FGMatrix FGMatrix::operator-(const FGMatrix& M)
 
   for (unsigned int i=1; i<=Rows(); i++) {
     for (unsigned int j=1; j<=Cols(); j++) {
-      Diff(i,j) = (*this)(i,j) - M(i,j);
+      Diff(i,j) = data[i][j] - M(i,j);
     }
   }
   return Diff;
@@ -215,7 +215,7 @@ void FGMatrix::operator-=(const FGMatrix &M)
 
   for (unsigned int i=1; i<=Rows(); i++) {
     for (unsigned int j=1; j<=Cols(); j++) {
-      (*this)(i,j) -= M(i,j);
+      data[i][j] -= M(i,j);
     }
   }
 }
@@ -234,7 +234,7 @@ FGMatrix FGMatrix::operator+(const FGMatrix& M)
 
   for (unsigned int i=1; i<=Rows(); i++) {
     for (unsigned int j=1; j<=Cols(); j++) {
-      Sum(i,j) = (*this)(i,j) + M(i,j);
+      Sum(i,j) = data[i][j] + M(i,j);
     }
   }
   return Sum;
@@ -252,7 +252,7 @@ void FGMatrix::operator+=(const FGMatrix &M)
 
   for (unsigned int i=1; i<=Rows(); i++) {
     for (unsigned int j=1; j<=Cols(); j++) {
-      (*this)(i,j)+=M(i,j);
+      data[i][j]+=M(i,j);
     }
   }
 }
@@ -277,7 +277,7 @@ void FGMatrix::operator*=(const double scalar)
 {
   for (unsigned int i=1; i<=Rows(); i++) {
     for (unsigned int j=1; j<=Cols(); j++) {
-      (*this)(i,j) *= scalar;
+      data[i][j] *= scalar;
     }
   }
 }
@@ -298,7 +298,7 @@ FGMatrix FGMatrix::operator*(const FGMatrix& M)
     for (unsigned int j=1; j<=M.Cols(); j++)  {
       Product(i,j) = 0;
       for (unsigned int k=1; k<=Cols(); k++) {
-         Product(i,j) += (*this)(i,k) * M(k,j);
+         Product(i,j) += data[i][k] * M(k,j);
       }
     }
   }
@@ -322,7 +322,7 @@ void FGMatrix::operator*=(const FGMatrix& M)
     for (unsigned int j=1; j<=M.Cols(); j++) {
       prod[i][j] = 0;
       for (unsigned int k=1; k<=Cols(); k++) {
-        prod[i][j] += (*this)(i,k) * M(k,j);
+        prod[i][j] += data[i][k] * M(k,j);
       }
     }
   }
@@ -337,23 +337,31 @@ 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) = (*this)(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++) {
-      (*this)(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; 
 }
 
 /******************************************************************************/
@@ -490,13 +498,38 @@ FGColumnVector FGColumnVector::operator*(const double scalar)
 
 /******************************************************************************/
 
+FGColumnVector FGColumnVector::operator-(const FGColumnVector& V)
+{
+  if ((Rows() != V.Rows()) || (Cols() != V.Cols())) {
+    MatrixException mE;
+    mE.Message = "Invalid row/column match in Column Vector operator -";
+    throw mE;
+  }
+
+  FGColumnVector Diff(Rows());
+
+  for (unsigned int i=1; i<=Rows(); i++) {
+    Diff(i) = data[i][1] - V(i);
+  }
+
+  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;
+  
+    
 }
 
 /******************************************************************************/
@@ -513,6 +546,7 @@ FGColumnVector operator*(const double scalar, const FGColumnVector& C)
 }
 
 /******************************************************************************/
+
 float FGColumnVector::Magnitude(void)
 {
   double num=0.0;
@@ -532,6 +566,53 @@ float FGColumnVector::Magnitude(void)
 
 FGColumnVector FGColumnVector::Normalize(void)
 {
-  return *this/Magnitude();
+  double Mag = Magnitude();
+
+  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)
+{
+  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[2][1] * V(3) - data[3][1] * V(2);
+  Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
+  Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
+
+  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;
+}
+
+/******************************************************************************/