YASim: Moved Airplane::setupState to BodyEnvironment

src/FDM/YASim/Airplane.cpp

@@ -274,32 +274,11 @@ void Airplane::setFuelFraction(float frac)
     }
 }
 
-// used by runCruise, runApproach, solveHelicopter and in yasim-test
-void Airplane::setupState(const float aoa, const float speed, const float gla, State* s) const
-{
-    float cosAoA = Math::cos(aoa);
-    float sinAoA = Math::sin(aoa);
-    s->orient[0] =  cosAoA; s->orient[1] = 0; s->orient[2] = sinAoA;
-    s->orient[3] =       0; s->orient[4] = 1; s->orient[5] =      0;
-    s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
-
-    //? what is gla? v[1]=y, v[2]=z? should sin go to v2 instead v1?
-    s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
-
-    for(int i=0; i<3; i++)
-	s->pos[i] = s->rot[i] = s->acc[i] = s->racc[i] = 0;
-
-    // Put us 1m above the origin, or else the gravity computation in
-    // Model goes nuts
-    s->pos[2] = 1;
-}
-
 /**
  * @brief add contact point for crash detection 
  * used to add wingtips and fuselage nose and tail
  * 
  * @param pos ...
- * @return void
  */
 
 void Airplane::addContactPoint(float* pos)
@@ -777,7 +756,7 @@ void Airplane::loadControls(const Vector& controls)
 /// Helper for solve()
 void Airplane::runCruise()
 {
-    setupState(_cruiseAoA, _cruiseSpeed,_cruiseGlideAngle, &_cruiseState);
+    _cruiseState.setupState(_cruiseAoA, _cruiseSpeed,_cruiseGlideAngle);
     _model.setState(&_cruiseState);
     _model.setAtmosphere(_cruiseAtmo);
 
@@ -814,7 +793,7 @@ void Airplane::runCruise()
 /// Helper for solve()
 void Airplane::runApproach()
 {
-    setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
+    _approachState.setupState(_approachAoA, _approachSpeed,_approachGlideAngle);
     _model.setState(&_approachState);
     _model.setAtmosphere(_approachAtmo);
 
@@ -1074,7 +1053,7 @@ void Airplane::solveHelicopter()
         applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
         applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
     }
-    setupState(0,0,0, &_cruiseState);
+    _cruiseState.setupState(0,0,0);
     _model.setState(&_cruiseState);
     setupWeights(true);
     _controls.reset();

src/FDM/YASim/Airplane.hpp

@@ -100,7 +100,6 @@ public:
     float getApproachElevator() const { return _approachElevator.val; }
     const char* getFailureMsg() const { return _failureMsg; }
 
-    void setupState(const float aoa, const float speed, const float gla, yasim::State* s) const;
     void loadApproachControls() { loadControls(_approachControls); }
     void loadCruiseControls() { loadControls(_cruiseControls); }
     

src/FDM/YASim/BodyEnvironment.hpp

@@ -13,20 +13,15 @@ namespace yasim {
 // You can get local->global transformations by calling Math::tmul33()
 // and using the same matrix.
 struct State {
-    double pos[3];    // position
+    double pos[3] {0, 0, 0};    // position
-    float  orient[9]; // global->local xform matrix
+    float  orient[9]  {1,0,0,  0,1,0,  0,0,1}; // global->local xform matrix
-    float  v[3];      // velocity
+    float  v[3] {0, 0, 0};      // velocity
-    float  rot[3];    // rotational velocity
+    float  rot[3] {0, 0, 0};    // rotational velocity
-    float  acc[3];    // acceleration
+    float  acc[3] {0, 0, 0};    // acceleration
-    float  racc[3];   // rotational acceleration
+    float  racc[3] {0, 0, 0};   // rotational acceleration
 
     // Simple initialization
     State() {
-        for(int i=0; i<3; i++) {
-            pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
-            for(int j=0; j<3; j++)
-                orient[3*i+j] = (i==j) ? 1.0f : 0.0f;
-        }
     }
 
     void posLocalToGlobal(const float* lpos, double *gpos) const {
@@ -36,15 +31,18 @@ struct State {
         gpos[1] = tmp[1] + pos[1];
         gpos[2] = tmp[2] + pos[2];
     }
+    
     void posGlobalToLocal(const double* gpos, float *lpos) const {
         lpos[0] = (float)(gpos[0] - pos[0]);
         lpos[1] = (float)(gpos[1] - pos[1]);
         lpos[2] = (float)(gpos[2] - pos[2]);
         Math::vmul33(orient, lpos, lpos);
     }
+    
     void velLocalToGlobal(const float* lvel, float *gvel) const {
         Math::tmul33(orient, lvel, gvel);
     }
+    
     void velGlobalToLocal(const float* gvel, float *lvel) const {
         Math::vmul33(orient, gvel, lvel);
     }
@@ -61,6 +59,26 @@ struct State {
                           + pos[2]*gplane[2] - gplane[3]);
     }
     
+    // used by Airplane::runCruise, runApproach, solveHelicopter and in yasim-test
+    void setupState(float aoa, float speed, float gla) 
+    {
+      float cosAoA = Math::cos(aoa);
+      float sinAoA = Math::sin(aoa);
+      orient[0] =  cosAoA; orient[1] = 0; orient[2] = sinAoA;
+      orient[3] =       0; orient[4] = 1; orient[5] =      0;
+      orient[6] = -sinAoA; orient[7] = 0; orient[8] = cosAoA;
+      
+      // FIXME check axis, guess sin should go to 2 instead of 1?
+      v[0] = speed*Math::cos(gla); 
+      v[1] = -speed*Math::sin(gla); 
+      v[2] = 0;
+      
+      for(int i=0; i<3; i++) {
+        pos[i] = rot[i] = acc[i] = racc[i] = 0;
+      }
+      
+      pos[2] = 1;
+    }
 };
 
 //

src/FDM/YASim/yasim-test.cpp

@@ -74,7 +74,7 @@ void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG
 
   for(int deg=-15; deg<=90; deg++) {
     float aoa = deg * DEG2RAD;
-    a->setupState(aoa, kts * KTS2MPS, 0 ,&s);
+    s.setupState(aoa, kts * KTS2MPS, 0);
     m->getBody()->reset();
     m->initIteration();
     m->calcForces(&s);
@@ -148,7 +148,7 @@ void yasim_drag(Airplane* a, const float aoa, const float alt, int cfg = CONFIG_
   printf("#kts, drag\n");
   
   for(int kts=15; kts<=150; kts++) {
-    a->setupState(aoa, kts * KTS2MPS, 0 ,&s);
+    s.setupState(aoa, kts * KTS2MPS, 0);
     m->getBody()->reset();
     m->initIteration();
     m->calcForces(&s);