1 #ifndef _BODYENVIRONMENT_HPP
2 #define _BODYENVIRONMENT_HPP
4 #include "RigidBody.hpp"
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.
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
23 // Simple initialization
27 pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
30 orient[3*i+j] = i==j ? 1.0f : 0.0f;
34 void posLocalToGlobal(float* lpos, double *gpos) {
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];
41 void posGlobalToLocal(double* gpos, float *lpos) {
42 lpos[0] = (float)(gpos[0] - pos[0]);
43 lpos[1] = (float)(gpos[1] - pos[1]);
44 lpos[2] = (float)(gpos[2] - pos[2]);
45 Math::vmul33(orient, lpos, lpos);
47 void velLocalToGlobal(float* lvel, float *gvel) {
48 Math::tmul33(orient, lvel, gvel);
50 void velGlobalToLocal(float* gvel, float *lvel) {
51 Math::vmul33(orient, gvel, lvel);
54 void planeGlobalToLocal(double* gplane, float *lplane) {
55 // First the normal vector transformed to local coordinates.
56 lplane[0] = (float)-gplane[0];
57 lplane[1] = (float)-gplane[1];
58 lplane[2] = (float)-gplane[2];
59 Math::vmul33(orient, lplane, lplane);
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]);
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.
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
85 virtual void calcForces(State* state) = 0;
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,
91 virtual void newState(State* state) = 0;
93 virtual ~BodyEnvironment() {} // #!$!?! gcc warning...
97 #endif // _BODYENVIRONMENT_HPP