latest updates from JSBSim

[flightgear.git] / src / FDM / YASim / BodyEnvironment.hpp

diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp
index 8029555d8ff6989b9c62eaf2410a5dfe8557ff65..0ff096a42a9036fe74dc08f548eaeaf6c2292868 100644 (file)
--- a/src/FDM/YASim/BodyEnvironment.hpp
+++ b/src/FDM/YASim/BodyEnvironment.hpp
@@ -2,6 +2,7 @@
 #define _BODYENVIRONMENT_HPP
 
 #include "RigidBody.hpp"
+#include "Math.hpp"
 
 namespace yasim {
 
@@ -26,9 +27,42 @@ struct State {
             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 : 0;
+                orient[3*i+j] = i==j ? 1.0f : 0.0f;
         }
     }
+
+    void posLocalToGlobal(float* lpos, double *gpos) {
+        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) {
+        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) {
+        Math::tmul33(orient, lvel, gvel);
+    }
+    void velGlobalToLocal(float* gvel, float *lvel) {
+        Math::vmul33(orient, gvel, lvel);
+    }
+
+    void planeGlobalToLocal(double* gplane, float *lplane) {
+      // First the normal vector transformed to local coordinates.
+      lplane[0] = (float)-gplane[0];
+      lplane[1] = (float)-gplane[1];
+      lplane[2] = (float)-gplane[2];
+      Math::vmul33(orient, lplane, lplane);
+
+      // Then the distance from the plane to the Aircraft's origin.
+      lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1]
+                          + pos[2]*gplane[2] - gplane[3]);
+    }
+
 };
 
 //
@@ -55,6 +89,8 @@ public:
     // of body state vs. the environment can happen here (crashes,
     // etc...).
     virtual void newState(State* state) = 0;
+
+    virtual ~BodyEnvironment() {} // #!$!?! gcc warning...
 };
 
 }; // namespace yasim