Quick update with Bertrand Coconnier's fixes

src/FDM/JSBSim/math/FGQuaternion.cpp

@@ -57,7 +57,7 @@ using std::endl;
 
 namespace JSBSim {
   
-static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.16 2010/06/30 03:13:40 jberndt Exp $";
+static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.17 2010/11/28 13:15:26 bcoconni Exp $";
 static const char *IdHdr = ID_QUATERNION;
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -146,8 +146,6 @@ FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
   data[2] = t*(m(3,1) - m(1,3));
   data[3] = t*(m(1,2) - m(2,1));
 
-  ComputeDerivedUnconditional();
-
   Normalize();
 }
 

src/FDM/JSBSim/models/FGLGear.cpp

@@ -62,7 +62,7 @@ DEFINITIONS
 GLOBAL DATA
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-static const char *IdSrc = "$Id: FGLGear.cpp,v 1.78 2010/10/07 03:45:40 jberndt Exp $";
+static const char *IdSrc = "$Id: FGLGear.cpp,v 1.79 2010/11/28 13:20:47 bcoconni Exp $";
 static const char *IdHdr = ID_LGEAR;
 
 // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
@@ -806,6 +806,10 @@ void FGLGear::bind(void)
 
     property_name = base_property_name + "/static_friction_coeff";
     fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff );
+    property_name = base_property_name + "/rolling_friction_coeff";
+    fdmex->GetPropertyManager()->Tie( property_name.c_str(), &rollingFCoeff );
+    property_name = base_property_name + "/dynamic_friction_coeff";
+    fdmex->GetPropertyManager()->Tie( property_name.c_str(), &dynamicFCoeff );
 
     if (eSteerType == stCaster) {
       property_name = base_property_name + "/steering-angle-deg";

src/FDM/JSBSim/models/FGPropagate.cpp

@@ -71,7 +71,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.73 2010/11/18 12:38:06 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.74 2010/11/28 13:02:43 bcoconni Exp $";
 static const char *IdHdr = ID_PROPAGATE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -454,6 +454,55 @@ void FGPropagate::Integrate( FGQuaternion& Integrand,
   }
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Evaluates the rates (translation or rotation) that the friction forces have
+// to resist to. This includes the external forces and moments as well as the
+// relative movement between the aircraft and the ground.
+// Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
+// this algorithm has been adapted to handle the multistep algorithms that
+// JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
+// to handle the multistep integration schemes adds some complexity but it
+// significantly helps stabilizing the friction forces.
+
+void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
+                                         const FGColumnVector3& Val,
+                                         const FGColumnVector3& ValDot,
+                                         const FGColumnVector3& LocalTerrainVal,
+                                         deque <FGColumnVector3>& dqValDot,
+                                         const double dt,
+                                         const eIntegrateType integration_type)
+{
+  switch(integration_type) {
+  case eAdamsBashforth4:
+    vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
+    if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+      vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
+    break;
+  case eAdamsBashforth3:
+    vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
+    if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+      vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
+    break;
+  case eAdamsBashforth2:
+    vdot = ValDot - Ti2b * dqValDot[0]/3.;
+    if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+      vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
+    break;
+  case eTrapezoidal:
+    vdot = ValDot + Ti2b * dqValDot[0];
+    if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+      vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
+    break;
+  case eRectEuler:
+    vdot = ValDot;
+    if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+      vdot += (Val - Tec2b * LocalTerrainVal) / dt;
+    break;
+  case eNone:
+    break;
+  }
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 // Resolves the contact forces just before integrating the EOM.
 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
@@ -472,58 +521,52 @@ void FGPropagate::ResolveFrictionForces(double dt)
   const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
   const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
   vector <FGColumnVector3> JacF, JacM;
+  vector<double> lambda, lambdaMin, lambdaMax;
   FGColumnVector3 vdot, wdot;
   FGColumnVector3 Fc, Mc;
-  int n = 0, i;
+  int n = 0;
 
   // Compiles data from the ground reactions to build up the jacobian matrix
   for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
     JacF.push_back((*it)->ForceJacobian);
     JacM.push_back((*it)->MomentJacobian);
+    lambda.push_back((*it)->value);
+    lambdaMax.push_back((*it)->Max);
+    lambdaMin.push_back((*it)->Min);
   }
 
   // If no gears are in contact with the ground then return
   if (!n) return;
 
   vector<double> a(n*n); // Will contain J*M^-1*J^T
-  vector<double> eta(n);
+  vector<double> rhs(n);
-  vector<double> lambda(n);
-  vector<double> lambdaMin(n);
-  vector<double> lambdaMax(n);
-
-  // Initializes the Lagrange multipliers
-  i = 0;
-  for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, i++) {
-    lambda[i] = (*it)->value;
-    lambdaMax[i] = (*it)->Max;
-    lambdaMin[i] = (*it)->Min;
-  }
-
-  vdot = vUVWdot;
-  wdot = vPQRdot;
-
-  if (dt > 0.) {
-    // Instruct the algorithm to zero out the relative movement between the
-    // aircraft and the ground.
-    vdot += (VState.vUVW - Tec2b * LocalTerrainVelocity) / dt;
-    wdot += (VState.vPQR - Tec2b * LocalTerrainAngularVelocity) / dt;
-  }
 
   // Assemble the linear system of equations
-  for (i=0; i < n; i++) {
+  for (int 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]);
   }
 
+  // Assemble the RHS member
+
+  // Translation
+  EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
+                         VState.dqUVWidot, dt, integrator_translational_rate);
+
+  // Rotation
+  EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
+                         VState.dqPQRidot, dt, integrator_rotational_rate);
+
   // 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
+  // 1. Compute the right hand side member 'rhs'
+  // 2. Divide every line of 'a' and 'lhs' 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++) {
+  for (int i=0; i < n; i++) {
     double d = 1.0 / a[i*n+i];
 
-    eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
+    rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
     for (int j=0; j < n; j++)
       a[i*n+j] *= d;
   }
@@ -532,9 +575,9 @@ void FGPropagate::ResolveFrictionForces(double dt)
   for (int iter=0; iter < 50; iter++) {
     double norm = 0.;
 
-    for (i=0; i < n; i++) {
+    for (int i=0; i < n; i++) {
       double lambda0 = lambda[i];
-      double dlambda = eta[i];
+      double dlambda = rhs[i];
       
       for (int j=0; j < n; j++)
         dlambda -= a[i*n+j]*lambda[j];
@@ -553,7 +596,7 @@ void FGPropagate::ResolveFrictionForces(double dt)
   Fc.InitMatrix();
   Mc.InitMatrix();
 
-  for (i=0; i< n; i++) {
+  for (int i=0; i< n; i++) {
     Fc += lambda[i]*JacF[i];
     Mc += lambda[i]*JacM[i];
   }
@@ -565,7 +608,7 @@ void FGPropagate::ResolveFrictionForces(double dt)
 
   // Save the value of the Lagrange multipliers to accelerate the convergence
   // of the Gauss-Seidel algorithm at next iteration.
-  i = 0;
+  int i = 0;
   for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
     (*it)->value = lambda[i++];
 

src/FDM/JSBSim/models/FGPropagate.h

@@ -49,7 +49,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.52 2010/10/31 04:48:46 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.52 2010/10/31 04:48:46 jberndt Exp $
+    @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $
   */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -666,6 +666,14 @@ 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);