diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp index 0ff096a42..a07a6562b 100644 --- a/src/FDM/YASim/BodyEnvironment.hpp +++ b/src/FDM/YASim/BodyEnvironment.hpp @@ -22,36 +22,34 @@ struct State { // Simple initialization State() { - int i; - for(i=0; i<3; i++) { + for(int i=0; i<3; i++) { pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0; - int j; - for(j=0; j<3; j++) - orient[3*i+j] = i==j ? 1.0f : 0.0f; + for(int j=0; j<3; j++) + orient[3*i+j] = (i==j) ? 1.0f : 0.0f; } } - void posLocalToGlobal(float* lpos, double *gpos) { + void posLocalToGlobal(const float* lpos, double *gpos) const { float tmp[3]; Math::tmul33(orient, lpos, tmp); gpos[0] = tmp[0] + pos[0]; gpos[1] = tmp[1] + pos[1]; gpos[2] = tmp[2] + pos[2]; } - void posGlobalToLocal(double* gpos, float *lpos) { + 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(float* lvel, float *gvel) { + void velLocalToGlobal(const float* lvel, float *gvel) const { Math::tmul33(orient, lvel, gvel); } - void velGlobalToLocal(float* gvel, float *lvel) { + void velGlobalToLocal(const float* gvel, float *lvel) const { Math::vmul33(orient, gvel, lvel); } - void planeGlobalToLocal(double* gplane, float *lplane) { + void planeGlobalToLocal(const double* gplane, float *lplane) const { // First the normal vector transformed to local coordinates. lplane[0] = (float)-gplane[0]; lplane[1] = (float)-gplane[1]; diff --git a/src/FDM/YASim/Integrator.cpp b/src/FDM/YASim/Integrator.cpp index efb080c04..57bf1c93c 100644 --- a/src/FDM/YASim/Integrator.cpp +++ b/src/FDM/YASim/Integrator.cpp @@ -2,43 +2,6 @@ #include "Integrator.hpp" namespace yasim { -void Integrator::setBody(RigidBody* body) -{ - _body = body; -} - -void Integrator::setEnvironment(BodyEnvironment* env) -{ - _env = env; -} - -void Integrator::setInterval(float dt) -{ - _dt = dt; -} - -float Integrator::getInterval() -{ - return _dt; -} - -void Integrator::setState(State* s) -{ - _s = *s; -} - -State* Integrator::getState() -{ - return &_s; -} - -// Transforms a "local" vector to a "global" vector (not coordinate!) -// using the specified orientation. -void Integrator::l2gVector(float* orient, float* v, float* out) -{ - Math::tmul33(orient, v, out); -} - // Updates a position vector for a body c.g. motion with velocity v // over time dt, from orientation o0 to o1. Because the position // references the local coordinate origin, but the velocity is that of diff --git a/src/FDM/YASim/Integrator.hpp b/src/FDM/YASim/Integrator.hpp index 323177ced..6a84aedf7 100644 --- a/src/FDM/YASim/Integrator.hpp +++ b/src/FDM/YASim/Integrator.hpp @@ -17,24 +17,24 @@ class Integrator { public: // Sets the RigidBody that will be integrated. - void setBody(RigidBody* body); + void setBody(RigidBody* body) { _body = body; } // Sets the BodyEnvironment object used to calculate the second // derivatives. - void setEnvironment(BodyEnvironment* env); + void setEnvironment(BodyEnvironment* env) { _env = env; } // Sets the time interval between steps. Units can be anything, // but they must match the other values (if you specify speed in // m/s, then angular acceleration had better be in 1/s^2 and the // time interval should be in seconds, etc...) - void setInterval(float dt); - float getInterval(); + void setInterval(float dt) { _dt = dt; } + float getInterval() const { return _dt; } // The current state, i.e. initial conditions for the next // integration iteration. Note that the acceleration parameters // in the State object are ignored. - State* getState(); - void setState(State* s); + State* getState() { return &_s; } + void setState(State* s) { _s = *s; } // Do a 4th order Runge-Kutta integration over one time interval. // This is the top level of the simulation. @@ -43,9 +43,13 @@ public: private: void orthonormalize(float* m); void rotMatrix(float* r, float dt, float* out); - void l2gVector(float* orient, float* v, float* out); - void extrapolatePosition(double* pos, float* v, float dt, - float* o1, float* o2); + + // Transforms a "local" vector to a "global" vector (not coordinate!) + // using the specified orientation. + void l2gVector(const float* orient, const float* v, float* out) { + Math::tmul33(orient, v, out); } + + void extrapolatePosition(double* pos, float* v, float dt, float* o1, float* o2); BodyEnvironment* _env; RigidBody* _body; diff --git a/src/FDM/YASim/Thruster.cpp b/src/FDM/YASim/Thruster.cpp index cfc31a7a9..4616efb00 100644 --- a/src/FDM/YASim/Thruster.cpp +++ b/src/FDM/YASim/Thruster.cpp @@ -17,51 +17,6 @@ Thruster::~Thruster() { } -void Thruster::getPosition(float* out) -{ - int i; - for(i=0; i<3; i++) out[i] = _pos[i]; -} - -void Thruster::setPosition(float* pos) -{ - int i; - for(i=0; i<3; i++) _pos[i] = pos[i]; -} - -void Thruster::getDirection(float* out) -{ - int i; - for(i=0; i<3; i++) out[i] = _dir[i]; -} - -void Thruster::setDirection(float* dir) -{ - Math::unit3(dir, _dir); -} - -void Thruster::setThrottle(float throttle) -{ - _throttle = Math::clamp(throttle, -1, 1); -} - -void Thruster::setMixture(float mixture) -{ - _mixture = Math::clamp(mixture, 0, 1); -} - - -void Thruster::setStarter(bool starter) -{ - _starter = starter; -} - -void Thruster::setWind(float* wind) -{ - int i; - for(i=0; i<3; i++) _wind[i] = wind[i]; -} - void Thruster::setAir(float pressure, float temp, float density) { _pressure = pressure; diff --git a/src/FDM/YASim/Thruster.hpp b/src/FDM/YASim/Thruster.hpp index c022ebaa4..f8208283c 100644 --- a/src/FDM/YASim/Thruster.hpp +++ b/src/FDM/YASim/Thruster.hpp @@ -1,5 +1,6 @@ #ifndef _THRUSTER_HPP #define _THRUSTER_HPP +#include "Math.hpp" namespace yasim { @@ -22,15 +23,15 @@ public: virtual Engine* getEngine() { return 0; } // Static data - void getPosition(float* out); - void setPosition(float* pos); - void getDirection(float* out); - void setDirection(float* dir); + void getPosition(float* out) const { Math::set3(_pos, out); } + void setPosition(const float* pos) { Math::set3(pos, _pos); } + void getDirection(float* out) const { Math::set3(_dir, out); } + void setDirection(const float* dir) { Math::unit3(dir, _dir); } // Controls - void setThrottle(float throttle); - void setMixture(float mixture); - void setStarter(bool starter); + void setThrottle(float throttle) { _throttle = Math::clamp(throttle, -1, 1); } + void setMixture(float mixture) { _mixture = Math::clamp(mixture, 0, 1); } + void setStarter(bool starter) { _starter = starter; } void setFuelState(bool hasFuel) { _fuel = hasFuel; } // Dynamic output @@ -42,7 +43,7 @@ public: virtual float getFuelFlow()=0; // in kg/s // Runtime instructions - void setWind(float* wind); + void setWind(const float* wind) { Math::set3(wind, _wind); }; void setAir(float pressure, float temp, float density); virtual void init() {} virtual void integrate(float dt)=0;