Guard against NaN SGGeods in FGPositioned spatial queries, and in the GPS lat/lon...

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 7e4b2b398f80bd7eef03c5e705d6b0505195b04d..853d49d466fc05cecd2ed6d2569e6c417d2e95f2 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -72,6 +72,13 @@ SGGeod SGGeodProperty::get() const
 {
     double lon = _lon->getDoubleValue(),
         lat = _lat->getDoubleValue();
+        
+    if (osg::isNaN(lon) || osg::isNaN(lat)) {
+      SG_LOG(SG_INSTR, SG_WARN, "read NaN for lon/lat:" << _lon->getPath() 
+        << ", " << _lat->getPath());
+      return SGGeod();
+    }
+        
     if (_alt) {
         return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue());
     } else {
@@ -490,7 +497,7 @@ GPS::update (double delta_time_sec)
   } // of init mode check
   
   _last_pos = _indicated_pos;
-  _lastPosValid = true;
+  _lastPosValid = !(_last_pos == SGGeod());
 }
 
 ///////////////////////////////////////////////////////////////////////////

diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx
index fcf5ec84893d708140bdf12ac51ca192e86ad99c..b74e4e877b1a5390a8ce0ae180035559b05f14e7 100644 (file)
--- a/src/Navaids/positioned.cxx
+++ b/src/Navaids/positioned.cxx
@@ -32,6 +32,8 @@
 #include <boost/algorithm/string/case_conv.hpp>
 #include <boost/algorithm/string/predicate.hpp>
 
+#include <osg/Math> // for osg::isNaN
+
 #include <simgear/timing/timestamp.hxx>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/structure/exception.hxx>
@@ -500,6 +502,15 @@ char** searchAirportNamesAndIdents(const std::string& aFilter)
   return result;
 }
 
+static void validateSGGeod(const SGGeod& geod)
+{
+  if (osg::isNaN(geod.getLatitudeDeg()) ||
+      osg::isNaN(geod.getLongitudeDeg()))
+  {
+    throw sg_range_exception("position is invalid, NaNs");
+  }
+}
+
 ///////////////////////////////////////////////////////////////////////////////
 
 bool
@@ -672,6 +683,8 @@ const char* FGPositioned::nameForType(Type aTy)
 FGPositionedRef
 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 {
+  validateSGGeod(aPos);
+
   FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true));
   if (r.empty()) {
     return FGPositionedRef();
@@ -684,6 +697,8 @@ FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos
 FGPositioned::List
 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 {
+  validateSGGeod(aPos);
+
   List result;
   Octree::findAllWithinRange(SGVec3d::fromGeod(aPos), 
     aRangeNm * SG_NM_TO_METER, aFilter, result);
@@ -705,18 +720,22 @@ FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aE
 FGPositionedRef
 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 {
-   List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
-   if (l.empty()) {
-      return NULL;
-   }
-   
-   assert(l.size() == 1);
-   return l.front();
+  validateSGGeod(aPos);
+  
+  List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
+  if (l.empty()) {
+    return NULL;
+  }
+
+  assert(l.size() == 1);
+  return l.front();
 }
 
 FGPositioned::List
 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 {
+  validateSGGeod(aPos);
+  
   List result;
   Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result);
   return result;
@@ -772,6 +791,8 @@ FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId
 void
 FGPositioned::sortByRange(List& aResult, const SGGeod& aPos)
 {
+  validateSGGeod(aPos);
+  
   SGVec3d cartPos(SGVec3d::fromGeod(aPos));
 // computer ordering values
   Octree::FindNearestResults r;