+ double mass = FDMExec->GetMassBalance()->GetMass(); // mass
+ const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
+
+ vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
+
+ // Include Centripetal acceleration.
+ vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
+
+ // Include Gravitation accel
+ switch (gravType) {
+ case gtStandard:
+ vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
+ break;
+ case gtWGS84:
+ vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
+ break;
+ }
+
+ vUVWdot += vGravAccel;
+ vUVWidot = Tb2i * (vForces/mass + vGravAccel);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // Transform the velocity vector of the body relative to the origin (Earth
+ // center) to be expressed in the inertial frame, and add the vehicle velocity
+ // contribution due to the rotation of the planet.
+ // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
+ // Second edition (2004), eqn 1.5-16c (page 50)
+
+void FGPropagate::CalculateInertialVelocity(void)
+{
+ VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // Transform the velocity vector of the inertial frame to be expressed in the
+ // body frame relative to the origin (Earth center), and substract the vehicle
+ // velocity contribution due to the rotation of the planet.
+
+void FGPropagate::CalculateUVW(void)
+{
+ VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::Integrate( FGColumnVector3& Integrand,
+ FGColumnVector3& Val,
+ deque <FGColumnVector3>& ValDot,
+ double dt,
+ eIntegrateType integration_type)
+{
+ ValDot.push_front(Val);
+ ValDot.pop_back();
+
+ switch(integration_type) {
+ case eRectEuler: Integrand += dt*ValDot[0];
+ break;
+ case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
+ break;
+ case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
+ break;
+ case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
+ 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
+ break;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::Integrate( FGQuaternion& Integrand,
+ FGQuaternion& Val,
+ deque <FGQuaternion>& ValDot,
+ double dt,
+ eIntegrateType integration_type)
+{
+ ValDot.push_front(Val);
+ ValDot.pop_back();
+
+ switch(integration_type) {
+ case eRectEuler: Integrand += dt*ValDot[0];
+ break;
+ case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
+ break;
+ case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
+ break;
+ case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
+ 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 rotational rate
+ break;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// 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
+// (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