Sync. with JSBSim CVS.

src/FDM/JSBSim/FGFDMExec.cpp

@@ -71,7 +71,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.78 2010/04/12 12:25:19 jberndt Exp $";
+static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.79 2010/07/25 13:52:20 jberndt Exp $";
 static const char *IdHdr = ID_FDMEXEC;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -665,10 +665,10 @@ bool FGFDMExec::LoadModel(string model, bool addModelToPath)
 
     modelLoaded = true;
 
-    MassBalance->Run(); // Update all mass properties for the report.
-    MassBalance->GetMassPropertiesReport();
-
     if (debug_lvl > 0) {
+      MassBalance->Run(); // Update all mass properties for the report.
+      MassBalance->GetMassPropertiesReport();
+
       cout << endl << fgblue << highint
            << "End of vehicle configuration loading." << endl
            << "-------------------------------------------------------------------------------"

src/FDM/JSBSim/math/FGColumnVector3.h

@@ -245,6 +245,13 @@ public:
       is equal to zero it is left untouched.   */
   FGColumnVector3& Normalize(void);
 
+  /** Dot product of two vectors
+      Compute and return the euclidean dot (or scalar) product of two vectors
+      v1 and v2 */
+  friend inline double DotProduct(const FGColumnVector3& v1, const FGColumnVector3& v2) {
+    return v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3);
+  }
+
 private:
   double data[3];
 

src/FDM/JSBSim/math/FGLocation.h

@@ -48,7 +48,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_LOCATION "$Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $"
+#define ID_LOCATION "$Id: FGLocation.h,v 1.22 2010/07/25 22:15:57 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -142,7 +142,7 @@ CLASS DOCUMENTATION
     @see W. C. Durham "Aircraft Dynamics & Control", section 2.2
 
     @author Mathias Froehlich
-    @version $Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $
+    @version $Id: FGLocation.h,v 1.22 2010/07/25 22:15:57 jberndt Exp $
   */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -326,7 +326,8 @@ public:
       @return the distance of the location represented with this class
       instance to the center of the earth in ft. The radius value is
       always positive. */
