YASim BodyEnvironment rename methods and split setupState into two methods.

src/FDM/YASim/BodyEnvironment.hpp

@@ -39,12 +39,12 @@ struct State {
         Math::vmul33(orient, lpos, lpos);
     }
     
-    void velLocalToGlobal(const float* lvel, float *gvel) const {
+    void localToGlobal(const float* local, float *global) const {
-        Math::tmul33(orient, lvel, gvel);
+        Math::tmul33(orient, local, global);
     }
     
-    void velGlobalToLocal(const float* gvel, float *lvel) const {
+    void globalToLocal(const float* global, float *local) const {
-        Math::vmul33(orient, gvel, lvel);
+        Math::vmul33(orient, global, local);
     }
 
     void planeGlobalToLocal(const double* gplane, float *lplane) const {
@@ -60,25 +60,33 @@ struct State {
     }
     
     // used by Airplane::runCruise, runApproach, solveHelicopter and in yasim-test
-    void setupState(float aoa, float speed, float gla) 
+    void setupOrientationFromAoa(float aoa) 
     {
       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;
+    }
+    
+    void setupSpeedAndPosition(float speed, float gla) 
+    {
       
       // 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;
     }
+
+    void setupState(float aoa, float speed, float gla) {
+        setupOrientationFromAoa(aoa);
+        setupSpeedAndPosition(speed, gla);
+    }
 };
 
 //

src/FDM/YASim/Gear.cpp

@@ -165,7 +165,7 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
 
     // The velocity of the contact patch transformed to local coordinates.
     float glvel[3];
-    s->velGlobalToLocal(_global_vel, glvel);
+    s->globalToLocal(_global_vel, glvel);
 
     // First off, make sure that the gear "tip" is below the ground.
     // If it's not, there's no force.

src/FDM/YASim/Hitch.cpp

@@ -413,7 +413,7 @@ void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
                                   //With this trick, both player in aerotow get the same length
     Math::unit3(delta,deltaN);
     float lvel[3];
-    s->velGlobalToLocal(s->v,lvel);
+    s->globalToLocal(s->v,lvel);
     _speed_in_tow_direction=Math::dot3(lvel,deltaN);
     if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
     {
@@ -421,7 +421,7 @@ void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
         _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
         if(_forceIsCalculatedByMaster && !_open)
         {
-            s->velGlobalToLocal(_mp_force,_force);
+            s->globalToLocal(_mp_force,_force);
             return;
         }
     }
@@ -528,7 +528,7 @@ void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
     //the same for the tow end:
     Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
     Math::add3(grav_force_v,_towEndForce,_towEndForce);
-    s->velLocalToGlobal(_towEndForce,_towEndForce);
+    s->localToGlobal(_towEndForce,_towEndForce);
 
     if(_forceMagnitude>=_towBrakeForce)
     {

src/FDM/YASim/Hook.cpp

@@ -216,8 +216,8 @@ void Hook::calcForce(Ground* g_cb, RigidBody* body, State* s, float* lv, float*
     float wire_lpos[2][3];
     s->posGlobalToLocal(dpos[0], wire_lpos[0]);
     s->posGlobalToLocal(dpos[1], wire_lpos[1]);
-    s->velGlobalToLocal(wire_vel[0], wire_vel[0]);
+    s->globalToLocal(wire_vel[0], wire_vel[0]);
-    s->velGlobalToLocal(wire_vel[1], wire_vel[1]);
+    s->globalToLocal(wire_vel[1], wire_vel[1]);
 
     // Compute the velocity of the hooks mount point in the local frame.
     float mount_vel[3];

src/FDM/YASim/Launchbar.cpp

@@ -374,8 +374,8 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
     // Transform the velocities of the endpoints to the
     // local coordinate sytem.
     float lvel[2][3];
-    s->velGlobalToLocal(vel[0], lvel[0]);
+    s->globalToLocal(vel[0], lvel[0]);
-    s->velGlobalToLocal(vel[1], lvel[1]);
+    s->globalToLocal(vel[1], lvel[1]);
 
     // Compute the position of the launchbar tip relative to the cat.
     float tip_pos_on_cat = getPercentPosOnCat(llb_mount, 0.0, lend);

src/FDM/YASim/Rotor.cpp

@@ -829,7 +829,7 @@ void Rotor::calcLiftFactor(float* v, float rho, State *s)
 
     //store the gravity direction
     Glue::geodUp(s->pos, _grav_direction);
-    s->velGlobalToLocal(_grav_direction, _grav_direction);
+    s->globalToLocal(_grav_direction, _grav_direction);
 }
 
 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)