src/FDM/YASim/Integrator.hpp

   1 #ifndef _INTEGRATOR_HPP
   2 #define _INTEGRATOR_HPP
   3
   4 #include "BodyEnvironment.hpp"
   5 #include "RigidBody.hpp"
   6
   7 namespace yasim {
   8
   9 //
  10 // These objects are responsible for extracting force data from a
  11 // BodyEnvironment object, using a RigidBody object to calculate
  12 // accelerations, and then tying that all together into a new
  13 // "solution" of position/orientation/etc... for the body.  The method
  14 // used is a fourth-order Runge-Kutta integration.
  15 //
  16 class Integrator
  17 {
  18 public:
  19     // Sets the RigidBody that will be integrated.
  20     void setBody(RigidBody* body);
  21
  22     // Sets the BodyEnvironment object used to calculate the second
  23     // derivatives.
  24     void setEnvironment(BodyEnvironment* env);
  25
  26     // Sets the time interval between steps.  Units can be anything,
  27     // but they must match the other values (if you specify speed in
  28     // m/s, then angular acceleration had better be in 1/s^2 and the
  29     // time interval should be in seconds, etc...)
  30     void setInterval(float dt);
  31     float getInterval();
  32
  33     // The current state, i.e. initial conditions for the next
  34     // integration iteration.  Note that the acceleration parameters
  35     // in the State object are ignored.
  36     State* getState();
  37     void setState(State* s);
  38
  39     // Do a 4th order Runge-Kutta integration over one time interval.
  40     // This is the top level of the simulation.
  41     void calcNewInterval();
  42
  43 private:
  44     void orthonormalize(float* m);
  45     void rotMatrix(float* r, float dt, float* out);
  46     void l2gVector(float* orient, float* v, float* out);
  47     void extrapolatePosition(double* pos, float* v, float dt,
  48                              float* o1, float* o2);
  49
  50     BodyEnvironment* _env;
  51     RigidBody* _body;
  52     float _dt;
  53
  54     State _s;
  55 };
  56
  57 }; // namespace yasim
  58 #endif // _INTEGRATOR_HPP