Encapsulate the interpolstion version of FGEnvironment and fix some bugs

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

diff --git a/src/FDM/JSBSim/FGMassBalance.cpp b/src/FDM/JSBSim/FGMassBalance.cpp
index 053428829effd5ef82d90baacbcd552a3b52a81a..582f1d957dc5e7e7991615a5a6930be77bbf4d60 100644 (file)
--- a/src/FDM/JSBSim/FGMassBalance.cpp
+++ b/src/FDM/JSBSim/FGMassBalance.cpp
@@ -41,6 +41,8 @@ INCLUDES
 #include "FGMassBalance.h"
 #include "FGPropertyManager.h"
 
+namespace JSBSim {
+
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_MASSBALANCE;
 
@@ -52,6 +54,12 @@ CLASS IMPLEMENTATION
 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
 {
   Name = "FGMassBalance";
+  Weight = EmptyWeight = Mass = 0.0;
+  Ixx = Iyy = Izz = Ixy = Ixz = 0.0;
+  baseIxx = baseIyy = baseIzz = baseIxy = baseIxz = 0.0;
+  vbaseXYZcg(eX) = 0.0;
+  vbaseXYZcg(eY) = 0.0;
+  vbaseXYZcg(eZ) = 0.0;
   bind();
 
   Debug(0);
@@ -77,8 +85,8 @@ bool FGMassBalance::Run(void)
 
 // Calculate new CG here.
 
-    vXYZcg = (Propulsion->GetTanksCG() + EmptyWeight*vbaseXYZcg
-                                       + GetPointMassCG()       ) / Weight;
+    vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
+                                       + GetPointMassMoment() ) / Weight;
 
 // Calculate new moments of inertia here
 
@@ -118,7 +126,7 @@ double FGMassBalance::GetPointMassWeight(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-FGColumnVector3& FGMassBalance::GetPointMassCG(void)
+FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
 {
   PointMassCG.InitMatrix();
 
@@ -190,6 +198,39 @@ double FGMassBalance::GetPMIxz(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
+{
+  // Under the assumption that in the structural frame the:
+  //
+  // - X-axis is directed afterwards,
+  // - Y-axis is directed towards the right,
+  // - Z-axis is directed upwards,
+  //
+  // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
+  // we have to subtract first the center of gravity of the plane which
+  // is also defined in the structural frame:
+  //
+  //   FGColumnVector3 cgOff = r - vXYZcg;
+  //
+  // Next, we do a change of units:
+  //
+  //   cgOff *= inchtoft;
+  //
+  // And then a 180 degree rotation is done about the Y axis so that the:
+  //
+  // - X-axis is directed forward,
+  // - Y-axis is directed towards the right,
+  // - Z-axis is directed downward.
+  //
+  // This is needed because the structural and body frames are 180 degrees apart.
+
+  return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
+                         inchtoft*(r(2)-vXYZcg(2)),
+                         inchtoft*(vXYZcg(3)-r(3)));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGMassBalance::bind(void)
 { 
   typedef double (FGMassBalance::*PMF)(int) const;
@@ -284,4 +325,4 @@ void FGMassBalance::Debug(int from)
     }
   }
 }
-
+}