Add Nasal Vs. 1.5

[flightgear.git] / src / Environment / environment_ctrl.cxx

diff --git a/src/Environment/environment_ctrl.cxx b/src/Environment/environment_ctrl.cxx
index 0d57267f6afa6046489c69dfc1477e2ff7ad3cca..eb2761b12a58099081fbb36401c2e866c4004583 100644 (file)
--- a/src/Environment/environment_ctrl.cxx
+++ b/src/Environment/environment_ctrl.cxx
@@ -23,10 +23,14 @@
 #include <simgear/debug/logstream.hxx>
 
 #include <stdlib.h>
+#include <algorithm>
+
 #include <Main/fg_props.hxx>
 
 #include "environment_ctrl.hxx"
 
+SG_USING_STD(sort);
+
 
 \f
 ////////////////////////////////////////////////////////////////////////
@@ -116,11 +120,16 @@ FGUserDefEnvironmentCtrl::init ()
 }
 
 void
-FGUserDefEnvironmentCtrl::update (int dt)
+FGUserDefEnvironmentCtrl::update (double dt)
 {
   double base_wind_speed = _base_wind_speed_node->getDoubleValue();
   double gust_wind_speed = _gust_wind_speed_node->getDoubleValue();
 
+  if (gust_wind_speed < base_wind_speed) {
+      gust_wind_speed = base_wind_speed;
+      _gust_wind_speed_node->setDoubleValue(gust_wind_speed);
+  }
+
   if (base_wind_speed == gust_wind_speed) {
     _current_wind_speed_kt = base_wind_speed;
   } else {
@@ -151,4 +160,147 @@ FGUserDefEnvironmentCtrl::update (int dt)
     _environment->set_wind_speed_kt(_current_wind_speed_kt);
 }
 
+
+\f
+////////////////////////////////////////////////////////////////////////
+// Implementation of FGInterpolateEnvironmentCtrl.
+////////////////////////////////////////////////////////////////////////
+
+FGInterpolateEnvironmentCtrl::FGInterpolateEnvironmentCtrl ()
+{
+}
+
+FGInterpolateEnvironmentCtrl::~FGInterpolateEnvironmentCtrl ()
+{
+    unsigned int i;
+    for (i = 0; i < _boundary_table.size(); i++)
+        delete _boundary_table[i];
+    for (i = 0; i < _aloft_table.size(); i++)
+        delete _aloft_table[i];
+}
+
+
+
+void
+FGInterpolateEnvironmentCtrl::init ()
+{
+    read_table(fgGetNode("/environment/config/boundary", true),
+               _boundary_table);
+    read_table(fgGetNode("/environment/config/aloft", true),
+               _aloft_table);
+}
+
+void
+FGInterpolateEnvironmentCtrl::reinit ()
+{
+    unsigned int i;
+    for (i = 0; i < _boundary_table.size(); i++)
+        delete _boundary_table[i];
+    for (i = 0; i < _aloft_table.size(); i++)
+        delete _aloft_table[i];
+    _boundary_table.clear();
+    _aloft_table.clear();
+    init();
+}
+
+void
+FGInterpolateEnvironmentCtrl::read_table (const SGPropertyNode * node,
+                                          vector<bucket *> &table)
+{
+    for (int i = 0; i < node->nChildren(); i++) {
+        const SGPropertyNode * child = node->getChild(i);
+        if ( strcmp(child->getName(), "entry") == 0
+            && child->getStringValue("elevation-ft", "")[0] != '\0'
+            && ( child->getDoubleValue("elevation-ft") > 0.1 || i == 0 ) )
+       {
+            bucket * b = new bucket;
+            if (i > 0)
+                b->environment.copy(table[i-1]->environment);
+            b->environment.read(child);
+            b->altitude_ft = b->environment.get_elevation_ft();
+            table.push_back(b);
+        }
+    }
+    sort(table.begin(), table.end());
+}
+
+void
+FGInterpolateEnvironmentCtrl::update (double delta_time_sec)
+{
+                                // FIXME
+    double altitude_ft = fgGetDouble("/position/altitude-ft");
+    double altitude_agl_ft = fgGetDouble("/position/altitude-agl-ft");
+    double boundary_transition =
+        fgGetDouble("/environment/config/boundary-transition-ft", 500);
+
+    // double ground_elevation_ft = altitude_ft - altitude_agl_ft;
+
+    int length = _boundary_table.size();
+
+    if (length > 0) {
+                                // boundary table
+        double boundary_limit = _boundary_table[length-1]->altitude_ft;
+        if (boundary_limit >= altitude_agl_ft) {
+            do_interpolate(_boundary_table, altitude_agl_ft,
+                           _environment);
+            return;
+        } else if ((boundary_limit + boundary_transition) >= altitude_agl_ft) {
+                                // both tables
+            do_interpolate(_boundary_table, altitude_agl_ft, &env1);
+            do_interpolate(_aloft_table, altitude_ft, &env2);
+            double fraction =
+                (altitude_agl_ft - boundary_limit) / boundary_transition;
+            interpolate(&env1, &env2, fraction, _environment);
+            return;
+        }
+    }
+
+                                // aloft table
+    do_interpolate(_aloft_table, altitude_ft, _environment);
+}
+
+void
+FGInterpolateEnvironmentCtrl::do_interpolate (vector<bucket *> &table,
+                                              double altitude_ft,
+                                              FGEnvironment * environment)
+{
+    int length = table.size();
+    if (length == 0)
+        return;
+
+                                // Boundary conditions
+    if ((length == 1) || (table[0]->altitude_ft >= altitude_ft)) {
+        environment->copy(table[0]->environment);
+        return;
+    } else if (table[length-1]->altitude_ft <= altitude_ft) {
+        environment->copy(table[length-1]->environment);
+        return;
+    }
+        
+                                // Search the interpolation table
+    for (int i = 0; i < length - 1; i++) {
+        if ((i == length - 1) || (table[i]->altitude_ft <= altitude_ft)) {
+                FGEnvironment * env1 = &(table[i]->environment);
+                FGEnvironment * env2 = &(table[i+1]->environment);
+                double fraction;
+                if (table[i]->altitude_ft == table[i+1]->altitude_ft)
+                    fraction = 1.0;
+                else
+                    fraction =
+                        ((altitude_ft - table[i]->altitude_ft) /
+                         (table[i+1]->altitude_ft - table[i]->altitude_ft));
+                interpolate(env1, env2, fraction, environment);
+
+                return;
+        }
+    }
+}
+
+bool
+FGInterpolateEnvironmentCtrl::bucket::operator< (const bucket &b) const
+{
+    return (altitude_ft < b.altitude_ft);
+}
+
+
 // end of environment_ctrl.cxx