-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// 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 / 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;
-
- vector<double> a(n*n); // Will contain J*M^-1*J^T
- vector<double> eta(n);
- vector<double> lambda(n);
- vector<double> lambdaMin(n);
- vector<double> lambdaMax(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.) {
- // Instruct the algorithm to zero out the relative movement between the
- // aircraft and the ground.
- vdot += (VState.vUVW - Tec2b * LocalTerrainVelocity) / 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;
- vUVWidot += invMass * Tb2i * 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++];