+ // 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;
+ }
+
+ // Calculate the total friction forces and moments
+
+ Fc.InitMatrix();
+ Mc.InitMatrix();
+
+ for (int 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;
+ vPQRidot += Tb2i* Jinv * Mc;
+
+ // Save the value of the Lagrange multipliers to accelerate the convergence
+ // of the Gauss-Seidel algorithm at next iteration.
+ int i = 0;
+ for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
+ (*it)->value = lambda[i++];
+
+ FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::UpdateLocationMatrices(void)
+{
+ Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
+ Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
+ Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
+ Tl2i = Ti2l.Transposed(); // local to ECI transform
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::UpdateBodyMatrices(void)
+{
+ Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
+ Tb2i = Ti2b.Transposed(); // body to ECI frame transform
+ Tl2b = Ti2b*Tl2i; // local to body frame transform
+ Tb2l = Tl2b.Transposed(); // body to local frame transform
+ Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
+ Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
+ VState.qAttitudeECI = Qi;
+ VState.qAttitudeECI.Normalize();
+ UpdateBodyMatrices();
+ VState.qAttitudeLocal = Tl2b.GetQuaternion();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
+ VState.vInertialVelocity = Vi;
+ CalculateUVW();
+ vVel = Tb2l * VState.vUVW;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
+ VState.vPQRi_i = vRates;
+ VState.vPQRi = Ti2b * VState.vPQRi_i;
+ VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::InitializeDerivatives(void)
+{
+ // Make an initial run and set past values
+ CalculatePQRdot(); // Angular rate derivative
+ CalculateUVWdot(); // Translational rate derivative
+ ResolveFrictionForces(0.); // Update rate derivatives with friction forces
+ CalculateQuatdot(); // Angular orientation derivative
+ CalculateInertialVelocity(); // Translational position derivative
+
+ // Initialize past values deques
+ VState.dqPQRidot.clear();
+ VState.dqUVWidot.clear();
+ VState.dqInertialVelocity.clear();
+ VState.dqQtrndot.clear();
+ for (int i=0; i<4; i++) {
+ VState.dqPQRidot.push_front(vPQRidot);
+ VState.dqUVWidot.push_front(vUVWdot);
+ VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
+ VState.dqQtrndot.push_front(vQtrndot);
+ }