From bcf727cf5835c3ac7ba1d19cd2132657166aec70 Mon Sep 17 00:00:00 2001
From: torsten <torsten>
Date: Tue, 22 Sep 2009 14:17:45 +0000
Subject: [PATCH] Catch a possible floating point error in
 SGGeodesy::SGCartToGeod() for cartesian coordinates close to the geocenter
 region.

---
 simgear/math/SGGeodesy.cxx | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx
index 9971829a..208218b7 100644
--- a/simgear/math/SGGeodesy.cxx
+++ b/simgear/math/SGGeodesy.cxx
@@ -79,6 +79,17 @@ SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
   double Y = cart(1);
   double Z = cart(2);
   double XXpYY = X*X+Y*Y;
+  if( XXpYY + Z*Z < 25 ) {
+    // This function fails near the geocenter region, so catch that special case here.
+    // Define the innermost sphere of small radius as earth center and return the 
+    // coordinates 0/0/-EQURAD. It may be any other place on geoide's surface,
+    // the Northpole, Hawaii or Wentorf. This one was easy to code ;-)
+    geod.setLongitudeRad( 0.0 );
+    geod.setLongitudeRad( 0.0 );
+    geod.setElevationM( -EQURAD );
+    return;
+  }
+    
   double sqrtXXpYY = sqrt(XXpYY);
   double p = XXpYY*ra2;
   double q = Z*Z*(1-e2)*ra2;
-- 
2.39.5