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 3d6d38efcbae8530dc4f0c9b6706e04976a8ffee..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 {
 
@@ -21,12 +22,47 @@ struct State {
 
     // Simple initialization
     State() {
-        for(int i=0; i<3; i++) {
+        int i;
+        for(i=0; i<3; i++) {
             pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
-            for(int j=0; j<3; j++)
-                orient[3*i+j] = i==j ? 1 : 0;
+            int j;
+            for(j=0; j<3; j++)
+                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]);
+    }
+
 };
 
 //
@@ -53,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