src/FDM/YASim/BodyEnvironment.hpp

   1 #ifndef _BODYENVIRONMENT_HPP
   2 #define _BODYENVIRONMENT_HPP
   3
   4 #include "RigidBody.hpp"
   5 #include "Math.hpp"
   6
   7 namespace yasim {
   8
   9 // The values for position and orientation within the global reference
  10 // frame, along with their first and second time derivatives.  The
  11 // orientation is stored as a matrix for simplicity.  Note that
  12 // because it is orthonormal, its inverse is simply its transpose.
  13 // You can get local->global transformations by calling Math::tmul33()
  14 // and using the same matrix.
  15 struct State {
  16     double pos[3];    // position
  17     float  orient[9]; // global->local xform matrix
  18     float  v[3];      // velocity
  19     float  rot[3];    // rotational velocity
  20     float  acc[3];    // acceleration
  21     float  racc[3];   // rotational acceleration
  22
  23     // Simple initialization
  24     State() {
  25         int i;
  26         for(i=0; i<3; i++) {
  27             pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
  28             int j;
  29             for(j=0; j<3; j++)
  30                 orient[3*i+j] = i==j ? 1.0f : 0.0f;
  31         }
  32     }
  33
  34     void posLocalToGlobal(float* lpos, double *gpos) {
  35         float tmp[3];
  36         Math::tmul33(orient, lpos, tmp);
  37         gpos[0] = tmp[0] + pos[0];
  38         gpos[1] = tmp[1] + pos[1];
  39         gpos[2] = tmp[2] + pos[2];
  40     }
  41     void posGlobalToLocal(double* gpos, float *lpos) {
  42         lpos[0] = gpos[0] - pos[0];
  43         lpos[1] = gpos[1] - pos[1];
  44         lpos[2] = gpos[2] - pos[2];
  45         Math::vmul33(orient, lpos, lpos);
  46     }
  47     void velLocalToGlobal(float* lvel, float *gvel) {
  48         Math::tmul33(orient, lvel, gvel);
  49     }
  50     void velGlobalToLocal(float* gvel, float *lvel) {
  51         Math::vmul33(orient, gvel, lvel);
  52     }
  53
  54     void planeGlobalToLocal(double* gplane, float *lplane) {
  55       // First the normal vector transformed to local coordinates.
  56       lplane[0] = -gplane[0];
  57       lplane[1] = -gplane[1];
  58       lplane[2] = -gplane[2];
  59       Math::vmul33(orient, lplane, lplane);
  60
  61       // Then the distance from the plane to the Aircraft's origin.
  62       lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1]
  63                           + pos[2]*gplane[2] - gplane[3]);
  64     }
  65
  66 };
  67
  68 //
  69 // Objects implementing this interface are responsible for calculating
  70 // external forces on a RigidBody object.  These will then be used by
  71 // an Integrator to decide on a new solution to the state equations,
  72 // which will be reported to the BodyEnvironment for further action.
  73 //
  74 class BodyEnvironment
  75 {
  76 public:
  77     // This method inspects the "environment" in which a RigidBody
  78     // exists, calculates forces and torques on the body, and sets
  79     // them via addForce()/addTorque().  Because this method is called
  80     // multiple times ("trials") as part of a Runge-Kutta integration,
  81     // this is NOT the place to make decisions about anything but the
  82     // forces on the object.  Note that the acc and racc fields of the
  83     // passed-in State object are undefined! (They are calculed BY
  84     // this method).
  85     virtual void calcForces(State* state) = 0;
  86
  87     // Called when the RK4 integrator has determined a "real" new
  88     // point on the curve of life.  Any side-effect producing checks
  89     // of body state vs. the environment can happen here (crashes,
  90     // etc...).
  91     virtual void newState(State* state) = 0;
  92 };
  93
  94 }; // namespace yasim
  95 #endif // _BODYENVIRONMENT_HPP