Allow geocentric distance computations to return radians.

[simgear.git] / simgear / math / SGGeodesy.cxx

diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx
index 9971829af315ce299bd8ca95abce973de0196069..b9974f14c66601be2063896c99d59fc8bfa5adc3 100644 (file)
--- 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;
@@ -529,7 +540,7 @@ SGGeodesy::courseRad(const SGGeoc& from, const SGGeoc& to)
 }
 
 double
-SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
+SGGeodesy::distanceRad(const SGGeoc& from, const SGGeoc& to)
 {
   // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
   //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
@@ -539,5 +550,12 @@ SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
   double tmp2 = sin(0.5*(from.getLongitudeRad() - to.getLongitudeRad()));
   double square = tmp1*tmp1 + cosLatFrom*cosLatTo*tmp2*tmp2;
   double s = SGMiscd::min(sqrt(SGMiscd::max(square, 0)), 1);
-  return 2 * asin(s) * SG_RAD_TO_NM * SG_NM_TO_METER;
+  return 2 * asin(s);
+}
+
+
+double
+SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
+{
+  return distanceRad(from, to) * SG_RAD_TO_NM * SG_NM_TO_METER;
 }