- 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
-// (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;