FG_HAVE_STD_INCLUDES -> SG_HAVE_STD_INCLUDES

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

diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp
index b7e9e90b80f42cfb1b8fbd61c749ead17ca83525..aa706890ee6bb2847925d3fed1546da8e118f056 100644 (file)
--- a/src/FDM/JSBSim/FGTranslation.cpp
+++ b/src/FDM/JSBSim/FGTranslation.cpp
@@ -1,4 +1,4 @@
-/*******************************************************************************
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  Module:       FGTranslation.cpp
  Author:       Jon Berndt
@@ -35,9 +35,9 @@ HISTORY
  7/23/99   TP    Added data member and modified Run and PutState to calcuate 
                               Mach number
  
-********************************************************************************
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 COMMENTS, REFERENCES,  and NOTES
-********************************************************************************
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
     Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
     School, January 1994
@@ -53,9 +53,9 @@ COMMENTS, REFERENCES,  and NOTES
   The order of rotations used in this class corresponds to a 3-2-1 sequence,
   or Y-P-R, or Z-Y-X, if you prefer.
  
-********************************************************************************
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 INCLUDES
-*******************************************************************************/
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGTranslation.h"
 #include "FGRotation.h"
@@ -68,9 +68,12 @@ INCLUDES
 #include "FGAuxiliary.h"
 #include "FGOutput.h"
 
-/*******************************************************************************
-************************************ CODE **************************************
-*******************************************************************************/
+static const char *IdSrc = "$Header$";
+static const char *IdHdr = ID_TRANSLATION;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 
 FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex),
@@ -87,14 +90,15 @@ FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex),
   Vt = 0.0;
   Mach = 0.0;
   alpha = beta = 0.0;
+  adot = bdot = 0.0;
   rho = 0.002378;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGTranslation::~FGTranslation(void) {}
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 bool FGTranslation::Run(void) {
   static FGColumnVector vlastUVWdot(3);
@@ -115,19 +119,37 @@ bool FGTranslation::Run(void) {
     mVel(3,3) =  0.0;
 
     vUVWdot = mVel*vPQR + vForces/Mass;
-
-    vNcg = vUVWdot * INVGRAVITY;
+    
+    vNcg=vUVWdot*INVGRAVITY;
 
     vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot) + vWindUVW;
-
+    
     Vt = vUVW.Magnitude();
 
     if (vUVW(eW) != 0.0)
       alpha = vUVW(eU)*vUVW(eU) > 0.0 ? atan2(vUVW(eW), vUVW(eU)) : 0.0;
     if (vUVW(eV) != 0.0)
       beta = vUVW(eU)*vUVW(eU)+vUVW(eW)*vUVW(eW) > 0.0 ? atan2(vUVW(eV),
-             (fabs(vUVW(eU))/vUVW(eU))*sqrt(vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW))) : 0.0;
-
+             sqrt(vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW))) : 0.0;
+    
+     
+       
+         // stolen, quite shamelessly, from LaRCsim
+    float mUW = (vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW));
+    float signU=1;
+    if (vUVW(eU) != 0.0)
+                 signU = vUVW(eU)/fabs(vUVW(eU));
+
+         if( (mUW == 0.0) || (Vt == 0.0) ) {
+                 adot = 0.0;
+                 bdot = 0.0;
+         } else {
+                 adot = (vUVW(eU)*vUVWdot(eW) - vUVW(eW)*vUVWdot(eU))/mUW;
+                 bdot = (signU*mUW*vUVWdot(eV) - vUVW(eV)*(vUVW(eU)*vUVWdot(eU) 
+              + vUVW(eW)*vUVWdot(eW)))/(Vt*Vt*sqrt(mUW));
+         }
+    //
+    
     qbar = 0.5*rho*Vt*Vt;
 
     Mach = Vt / State->Geta();
@@ -139,7 +161,7 @@ bool FGTranslation::Run(void) {
   return false;
 }
 
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGTranslation::GetState(void) {
   dt = State->Getdt();