Fix line endings

[flightgear.git] / src / FDM / flight.cxx

diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx
index 9bda7929a94ccee04dc0a25c3bd01f27240f8466..1f40e14934b61b67ef44d666bfa060bae837bf43 100644 (file)
--- a/src/FDM/flight.cxx
+++ b/src/FDM/flight.cxx
@@ -20,6 +20,9 @@
 //
 // $Id$
 
+#ifdef HAVE_CONFIG_H
+#  include "config.h"
+#endif
 
 #include <stdio.h>
 
@@ -196,6 +199,10 @@ FGInterface::common_init ()
     double alt_m = alt_ft * SG_FEET_TO_METER;
     set_Longitude( lon );
     set_Latitude( lat );
+    SG_LOG( SG_FLIGHT, SG_INFO, "Checking for lon = "
+            << lon*SGD_RADIANS_TO_DEGREES << "deg, lat = "
+            << lat*SGD_RADIANS_TO_DEGREES << "deg, alt = "
+            << alt_ft << "ft");
 
     double ground_elev_m = get_groundlevel_m(lat, lon, alt_m);
     double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET;
@@ -515,9 +522,7 @@ void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt )
     _set_Geodetic_Position( lat, lon, alt );
 
     _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET );
-    double alt_m = alt*SG_FEET_TO_METER;
-    double groundlevel_m = get_groundlevel_m(lat, lon, alt_m);
-    _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET );
+    _update_ground_elev_at_pos();
 
     _set_sin_lat_geocentric( lat_geoc );
     _set_cos_lat_geocentric( lat_geoc );
@@ -554,9 +559,7 @@ void FGInterface::_updateGeocentricPosition( double lat_geoc, double lon,
     _set_Geodetic_Position( lat_geod, lon, alt );
 
     _set_Sea_level_radius( sl_radius2 * SG_METER_TO_FEET );
-    double alt_m = alt*SG_FEET_TO_METER;
-    double groundlevel_m = get_groundlevel_m(lat_geod, lon, alt_m);
-    _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET );
+    _update_ground_elev_at_pos();
 
     _set_sin_lat_geocentric( lat_geoc );
     _set_cos_lat_geocentric( lat_geoc );
@@ -566,6 +569,13 @@ void FGInterface::_updateGeocentricPosition( double lat_geoc, double lon,
     _set_sin_cos_latitude( lat_geod );
 }
 
+void FGInterface::_update_ground_elev_at_pos( void ) {
+    double lat = get_Latitude();
+    double lon = get_Longitude();
+    double alt_m = get_Altitude()*SG_FEET_TO_METER;
+    double groundlevel_m = get_groundlevel_m(lat, lon, alt_m);
+    _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET );
+}
 
 // Extrapolate fdm based on time_offset (in usec)
 void FGInterface::extrapolate( int time_offset ) {
@@ -891,26 +901,50 @@ FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
 double
 FGInterface::get_groundlevel_m(double lat, double lon, double alt)
 {
-  // First compute the sea level radius,
   sgdVec3 pos, cpos;
-  sgGeodToCart(lat, lon, 0, pos);
-  double slr = sgdLengthVec3(pos);
-  // .. then the cartesian position of the given lat/lon/alt.
+  // Compute the cartesian position of the given lat/lon/alt.
   sgGeodToCart(lat, lon, alt, pos);
 
   // FIXME: how to handle t - ref_time differences ???
   double ref_time, radius;
   // Prepare the ground cache for that position.
-  if (!is_valid_m(&ref_time, cpos, &radius))
-    prepare_ground_cache_m(ref_time, pos, 10);
-  else if (radius*radius <= sgdDistanceSquaredVec3(pos, cpos))
-    prepare_ground_cache_m(ref_time, pos, radius);
+  if (!is_valid_m(&ref_time, cpos, &radius)) {
+    bool ok = prepare_ground_cache_m(ref_time, pos, 10);
+    /// This is most likely the case when the given altitude is
+    /// too low, try with a new altitude of 10000m, that should be
+    /// sufficient to find a ground level below everywhere on our planet
+    if (!ok) {
+      sgGeodToCart(lat, lon, 10000, pos);
+      /// If there is still no ground, return sea level radius
+      if (!prepare_ground_cache_m(ref_time, pos, 10))
+        return 0;
+    }
+  } else if (radius*radius <= sgdDistanceSquaredVec3(pos, cpos)) {
+    /// We reuse the old radius value, but only if it is at least 10 Meters ..
+    if (!(10 < radius)) // Well this strange compare is nan safe
+      radius = 10;
+
+    bool ok = prepare_ground_cache_m(ref_time, pos, radius);
+    /// This is most likely the case when the given altitude is
+    /// too low, try with a new altitude of 10000m, that should be
+    /// sufficient to find a ground level below everywhere on our planet
+    if (!ok) {
+      sgGeodToCart(lat, lon, 10000, pos);
+      /// If there is still no ground, return sea level radius
+      if (!prepare_ground_cache_m(ref_time, pos, radius))
+        return 0;
+    }
+  }
   
   double contact[3], normal[3], vel[3], lc, ff, agl;
   int type;
+  // Ignore the return value here, since it just tells us if
+  // the returns stem from the groundcache or from the coarse
+  // computations below the groundcache. The contact point is still something
+  // valid, the normals and the other returns just contain some defaults.
   get_agl_m(ref_time, pos, 2.0, contact, normal, vel, &type, &lc, &ff, &agl);
-
-  return sgdLengthVec3(contact) - slr;
+  Point3D geodPos = sgCartToGeod(Point3D(contact[0], contact[1], contact[2]));
+  return geodPos.elev();
 }
   
 bool