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 8a80034a211abbbe69e39ad8b3f09303af584252..327be431093915cd67720e305ed94a514793d9a6 100644 (file)
--- a/src/FDM/JSBSim/FGRotation.cpp
+++ b/src/FDM/JSBSim/FGRotation.cpp
@@ -66,12 +66,6 @@ INCLUDES
 #include "FGAuxiliary.h"
 #include "FGOutput.h"
 
-#ifndef M_PI
-/* get a definition for pi */
-#include <Include/fg_constants.h>
-#define M_PI FG_PI
-#endif
-
 /*******************************************************************************
 ************************************ CODE **************************************
 *******************************************************************************/
@@ -81,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;
 }
 
 /******************************************************************************/
@@ -97,6 +94,7 @@ FGRotation::~FGRotation(void)
 bool FGRotation::Run(void)
 {
     float L2, N1;
+    float tTheta;
     static FGColumnVector vlastPQRdot(3);
 
     if (!FGModel::Run()) {
@@ -114,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 {