+ 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
+// (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.
+// The friction forces are resolved in the body frame relative to the origin
+// (Earth center).
+
+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;
+
+ // 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> rhs(n);
+
+ // Assemble the linear system of equations
+ 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 :
+ // 1. Compute the right hand side member 'rhs'
+ // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
+ // a division computation at each iteration of Gauss-Seidel.
+ for (int i=0; i < n; i++) {
+ double d = 1.0 / a[i*n+i];
+
+ rhs[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 (int i=0; i < n; i++) {
+ double lambda0 = lambda[i];
+ double dlambda = rhs[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;