builddir -> srcdir so builds can be done outside the master source directory.

[flightgear.git] / src / FDM / JSBSim / FGRotation.cpp

diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp
index 6041ef4ec451926fb83ca415d5ce68a4a93cbb8b..327be431093915cd67720e305ed94a514793d9a6 100644 (file)
--- a/src/FDM/JSBSim/FGRotation.cpp
+++ b/src/FDM/JSBSim/FGRotation.cpp
@@ -75,9 +75,12 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex),
         vPQR(3),
         vPQRdot(3),
         vEuler(3),
+        vEulerRates(3),
         vMoments(3)
 {
     Name = "FGRotation";
+    cTht=cPhi=cPsi=1.0;
+    sTht=sPhi=sPsi=0.0;
 }
 
 /******************************************************************************/
@@ -91,6 +94,7 @@ FGRotation::~FGRotation(void)
 bool FGRotation::Run(void)
 {
     float L2, N1;
+    float tTheta;
     static FGColumnVector vlastPQRdot(3);
 
     if (!FGModel::Run()) {
@@ -108,7 +112,20 @@ bool FGRotation::Run(void)
         State->IntegrateQuat(vPQR, rate);
         State->CalcMatrices();
         vEuler = State->CalcEuler();
-
+        
+        
+        cTht=cos(vEuler(eTht));   sTht=sin(vEuler(eTht));
+        cPhi=cos(vEuler(ePhi));   sPhi=sin(vEuler(ePhi));
+        cPsi=cos(vEuler(ePsi));   sPsi=sin(vEuler(ePsi));
+
+
+        vEulerRates(eTht) = vPQR(2)*cPhi - vPQR(3)*sPhi;
+        if(cTht != 0.0) {
+          tTheta=sTht/cTht; // what's cheaper: / or tan() ?
+          vEulerRates(ePhi) = vPQR(1) + (vPQR(2)*sPhi + vPQR(3)*cPhi)*tTheta;
+          vEulerRates(ePsi) = (vPQR(2)*sPhi + vPQR(3)*cPhi)/cTht;
+        }
+        
         vlastPQRdot = vPQRdot;
 
     } else {