-  double GetRadius() const { return mECLoc.Magnitude(); }
+  //double GetRadius() const { return mECLoc.Magnitude(); } // may not work with FlightGear
+  double GetRadius() const { ComputeDerived(); return mRadius; }
 
   /** Transform matrix from local horizontal to earth centered frame.
       Returns a const reference to the rotation matrix of the transform from

src/FDM/JSBSim/models/FGAircraft.cpp

@@ -68,7 +68,7 @@ DEFINITIONS
 GLOBAL DATA
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.26 2010/02/15 03:28:24 jberndt Exp $";
+static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.27 2010/07/27 23:18:19 jberndt Exp $";
 static const char *IdHdr = ID_AIRCRAFT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -139,10 +139,10 @@ bool FGAircraft::Run(void)
 
   vBodyAccel = vForces/MassBalance->GetMass();
 
-  vNcg = vBodyAccel/Inertial->gravity();
+  vNcg = vBodyAccel/Inertial->SLgravity();
 
   vNwcg = Aerodynamics->GetTb2w() * vNcg;
-  vNwcg(3) = -1*vNwcg(3) + 1;
+  vNwcg(3) = 1.0 - vNwcg(3);
 
   RunPostFunctions();
 

src/FDM/JSBSim/models/FGAuxiliary.cpp

@@ -4,7 +4,7 @@
  Author:       Tony Peden, Jon Berndt
  Date started: 01/26/99
  Purpose:      Calculates additional parameters needed by the visual system, etc.
- Called by:    FGSimExec
+ Called by:    FGFDMExec
 
  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
 
@@ -59,7 +59,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.39 2010/07/09 12:06:11 jberndt Exp $";
+static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.42 2010/07/27 23:18:19 jberndt Exp $";
 static const char *IdHdr = ID_AUXILIARY;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -167,22 +167,10 @@ bool FGAuxiliary::Run()
     vEulerRates(ePhi) = vPQR(eP) + vEulerRates(ePsi)*sTht;
   }
 
-// 12/16/2005, JSB: For ground handling purposes, at this time, let's ramp
+// Combine the wind speed with aircraft speed to obtain wind relative speed
-// in the effects of wind from 10 fps to 30 fps when there is weight on the
+  FGColumnVector3 wind = Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
-// landing gear wheels.
+  vAeroPQR = vPQR - Atmosphere->GetTurbPQR();
-
+  vAeroUVW = vUVW - wind;
-  if (GroundReactions->GetWOW() && vUVW(eU) < 10) {
-    vAeroPQR = vPQR;
-    vAeroUVW = vUVW;
-  } else if (GroundReactions->GetWOW() && vUVW(eU) < 30) {
-    double factor = (vUVW(eU) - 10.0)/20.0;
-    vAeroPQR = vPQR - factor*Atmosphere->GetTurbPQR();
-    vAeroUVW = vUVW - factor*Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
-  } else {
-    FGColumnVector3 wind = Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
-    vAeroPQR = vPQR - Atmosphere->GetTurbPQR();
-    vAeroUVW = vUVW - wind;
-  }
 
   Vt = vAeroUVW.Magnitude();
   if ( Vt > 0.05) {
@@ -258,7 +246,7 @@ bool FGAuxiliary::Run()
 
      vAircraftAccel /= MassBalance->GetMass();
      // Nz is Acceleration in "g's", along normal axis (-Z body axis)
-     Nz = -vAircraftAccel(eZ)/Inertial->gravity();
+     Nz = -vAircraftAccel(eZ)/Inertial->SLgravity();
      vToEyePt = MassBalance->StructuralToBody(Aircraft->GetXYZep());
      vPilotAccel = vAircraftAccel + Propagate->GetPQRdot() * vToEyePt;
      vPilotAccel += vPQR * (vPQR * vToEyePt);
@@ -269,11 +257,11 @@ bool FGAuxiliary::Run()
      // any jitter that could be introduced by the landing gear. Theoretically,
      // this branch could be eliminated, with a penalty of having a short
      // transient at startup (lasting only a fraction of a second).
-     vPilotAccel = Propagate->GetTl2b() * FGColumnVector3( 0.0, 0.0, -Inertial->gravity() );
+     vPilotAccel = Propagate->GetTl2b() * FGColumnVector3( 0.0, 0.0, -Inertial->SLgravity() );
-     Nz = -vPilotAccel(eZ)/Inertial->gravity();
+     Nz = -vPilotAccel(eZ)/Inertial->SLgravity();
   }
 
-  vPilotAccelN = vPilotAccel/Inertial->gravity();
+  vPilotAccelN = vPilotAccel/Inertial->SLgravity();
 
   // VRP computation
   const FGLocation& vLocation = Propagate->GetLocation();

src/FDM/JSBSim/models/FGGroundReactions.cpp

@@ -46,14 +46,53 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.26 2009/11/12 13:08:11 jberndt Exp $";
+static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.29 2010/07/30 11:50:01 jberndt Exp $";
 static const char *IdHdr = ID_GROUNDREACTIONS;
 
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION for MultiplierIterator (See below for FGGroundReactions)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+MultiplierIterator::MultiplierIterator(FGGroundReactions* GndReactions)
+: GroundReactions(GndReactions),
+  multiplier(NULL),
+  gearNum(0),
+  entry(0)
+{
+  for (int i=0; i < GroundReactions->GetNumGearUnits(); i++) {
+		FGLGear* gear = GroundReactions->GetGearUnit(i);
+
+		if (!gear->GetWOW()) continue;
+
+    gearNum = i;
+    multiplier = gear->GetMultiplierEntry(0);
+    break;
+  }
+}
+
+MultiplierIterator& MultiplierIterator::operator++()
+{
+  for (int i=gearNum; i < GroundReactions->GetNumGearUnits(); i++) {
+		FGLGear* gear = GroundReactions->GetGearUnit(i);
+
+    if (!gear->GetWOW()) continue;
+
+    multiplier = gear->GetMultiplierEntry(++entry);
+    if (multiplier) {
+      gearNum = i;
+      break;
+    }
+    else
+      entry = -1;
+  }
+
+  return *this;
+}
+
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-
 FGGroundReactions::FGGroundReactions(FGFDMExec* fgex) : FGModel(fgex)
 {
   Name = "FGGroundReactions";
@@ -123,6 +162,20 @@ bool FGGroundReactions::GetWOW(void)
   return result;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// This function must be called after friction forces are resolved in order to
+// include them in the ground reactions total force and moment.
+void FGGroundReactions::UpdateForcesAndMoments(void)
+{
+  vForces.InitMatrix();
+  vMoments.InitMatrix();
+
+  for (unsigned int i=0; i<lGear.size(); i++) {
+    vForces  += lGear[i]->UpdateForces();
+    vMoments += lGear[i]->GetMoments();
+  }
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 bool FGGroundReactions::Load(Element* el)

src/FDM/JSBSim/models/FGGroundReactions.h

@@ -45,7 +45,7 @@ INCLUDES
 #include "math/FGColumnVector3.h"
 #include "input_output/FGXMLElement.h"
 
-#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.15 2009/10/02 10:30:09 jberndt Exp $"
+#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.17 2010/07/30 11:50:01 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -78,6 +78,19 @@ CLASS DOCUMENTATION
 CLASS DECLARATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
+class MultiplierIterator
+{
+public:
+  MultiplierIterator(FGGroundReactions* GndReactions);
+  MultiplierIterator& operator++();
+  FGPropagate::LagrangeMultiplier* operator*() { return multiplier; }
+private:
+  FGGroundReactions* GroundReactions;
+  FGPropagate::LagrangeMultiplier* multiplier;
+  int gearNum;
+  int entry;
+};
+
 class FGGroundReactions : public FGModel
 {
 public:
@@ -94,6 +107,7 @@ public:
   string GetGroundReactionStrings(string delimeter);
   string GetGroundReactionValues(string delimeter);
   bool GetWOW(void);
+  void UpdateForcesAndMoments(void);
 
   int GetNumGearUnits(void) const { return (int)lGear.size(); }
 

src/FDM/JSBSim/models/FGLGear.cpp

@@ -61,7 +61,7 @@ DEFINITIONS
 GLOBAL DATA
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-static const char *IdSrc = "$Id: FGLGear.cpp,v 1.74 2010/05/18 10:54:14 jberndt Exp $";
+static const char *IdSrc = "$Id: FGLGear.cpp,v 1.76 2010/07/30 11:50:01 jberndt Exp $";
 static const char *IdHdr = ID_LGEAR;
 
 // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
@@ -210,49 +210,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
          << sSteerType << " is undefined." << endl;
   }
 
-  RFRV = 0.7;  // Rolling force relaxation velocity, default value
+  Auxiliary       = fdmex->GetAuxiliary();
-  SFRV = 0.7;  // Side force relaxation velocity, default value
+  Propagate       = fdmex->GetPropagate();
-
+  FCS             = fdmex->GetFCS();
-  Element* relax_vel = el->FindElement("relaxation_velocity");
+  MassBalance     = fdmex->GetMassBalance();
-  if (relax_vel) {
+  GroundReactions = fdmex->GetGroundReactions();
-    if (relax_vel->FindElement("rolling")) {
-      RFRV = relax_vel->FindElementValueAsNumberConvertTo("rolling", "FT/SEC");
-    }
-    if (relax_vel->FindElement("side")) {
-      SFRV = relax_vel->FindElementValueAsNumberConvertTo("side", "FT/SEC");
-    }
-  }
-
-  Aircraft    = fdmex->GetAircraft();
-  Propagate   = fdmex->GetPropagate();
-  Auxiliary   = fdmex->GetAuxiliary();
-  FCS         = fdmex->GetFCS();
-  MassBalance = fdmex->GetMassBalance();
-
-  LongForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default longitudinal force filter coefficient
-  LatForceLagFilterCoeff  = 1/fdmex->GetDeltaT(); // default lateral force filter coefficient
-
-  Element* force_lag_filter_elem = el->FindElement("force_lag_filter");
-  if (force_lag_filter_elem) {
-    if (force_lag_filter_elem->FindElement("rolling")) {
-      LongForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("rolling");
-    }
-    if (force_lag_filter_elem->FindElement("side")) {
-      LatForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("side");
-    }
-  }
-
-  LongForceFilter = Filter(LongForceLagFilterCoeff, fdmex->GetDeltaT());
-  LatForceFilter = Filter(LatForceLagFilterCoeff, fdmex->GetDeltaT());
-
-  WheelSlipLagFilterCoeff = 1/fdmex->GetDeltaT();
-
-  Element *wheel_slip_angle_lag_elem = el->FindElement("wheel_slip_filter");
-  if (wheel_slip_angle_lag_elem) {
-    WheelSlipLagFilterCoeff = wheel_slip_angle_lag_elem->GetDataAsNumber();
-  }
-  
-  WheelSlipFilter = Filter(WheelSlipLagFilterCoeff, fdmex->GetDeltaT());
 
   GearUp = false;
   GearDown = true;
@@ -291,6 +253,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
   Peak = staticFCoeff;
   Curvature = 1.03;
 
+  // Initialize Lagrange multipliers
+  LMultiplier[ftRoll].value = 0.;
+  LMultiplier[ftSide].value = 0.;
+  LMultiplier[ftRoll].value = 0.;
+
   Debug(0);
 }
 
@@ -307,35 +274,35 @@ FGLGear::~FGLGear()
 FGColumnVector3& FGLGear::GetBodyForces(void)
 {
   double t = fdmex->GetSimTime();
-  dT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
+  dT = fdmex->GetDeltaT()*GroundReactions->GetRate();
 
   vFn.InitMatrix();
 
   if (isRetractable) ComputeRetractionState();
 
   if (GearDown) {
-    double verticalZProj = 0.;
-
     vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); // Get wheel in body frame
     vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location
 
     gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear);
-    // Compute the height of the theoretical location of the wheel (if strut is not compressed) with
+    // Compute the height of the theoretical location of the wheel (if strut is
-    // respect to the ground level
+    // not compressed) with respect to the ground level
     double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
     vGroundNormal = Propagate->GetTec2b() * normal;
 
-    // The height returned above is the AGL and is expressed in the Z direction of the local
+    // The height returned above is the AGL and is expressed in the Z direction
-    // coordinate frame. We now need to transform this height in actual compression of the strut (BOGEY)
+    // of the ECEF coordinate frame. We now need to transform this height in
-    // of in the normal direction to the ground (STRUCTURE)
+    // actual compression of the strut (BOGEY) of in the normal direction to the
+    // ground (STRUCTURE)
+    double normalZ = (Propagate->GetTec2l()*normal)(eZ);
+    double LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
+
     switch (eContactType) {
     case ctBOGEY:
-      verticalZProj = (Propagate->GetTb2l()*mTGear*FGColumnVector3(0.,0.,1.))(eZ);
+      compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0;
-      compressLength = verticalZProj > 0.0 ? -height / verticalZProj : 0.0;
       break;
     case ctSTRUCTURE:
-      verticalZProj = -(Propagate->GetTec2l()*normal)(eZ);
+      compressLength = height * normalZ / DotProduct(normal, normal);
-      compressLength = fabs(verticalZProj) > 0.0 ? -height / verticalZProj : 0.0;
       break;
     }
 
@@ -343,13 +310,22 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
 
       WOW = true;
 
-      // [The next equation should really use the vector to the contact patch of
+      // The following equations use the vector to the tire contact patch
-      // the tire including the strut compression and not the original vWhlBodyVec.]
+      // including the strut compression.
+      FGColumnVector3 vWhlDisplVec;
 
-      FGColumnVector3 vWhlDisplVec = mTGear * FGColumnVector3(0., 0., compressLength);
+      switch(eContactType) {
-      FGColumnVector3 vWhlContactVec = vWhlBodyVec - vWhlDisplVec;
+      case ctBOGEY:
-      vActingXYZn = vXYZn - Tb2s * vWhlDisplVec;
+        vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength);
-      FGColumnVector3 vBodyWhlVel  = Propagate->GetPQR() * vWhlContactVec;
+        break;
+      case ctSTRUCTURE:
+        vWhlDisplVec = compressLength * vGroundNormal;
+        break;
+      }
+
+      FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec;
+      vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
+      FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec;
       vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel;
 
       vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
@@ -360,47 +336,22 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
 
       vLocalWhlVel = Transform().Transposed() * vBodyWhlVel;
 
-      switch (eContactType) {
+      compressSpeed = -vLocalWhlVel(eX);
-      case ctBOGEY:
+      if (eContactType == ctBOGEY)
-        // Compression speed along the strut
+        compressSpeed /= LGearProj;
-        compressSpeed = -vWhlVelVec(eZ);
-      case ctSTRUCTURE:
-        // Compression speed along the ground normal
-        compressSpeed = -vLocalWhlVel(eX);
-      }
 
       ComputeVerticalStrutForce();
 
-      // Compute the forces in the wheel ground plane.
+      // Compute the friction coefficients in the wheel ground plane.
       if (eContactType == ctBOGEY) {
         ComputeSlipAngle();
         ComputeBrakeForceCoefficient();
         ComputeSideForceCoefficient();
-        double sign = vLocalWhlVel(eY)>0?1.0:(vLocalWhlVel(eY)<0?-1.0:0.0);
-        vFn(eY) = - ((1.0 - TirePressureNorm) * 30 + vFn(eX) * BrakeFCoeff) * sign;
-        vFn(eZ) = vFn(eX) * FCoeff;
-      }
-      else if (eContactType == ctSTRUCTURE) {
-        FGColumnVector3 vSlipVec = vLocalWhlVel;
-        vSlipVec(eX) = 0.;
-        vSlipVec.Normalize();
-        vFn -= staticFCoeff * vFn(eX) * vSlipVec;
       }
 
-      // Lag and attenuate the XY-plane forces dependent on velocity. This code
+      // Prepare the Jacobians and the Lagrange multipliers for later friction
-      // uses a lag filter, C/(s + C) where "C" is the filter coefficient. When
+      // forces calculations.
-      // "C" is chosen at the frame rate (in Hz), the jittering is significantly
+      ComputeJacobian(vWhlContactVec);
-      // reduced. This is because the jitter is present *at* the execution rate.
-      // If a coefficient is set to something equal to or less than zero, the
-      // filter is bypassed.
-
-      if (LongForceLagFilterCoeff > 0) vFn(eY) = LongForceFilter.execute(vFn(eY));
-      if (LatForceLagFilterCoeff > 0)  vFn(eZ) = LatForceFilter.execute(vFn(eZ));
-
-      if ((fabs(vLocalWhlVel(eY)) <= RFRV) && RFRV > 0) vFn(eY) *= fabs(vLocalWhlVel(eY))/RFRV;
-      if ((fabs(vLocalWhlVel(eZ)) <= SFRV) && SFRV > 0) vFn(eZ) *= fabs(vLocalWhlVel(eZ))/SFRV;
-
-      // End section for attenuating gear jitter
 
     } else { // Gear is NOT compressed
 
@@ -491,14 +442,13 @@ void FGLGear::ComputeRetractionState(void)
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Calculate tire slip angle.
 
 void FGLGear::ComputeSlipAngle(void)
 {
-  // Calculate tire slip angle.
+// Check that the speed is non-null otherwise use the current angle
-  WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
+  if (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)
-
+    WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
-  // Filter the wheel slip angle
-  if (WheelSlipLagFilterCoeff > 0) WheelSlip = WheelSlipFilter.execute(WheelSlip);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -519,7 +469,7 @@ void FGLGear::ComputeSteeringAngle(void)
       SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
     else {
       // Check that the speed is non-null otherwise use the current angle
-      if (vWhlVelVec.Magnitude(eX,eY) > 1E-3)
+      if (vWhlVelVec.Magnitude(eX,eY) > 0.1)
         SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
     }
     break;
@@ -576,20 +526,18 @@ void FGLGear::InitializeReporting(void)
 
 void FGLGear::ReportTakeoffOrLanding(void)
 {
-  double deltaT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
-
   if (FirstContact)
-    LandingDistanceTraveled += Auxiliary->GetVground()*deltaT;
+    LandingDistanceTraveled += Auxiliary->GetVground()*dT;
 
   if (StartedGroundRun) {
-    TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*deltaT;
+    TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*dT;
-    if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*deltaT;
+    if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*dT;
   }
 
   if ( ReportEnable
        && Auxiliary->GetVground() <= 0.05
        && !LandingReported
-       && fdmex->GetGroundReactions()->GetWOW())
+       && GroundReactions->GetWOW())
   {
     if (debug_lvl > 0) Report(erLand);
   }
@@ -597,7 +545,7 @@ void FGLGear::ReportTakeoffOrLanding(void)
   if ( ReportEnable
        && !TakeoffReported
        && (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0
-       && !fdmex->GetGroundReactions()->GetWOW())
+       && !GroundReactions->GetWOW())
   {
     if (debug_lvl > 0) Report(erTakeoff);
   }
@@ -738,6 +686,99 @@ double FGLGear::GetGearUnitPos(void)
   return GearPos;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Compute the jacobian entries for the friction forces resolution later
+// in FGPropagate
+
+void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec)
+{
+  // When the point of contact is moving, dynamic friction is used
+  // This type of friction is limited to ctSTRUCTURE elements because their
+  // friction coefficient is the same in every directions
+  if ((eContactType == ctSTRUCTURE) && (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)) {
+    FGColumnVector3 velocityDirection = vLocalWhlVel;
+
+    StaticFriction = false;
+
+    velocityDirection(eX) = 0.;
+    velocityDirection.Normalize();
+
+    LMultiplier[ftDynamic].ForceJacobian = Transform()*velocityDirection;
+    LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian;
+    LMultiplier[ftDynamic].Max = 0.;
+    LMultiplier[ftDynamic].Min = -fabs(dynamicFCoeff * vFn(eX));
+    LMultiplier[ftDynamic].value = Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max);
+  }
+  else {
+    // Static friction is used for ctSTRUCTURE when the contact point is not moving.
+    // It is always used for ctBOGEY elements because the friction coefficients
+    // of a tyre depend on the direction of the movement (roll & side directions).
+    // This cannot be handled properly by the so-called "dynamic friction".
+    StaticFriction = true;
+
+    LMultiplier[ftRoll].ForceJacobian = Transform()*FGColumnVector3(0.,1.,0.);
+    LMultiplier[ftSide].ForceJacobian = Transform()*FGColumnVector3(0.,0.,1.);
+    LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian;
+    LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
+
+    switch(eContactType) {
+    case ctBOGEY:
+      LMultiplier[ftRoll].Max = fabs(BrakeFCoeff * vFn(eX));
+      LMultiplier[ftSide].Max = fabs(FCoeff * vFn(eX));
+      break;
+    case ctSTRUCTURE:
+      LMultiplier[ftRoll].Max = fabs(staticFCoeff * vFn(eX));
+      LMultiplier[ftSide].Max = fabs(staticFCoeff * vFn(eX));
+      break;
+    }
+
+    LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max;
+    LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max;
+    LMultiplier[ftRoll].value = Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max);
+    LMultiplier[ftSide].value = Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max);
+  }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// This function is used by the MultiplierIterator class to enumerate the
+// Lagrange multipliers of a landing gear. This allows to encapsulate the storage
+// of the multipliers in FGLGear without exposing it. From an outside point of
+// view, each FGLGear instance has a number of Lagrange multipliers which can be
+// accessed through this routine without knowing the exact constraint which they
+// model.
+
+FGPropagate::LagrangeMultiplier* FGLGear::GetMultiplierEntry(int entry)
+{
+  switch(entry) {
+  case 0:
+    if (StaticFriction)
+      return &LMultiplier[ftRoll];
+    else
+      return &LMultiplier[ftDynamic];
+  case 1:
+    if (StaticFriction)
+      return &LMultiplier[ftSide];
+  default:
+    return NULL;
+  }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// This routine is called after the Lagrange multiplier has been computed. The
+// friction forces of the landing gear are then updated accordingly.
+FGColumnVector3& FGLGear::UpdateForces(void)
+{
+  if (StaticFriction) {
+    vFn(eY) = LMultiplier[ftRoll].value;
+    vFn(eZ) = LMultiplier[ftSide].value;
+  }
+  else
+    vFn += LMultiplier[ftDynamic].value * (Transform ().Transposed() * LMultiplier[ftDynamic].ForceJacobian);
+
+  // Return the updated force in the body frame
+  return FGForce::GetBodyForces();
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGLGear::bind(void)
@@ -807,11 +848,11 @@ void FGLGear::Report(ReportType repType)
          << " ft,     " << TakeoffDistanceTraveled*0.3048  << " meters"  << endl;
     cout << "  Distance traveled (over 50'):     " << TakeoffDistanceTraveled50ft
          << " ft,     " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
-    cout << "  [Altitude (ASL): " << fdmex->GetPropagate()->GetAltitudeASL() << " ft. / "
+    cout << "  [Altitude (ASL): " << Propagate->GetAltitudeASL() << " ft. / "
-         << fdmex->GetPropagate()->GetAltitudeASLmeters() << " m  | Temperature: "
+         << Propagate->GetAltitudeASLmeters() << " m  | Temperature: "
          << fdmex->GetAtmosphere()->GetTemperature() - 459.67 << " F / "
          << RankineToCelsius(fdmex->GetAtmosphere()->GetTemperature()) << " C]" << endl;
-    cout << "  [Velocity (KCAS): " << fdmex->GetAuxiliary()->GetVcalibratedKTS() << "]" << endl;
+    cout << "  [Velocity (KCAS): " << Auxiliary->GetVcalibratedKTS() << "]" << endl;
     TakeoffReported = true;
     break;
   case erNone:
@@ -866,9 +907,6 @@ void FGLGear::Debug(int from)
         cout << "      Grouping:         " << sBrakeGroup   << endl;
         cout << "      Max Steer Angle:  " << maxSteerAngle << endl;
         cout << "      Retractable:      " << isRetractable  << endl;
-        cout << "      Relaxation Velocities:" << endl;
-        cout << "        Rolling:          " << RFRV << endl;
-        cout << "        Side:             " << SFRV << endl;
       }
     }
   }

src/FDM/JSBSim/models/FGLGear.h

@@ -39,6 +39,7 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "models/propulsion/FGForce.h"
+#include "models/FGPropagate.h"
 #include "math/FGColumnVector3.h"
 #include <string>
 
@@ -46,7 +47,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_LGEAR "$Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $"
+#define ID_LGEAR "$Id: FGLGear.h,v 1.40 2010/07/30 11:50:01 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -177,19 +178,10 @@ CLASS DOCUMENTATION
             <retractable>{0 | 1}</retractable>
             <table type="{CORNERING_COEFF}">
             </table>
-            <relaxation_velocity>
-               <rolling unit="{FT/SEC | KTS | M/S}"> {number} </rolling>
-               <side unit="{FT/SEC | KTS | M/S}"> {number} </side>
-            </relaxation_velocity>
-            <force_lag_filter>
-               <rolling> {number} </rolling>
-               <side> {number} </side>
-            </force_lag_filter>
-            <wheel_slip_filter> {number} </wheel_slip_filter>  
         </contact>
 @endcode
     @author Jon S. Berndt
-    @version $Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $
+    @version $Id: FGLGear.h,v 1.40 2010/07/30 11:50:01 jberndt Exp $
     @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
      NASA-Ames", NASA CR-2497, January 1975
     @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
@@ -215,6 +207,8 @@ public:
   enum ReportType {erNone=0, erTakeoff, erLand};
   /// Damping types
   enum DampType {dtLinear=0, dtSquare};
+  /// Friction types
+  enum FrictionType {ftRoll=0, ftSide, ftDynamic};
   /** Constructor
       @param el a pointer to the XML element that contains the CONTACT info.
       @param Executive a pointer to the parent executive object
@@ -289,6 +283,9 @@ public:
   bool IsBogey(void) const             { return (eContactType == ctBOGEY);}
   double GetGearUnitPos(void);
   double GetSteerAngleDeg(void) const { return radtodeg*SteerAngle; }
+  FGPropagate::LagrangeMultiplier* GetMultiplierEntry(int entry);
+  void SetLagrangeMultiplier(double lambda, int entry);
+  FGColumnVector3& UpdateForces(void);
 
   void bind(void);
 
@@ -338,6 +335,7 @@ private:
   bool GearUp, GearDown;
   bool Servicable;
   bool Castered;
+  bool StaticFriction;
   std::string name;
   std::string sSteerType;
   std::string sBrakeGroup;
@@ -350,22 +348,14 @@ private:
   DampType    eDampType;
   DampType    eDampTypeRebound;
   double  maxSteerAngle;
-  double RFRV;  // Rolling force relaxation velocity
-  double SFRV;  // Side force relaxation velocity
-  double LongForceLagFilterCoeff; // Longitudinal Force Lag Filter Coefficient
-  double LatForceLagFilterCoeff; // Lateral Force Lag Filter Coefficient
-  double WheelSlipLagFilterCoeff; // Wheel slip angle lag filter coefficient
 
-  Filter LongForceFilter;
+  FGPropagate::LagrangeMultiplier LMultiplier[3];
-  Filter LatForceFilter;
-  Filter WheelSlipFilter;
 
-  FGState*       State;
+  FGAuxiliary*       Auxiliary;
-  FGAircraft*    Aircraft;
+  FGPropagate*       Propagate;
-  FGPropagate*   Propagate;
+  FGFCS*             FCS;
-  FGAuxiliary*   Auxiliary;
+  FGMassBalance*     MassBalance;
-  FGFCS*         FCS;
+  FGGroundReactions* GroundReactions;
-  FGMassBalance* MassBalance;
 
   void ComputeRetractionState(void);
   void ComputeBrakeForceCoefficient(void);
@@ -374,6 +364,7 @@ private:
   void ComputeSideForceCoefficient(void);
   void ComputeVerticalStrutForce(void);
   void ComputeGroundCoordSys(void);
+  void ComputeJacobian(const FGColumnVector3& vWhlContactVec);
   void CrashDetect(void);
   void InitializeReporting(void);
   void ResetReporting(void);

src/FDM/JSBSim/models/FGPropagate.cpp

@@ -48,6 +48,7 @@ COMMENTS, REFERENCES,  and NOTES
     Wiley & Sons, 1979 ISBN 0-471-03032-5
 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
     1982 ISBN 0-471-08936-2
+[6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 INCLUDES
@@ -70,7 +71,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.55 2010/07/09 04:11:45 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.59 2010/07/30 11:50:01 jberndt Exp $";
 static const char *IdHdr = ID_PROPAGATE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -216,6 +217,7 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
   // Make an initial run and set past values
   CalculatePQRdot();           // Angular rate derivative
   CalculateUVWdot();           // Translational rate derivative
+  ResolveFrictionForces(0.);   // Update rate derivatives with friction forces
   CalculateQuatdot();          // Angular orientation derivative
   CalculateInertialVelocity(); // Translational position derivative
 
@@ -269,6 +271,7 @@ static int ctr;
   // Calculate state derivatives
   CalculatePQRdot();           // Angular rate derivative
   CalculateUVWdot();           // Translational rate derivative
+  ResolveFrictionForces(dt);   // Update rate derivatives with friction forces
   CalculateQuatdot();          // Angular orientation derivative
   CalculateInertialVelocity(); // Translational position derivative
 
@@ -304,8 +307,7 @@ static int ctr;
   VState.vLocation = Ti2ec*VState.vInertialPosition;
   RecomputeLocalTerrainRadius();
 
-  // Calculate current aircraft radius from center of planet
+  VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
-  VehicleRadius = VState.vInertialPosition.Magnitude();
   radInv = 1.0/VehicleRadius;
 
   VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
@@ -386,9 +388,7 @@ void FGPropagate::CalculateUVWdot(void)
   vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
 
   // Include Centripetal acceleration.
-  if (!GroundReactions->GetWOW() && Aircraft->GetHoldDown() == 0) {
+  vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
-    vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
-  }
 
   // Include Gravitation accel
   switch (gravType) {
@@ -464,11 +464,133 @@ void FGPropagate::Integrate( FGQuaternion& Integrand,
     break;
   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
     break;
-  case eNone: // do nothing, freeze translational rate
+  case eNone: // do nothing, freeze rotational rate
     break;
   }
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Resolves the contact forces just before integrating the EOM.
+// This routine is using Lagrange multipliers and the projected Gauss-Seidel
+// (PGS) method.
+// Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence", 
+//            February 22, 2005
+// In JSBSim there is only one rigid body (the aircraft) and there can be
+// multiple points of contact between the aircraft and the ground. As a
+// consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
+// in Catto's paper has been adapted accordingly.
+
+void FGPropagate::ResolveFrictionForces(double dt)
+{
+  const double invMass = 1.0 / MassBalance->GetMass();
+  const FGMatrix33& Jinv = MassBalance->GetJinv();
+  vector <FGColumnVector3> JacF, JacM;
+  FGColumnVector3 vdot, wdot;
+  FGColumnVector3 Fc, Mc;
+  int n = 0, i;
+
+  // Compiles data from the ground reactions to build up the jacobian matrix
+  for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
+    JacF.push_back((*it)->ForceJacobian);
+    JacM.push_back((*it)->MomentJacobian);
+  }
+
+  // If no gears are in contact with the ground then return
+  if (!n) return;
+
+  double *a = new double[n*n]; // Will contain J*M^-1*J^T
+  double *eta = new double[n];
+  double *lambda = new double[n];
+  double *lambdaMin = new double[n];
+  double *lambdaMax = new double[n];
+
+  // Initializes the Lagrange multipliers
+  i = 0;
+  for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
+    lambda[i] = (*it)->value;
+    lambdaMax[i] = (*it)->Max;
+    lambdaMin[i] = (*it)->Min;
+  }
+
+  vdot = vUVWdot;
+  wdot = vPQRdot;
+
+  if (dt > 0.) {
+    // First compute the ground velocity below the aircraft center of gravity
+    FGLocation contact;
+    FGColumnVector3 normal, cvel;
+    double t = FDMExec->GetSimTime();
+    double height = FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contact, normal, cvel);
+
+    // Instruct the algorithm to zero out the relative movement between the
+    // aircraft and the ground.
+    vdot += (VState.vUVW - Tec2b * cvel) / dt;
+    wdot += VState.vPQR / dt;
+  }
+
+  // Assemble the linear system of equations
+  for (i=0; i < n; i++) {
+    for (int j=0; j < i; j++)
+      a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
+    for (int j=i; j < n; j++)
+      a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
+  }
+
+  // Prepare the linear system for the Gauss-Seidel algorithm :
+  // divide every line of 'a' and eta by a[i,i]. This is in order to save
+  // a division computation at each iteration of Gauss-Seidel.
+  for (i=0; i < n; i++) {
+    double d = 1.0 / a[i*n+i];
+
+    eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
+    for (int j=0; j < n; j++)
+      a[i*n+j] *= d;
+  }
+
+  // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
+  for (int iter=0; iter < 50; iter++) {
+    double norm = 0.;
+
+    for (i=0; i < n; i++) {
+      double lambda0 = lambda[i];
+      double dlambda = eta[i];
+      
+      for (int j=0; j < n; j++)
+        dlambda -= a[i*n+j]*lambda[j];
+
+      lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
+      dlambda = lambda[i] - lambda0;
+
+      norm += fabs(dlambda);
+    }
+
+    if (norm < 1E-5) break;
+  }
+
+  // Calculate the total friction forces and moments
+
+  Fc.InitMatrix();
+  Mc.InitMatrix();
+
+  for (i=0; i< n; i++) {
+    Fc += lambda[i]*JacF[i];
+    Mc += lambda[i]*JacM[i];
+  }
+
+  vUVWdot += invMass * Fc;
+  vPQRdot += Jinv * Mc;
+
+  // Save the value of the Lagrange multipliers to accelerate the convergence
+  // of the Gauss-Seidel algorithm at next iteration.
+  i = 0;
+  for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
+    (*it)->value = lambda[i++];
+
+  GroundReactions->UpdateForcesAndMoments();
+
+  delete a, eta, lambda, lambdaMin, lambdaMax;
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
@@ -488,9 +610,8 @@ void FGPropagate::RecomputeLocalTerrainRadius(void)
   double t = FDMExec->GetSimTime();
 
   // Get the LocalTerrain radius.
-//  FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
+  FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
-//  LocalTerrainRadius = contactloc.GetRadius();
+  LocalTerrainRadius = contactloc.GetRadius(); 
-  LocalTerrainRadius = FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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.43 2010/07/25 15:35:11 jberndt 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.43 2010/07/25 15:35:11 jberndt Exp $
   */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -181,10 +181,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.
@@ -568,6 +564,14 @@ public:
     VState.vLocation -= vDeltaXYZEC;
   }
 
+  struct LagrangeMultiplier {
+    FGColumnVector3 ForceJacobian;
+    FGColumnVector3 MomentJacobian;
+    double Min;
+    double Max;
+    double value;
+  };
+
 private:
 
 // state vector
@@ -606,6 +610,11 @@ private:
   eIntegrateType integrator_translational_position;
   int gravType;
 
+  void CalculatePQRdot(void);
+  void CalculateQuatdot(void);
+  void CalculateInertialVelocity(void);
+  void CalculateUVWdot(void);
+
   void Integrate( FGColumnVector3& Integrand,
                   FGColumnVector3& Val,
                   deque <FGColumnVector3>& ValDot,
@@ -618,6 +627,8 @@ private:
                   double dt,
                   eIntegrateType integration_type);
 
+  void ResolveFrictionForces(double dt);
+
   void bind(void);
   void Debug(int from);
 };