+ case eBuss1:
+ {
+ // This is the first order method as described in Samuel R. Buss paper[6].
+ // The formula from Buss' paper is transposed below to quaternions and is
+ // actually the exact solution of the quaternion differential equation
+ // qdot = 1/2*w*q when w is constant.
+ Integrand = Integrand * QExp(0.5 * dt * VState.vPQRi);
+ }
+ return; // No need to normalize since the quaternion exponential is always normal
+ case eBuss2:
+ {
+ // This is the 'augmented second-order method' from S.R. Buss paper [6].
+ // Unlike Runge-Kutta or Adams-Bashforth, it is a one-pass second-order
+ // method (see reference [6]).
+ FGColumnVector3 wi = VState.vPQRi;
+ FGColumnVector3 wdoti = in.vPQRidot;
+ FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi;
+ Integrand = Integrand * QExp(0.5 * dt * omega);
+ }
+ return; // No need to normalize since the quaternion exponential is always normal
+ case eLocalLinearization:
+ {
+ // This is the local linearization algorithm of Barker et al. (see ref. [7])
+ // It is also a one-pass second-order method. The code below is based on the
+ // more compact formulation issued from equation (107) of ref. [8]. The
+ // constants C1, C2, C3 and C4 have the same value than those in ref. [7] pp. 11
+ FGColumnVector3 wi = 0.5 * VState.vPQRi;
+ FGColumnVector3 wdoti = 0.5 * in.vPQRidot;
+ double omegak2 = DotProduct(VState.vPQRi, VState.vPQRi);
+ double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6;
+ double rhok = 0.5 * dt * omegak;
+ double C1 = cos(rhok);
+ double C2 = 2.0 * sin(rhok) / omegak;
+ double C3 = 4.0 * (1.0 - C1) / (omegak*omegak);
+ double C4 = 4.0 * (dt - C2) / (omegak*omegak);
+ FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti;
+ FGQuaternion q;
+
+ q(1) = C1 - C4*DotProduct(wi, wdoti);
+ q(2) = Omega(eP);
+ q(3) = Omega(eQ);
+ q(4) = Omega(eR);
+
+ Integrand = Integrand * q;
+
+ /* Cross check with ref. [7] pp.11-12 formulas and code pp. 20
+ double pk = VState.vPQRi(eP);
+ double qk = VState.vPQRi(eQ);
+ double rk = VState.vPQRi(eR);
+ double pdotk = in.vPQRidot(eP);
+ double qdotk = in.vPQRidot(eQ);
+ double rdotk = in.vPQRidot(eR);
+ double Ap = -0.25 * (pk*pdotk + qk*qdotk + rk*rdotk);
+ double Bp = 0.25 * (pk*qdotk - qk*pdotk);
+ double Cp = 0.25 * (pdotk*rk - pk*rdotk);
+ double Dp = 0.25 * (qk*rdotk - qdotk*rk);
+ double C2p = sin(rhok) / omegak;
+ double C3p = 2.0 * (1.0 - cos(rhok)) / (omegak*omegak);
+ double H = C1 + C4 * Ap;
+ double G = -C2p*rk - C3p*rdotk + C4*Bp;
+ double J = C2p*qk + C3p*qdotk - C4*Cp;
+ double K = C2p*pk + C3p*pdotk - C4*Dp;
+
+ cout << "q: " << q << endl;
+
+ // Warning! In the paper of Barker et al. the quaternion components are not
+ // ordered the same way as in JSBSim (see equations (2) and (3) of ref. [7]
+ // as well as the comment just below equation (3))
+ cout << "FORTRAN: " << H << " , " << K << " , " << J << " , " << -G << endl;*/
+ }
+ break; // The quaternion q is not normal so the normalization needs to be done.