Andreas Gaeb: fix #222 (JSBSIm reset problems)

[flightgear.git] / src / FDM / JSBSim / models / FGPropagate.h

diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h
index a98a87e2089e49ceba749debb8fe7647431b0bd4..00de83c7f069e6747bad79c2210a7d30cfaba01f 100644 (file)
--- a/src/FDM/JSBSim/models/FGPropagate.h
+++ b/src/FDM/JSBSim/models/FGPropagate.h
@@ -49,7 +49,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -102,7 +102,7 @@ CLASS DOCUMENTATION
     @endcode
 
     @author Jon S. Berndt, Mathias Froehlich
-    @version $Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $
+    @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $
   */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -135,6 +135,11 @@ public:
         units rad/sec */
     FGColumnVector3 vPQRi;
 
+    /** The angular velocity vector for the vehicle body frame relative to the
+        ECI frame, expressed in the ECI frame.
+        units rad/sec */
+    FGColumnVector3 vPQRi_i;
+
     /** The current orientation of the vehicle, that is, the orientation of the
         body frame relative to the local, NED frame. */
     FGQuaternion qAttitudeLocal;
@@ -147,10 +152,10 @@ public:
 
     FGColumnVector3 vInertialPosition;
 
-    deque <FGColumnVector3> dqPQRdot;
-    deque <FGColumnVector3> dqUVWdot;
+    deque <FGColumnVector3> dqPQRidot;
+    deque <FGColumnVector3> dqUVWidot;
     deque <FGColumnVector3> dqInertialVelocity;
-    deque <FGQuaternion> dqQtrndot;
+    deque <FGQuaternion>    dqQtrndot;
   };
 
   /** Constructor.
@@ -181,10 +186,6 @@ public:
       @return false if no error */
   bool Run(void);
 
-  void CalculatePQRdot(void);
-  void CalculateQuatdot(void);
-  void CalculateInertialVelocity(void);
-  void CalculateUVWdot(void);
   const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
 
   /** Retrieves the velocity vector.
@@ -521,19 +522,23 @@ public:
 
   void SetVState(VehicleState* vstate) {
       VState.vLocation = vstate->vLocation;
+      UpdateLocationMatrices();
+      SetInertialOrientation(vstate->qAttitudeECI);
+      VehicleRadius = GetRadius();
       VState.vUVW = vstate->vUVW;
+      vVel = GetTb2l() * VState.vUVW;
       VState.vPQR = vstate->vPQR;
-      VState.qAttitudeLocal = vstate->qAttitudeLocal;
-      VState.qAttitudeECI = vstate->qAttitudeECI;
+      VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+      VState.vInertialPosition = vstate->vInertialPosition;
 
-      VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
-      VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
-      VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
-      VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+      InitializeDerivatives();
   }
 
+  void InitializeDerivatives(void);
+
   void SetInertialOrientation(FGQuaternion Qi);
   void SetInertialVelocity(FGColumnVector3 Vi);
+  void SetInertialRates(FGColumnVector3 vRates);
 
   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 
@@ -549,18 +554,46 @@ public:
 
 // SET functions
 
-  void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
-  void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
-  void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
-  void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
-  void SetRadius(double r) { VState.vLocation.SetRadius(r); }
-  void SetLocation(const FGLocation& l) { VState.vLocation = l; }
+  void SetLongitude(double lon) {
+      VState.vLocation.SetLongitude(lon);
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      UpdateLocationMatrices();
+  }
+  void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
+  void SetLatitude(double lat) {
+      VState.vLocation.SetLatitude(lat);
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      UpdateLocationMatrices();
+  }
+  void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
+  void SetRadius(double r) {
+      VState.vLocation.SetRadius(r);
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      UpdateLocationMatrices();
+  }
   void SetAltitudeASL(double altASL);
   void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
   void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
   void SetTerrainElevation(double tt);
   void SetDistanceAGL(double tt);
   void SetInitialState(const FGInitialCondition *);
+  void SetPosition(const double Lon, const double Lat, const double Radius) {
+      VState.vLocation.SetPosition(Lon, Lat, Radius);
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      VehicleRadius = GetRadius();
+      UpdateLocationMatrices();
+  }
+  void SetLocation(const FGLocation& l) {
+      VState.vLocation = l;
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      UpdateLocationMatrices();
+  }
+  void SetLocation(const FGColumnVector3& l) {
+      VState.vLocation = l;
+      VState.vInertialPosition = GetTec2i() * VState.vLocation;
+      UpdateLocationMatrices();
+  }
+
   void RecomputeLocalTerrainRadius(void);
 
   void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
@@ -568,6 +601,16 @@ public:
     VState.vLocation -= vDeltaXYZEC;
   }
 
+  struct LagrangeMultiplier {
+    FGColumnVector3 ForceJacobian;
+    FGColumnVector3 MomentJacobian;
+    double Min;
+    double Max;
+    double value;
+  };
+
+  void DumpState(void);
+
 private:
 
 // state vector
@@ -576,7 +619,8 @@ private:
 
   FGColumnVector3 vVel;
   FGColumnVector3 vPQRdot;
-  FGColumnVector3 vUVWdot;
+  FGColumnVector3 vPQRidot;
+  FGColumnVector3 vUVWdot, vUVWidot;
   FGColumnVector3 vInertialVelocity;
   FGColumnVector3 vLocation;
   FGColumnVector3 vDeltaXYZEC;
@@ -595,17 +639,21 @@ private:
   FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
   FGMatrix33 Ti2l;
   FGMatrix33 Tl2i;
-  FGLocation contactloc;
-  FGColumnVector3 dv;
   
   double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
-  double radInv;
+  FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
   eIntegrateType integrator_rotational_rate;
   eIntegrateType integrator_translational_rate;
   eIntegrateType integrator_rotational_position;
   eIntegrateType integrator_translational_position;
   int gravType;
 
+  void CalculatePQRdot(void);
+  void CalculateQuatdot(void);
+  void CalculateInertialVelocity(void);
+  void CalculateUVW(void);
+  void CalculateUVWdot(void);
+
   void Integrate( FGColumnVector3& Integrand,
                   FGColumnVector3& Val,
                   deque <FGColumnVector3>& ValDot,
@@ -618,6 +666,19 @@ private:
                   double dt,
                   eIntegrateType integration_type);
 
+  void EvaluateRateToResistTo(FGColumnVector3& vdot,
+                              const FGColumnVector3& Val,
+                              const FGColumnVector3& ValDot,
+                              const FGColumnVector3& LocalTerrainVal,
+                              deque <FGColumnVector3>& dqValDot,
+                              const double dt,
+                              const eIntegrateType integration_type);
+
+  void ResolveFrictionForces(double dt);
+
+  void UpdateLocationMatrices(void);
+  void UpdateBodyMatrices(void);
+
   void bind(void);
   void Debug(int from);
 };