59 lines
1.7 KiB
C++
59 lines
1.7 KiB
C++
|
#ifndef _INTEGRATOR_HPP
|
||
|
|
#define _INTEGRATOR_HPP
|
||
|
|
|
||
|
|
#include "BodyEnvironment.hpp"
|
||
|
|
#include "RigidBody.hpp"
|
||
|
|
|
||
|
|
namespace yasim {
|
||
|
|
|
||
|
|
//
|
||
|
|
// These objects are responsible for extracting force data from a
|
||
|
|
// BodyEnvironment object, using a RigidBody object to calculate
|
||
|
|
// accelerations, and then tying that all together into a new
|
||
|
|
// "solution" of position/orientation/etc... for the body. The method
|
||
|
|
// used is a fourth-order Runge-Kutta integration.
|
||
|
|
//
|
||
|
|
class Integrator
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// Sets the RigidBody that will be integrated.
|
||
|
|
void setBody(RigidBody* body);
|
||
|
|
|
||
|
|
// Sets the BodyEnvironment object used to calculate the second
|
||
|
|
// derivatives.
|
||
|
|
void setEnvironment(BodyEnvironment* 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();
|
||
|
|
|
||
|
|
// 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);
|
||
|
|
|
||
|
|
// Do a 4th order Runge-Kutta integration over one time interval.
|
||
|
|
// This is the top level of the simulation.
|
||
|
|
void calcNewInterval();
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
BodyEnvironment* _env;
|
||
|
|
RigidBody* _body;
|
||
|
|
float _dt;
|
||
|
|
|
||
|
|
State _s;
|
||
|
|
};
|
||
|
|
|
||
|
|
}; // namespace yasim
|
||
|
|
#endif // _INTEGRATOR_HPP
|