diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index d56c4e5f9..f8a997d1c 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/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(); diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index ef9dfefda..9575d33c3 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/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); } diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp index a07a6562b..1218b4863 100644 --- a/src/FDM/YASim/BodyEnvironment.hpp +++ b/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 - float orient[9]; // global->local xform matrix - float v[3]; // velocity - float rot[3]; // rotational velocity - float acc[3]; // acceleration - float racc[3]; // rotational acceleration + double pos[3] {0, 0, 0}; // position + float orient[9] {1,0,0, 0,1,0, 0,0,1}; // global->local xform matrix + float v[3] {0, 0, 0}; // velocity + float rot[3] {0, 0, 0}; // rotational velocity + float acc[3] {0, 0, 0}; // 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); } @@ -60,7 +58,27 @@ struct State { lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1] + 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; + } }; // diff --git a/src/FDM/YASim/yasim-test.cpp b/src/FDM/YASim/yasim-test.cpp index 2a90e1116..25783bfa1 100644 --- a/src/FDM/YASim/yasim-test.cpp +++ b/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);