Use the density values from the environment subsystem, to properly handle

[flightgear.git] / src / FDM / YASim / Glue.cpp

diff --git a/src/FDM/YASim/Glue.cpp b/src/FDM/YASim/Glue.cpp
index 119a1b161e68c6fb508e76e8f1b71573b6a6e731..3715bf6a3dc606a6245708e7d0132e5e8ce79d1e 100644 (file)
--- a/src/FDM/YASim/Glue.cpp
+++ b/src/FDM/YASim/Glue.cpp
@@ -2,6 +2,15 @@
 #include "Glue.hpp"
 namespace yasim {
 
+// WGS84 numbers
+static const double EQURAD = 6378137;         // equatorial radius
+static const double STRETCH = 1.003352810665; // equ./polar radius
+
+// Derived from the above
+static const double SQUASH = 0.99665839311;      // 1/STRETCH
+static const double POLRAD = 6356823.77346;      // EQURAD*SQUASH
+static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD
+
 void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
 {
     // Convert the velocity to the aircraft frame.
@@ -115,10 +124,10 @@ void Glue::xyz2nedMat(double lat, double lon, float* out)
     // Shorthand for our output vectors:
     float *north = out, *east = out+3, *down = out+6;
 
-    float slat = Math::sin(lat);
-    float clat = Math::cos(lat);
-    float slon = Math::sin(lon);
-    float clon = Math::cos(lon);
+    float slat = (float) Math::sin(lat);
+    float clat = (float)Math::cos(lat);
+    float slon = (float)Math::sin(lon);
+    float clon = (float)Math::cos(lon);
 
     north[0] = -clon * slat;
     north[1] = -slon * slat;
@@ -145,16 +154,18 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
     // rotation and are done out longhand below for efficiency.
 
     // Init to the identity matrix
-    for(int i=0; i<3; i++)
-        for(int j=0; j<3; j++)
-            out[3*i+j] = (i==j) ? 1 : 0;
+    int i, j;
+    for(i=0; i<3; i++)
+        for(j=0; j<3; j++)
+            out[3*i+j] = (i==j) ? 1.0f : 0.0f;
 
     // Negate Y and Z
     out[4] = out[8] = -1;
     
     float s = Math::sin(roll);
     float c = Math::cos(roll);
-    for(int col=0; col<3; col++) {
+    int col;
+    for(col=0; col<3; col++) {
        float y=out[col+3], z=out[col+6];
        out[col+3] = c*y - s*z;
        out[col+6] = s*y + c*z;
@@ -162,7 +173,7 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
 
     s = Math::sin(pitch);
     c = Math::cos(pitch);
-    for(int col=0; col<3; col++) {
+    for(col=0; col<3; col++) {
        float x=out[col], z=out[col+6];
        out[col]   = c*x + s*z;
        out[col+6] = c*z - s*x;
@@ -170,7 +181,7 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
 
     s = Math::sin(hdg);
     c = Math::cos(hdg);
-    for(int col=0; col<3; col++) {
+    for(col=0; col<3; col++) {
        float x=out[col], y=out[col+3];
        out[col]   = c*x - s*y;
        out[col+3] = s*x + c*y;
@@ -222,10 +233,10 @@ void Glue::geodUp(double* pos, float* out)
     double lat, lon, alt;
     xyz2geod(pos, &lat, &lon, &alt);
        
-    float slat = Math::sin(lat);
-    float clat = Math::cos(lat);
-    float slon = Math::sin(lon);
-    float clon = Math::cos(lon);
+    float slat = (float)Math::sin(lat);
+    float clat = (float)Math::cos(lat);
+    float slon = (float)Math::sin(lon);
+    float clon = (float)Math::cos(lon);
     out[0] = clon * clat;
     out[1] = slon * clat;
     out[2] = slat;