Merge branch 'tat/framework'

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

diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx
index 819acdd029cbc5db83827f50307afd67cf9aa965..b9974f14c66601be2063896c99d59fc8bfa5adc3 100644 (file)
--- a/simgear/math/SGGeodesy.cxx
+++ b/simgear/math/SGGeodesy.cxx
@@ -21,6 +21,7 @@
 
 #include <cmath>
 
+#include <simgear/structure/exception.hxx>
 #include "SGMath.hxx"
 
 // These are hard numbers from the WGS84 standard.  DON'T MODIFY
@@ -56,7 +57,7 @@ const double SGGeodesy::POLRAD = _POLRAD;
 #define E2 fabs(1 - _SQUASH*_SQUASH)
 static double a = _EQURAD;
 static double ra2 = 1/(_EQURAD*_EQURAD);
-static double e = sqrt(E2);
+//static double e = sqrt(E2);
 static double e2 = E2;
 static double e4 = E2*E2;
 
@@ -78,11 +79,30 @@ 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;
   double r = 1/6.0*(p+q-e4);
   double s = e4*p*q/(4*r*r*r);
+/* 
+  s*(2+s) is negative for s = [-2..0]
+  slightly negative values for s due to floating point rounding errors
+  cause nan for sqrt(s*(2+s))
+  We can probably clamp the resulting parable to positive numbers
+*/
+  if( s >= -2.0 && s <= 0.0 )
+    s = 0.0;
   double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
   double u = r*(1+t+1/t);
   double v = sqrt(u*u+e4*q);
@@ -314,7 +334,7 @@ static int _geo_inverse_wgs_84( double lat1, double lon1, double lat2,
     double sinphi2 = sin(phi2), cosphi2 = cos(phi2);
        
     if( (fabs(lat1-lat2) < testv && 
-        ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) )
+        ( fabs(lon1-lon2) < testv)) || (fabs(lat1-90.0) < testv ) )
     {  
        // TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
        *az1 = 0.0; *az2 = 0.0; *s = 0.0;
@@ -423,6 +443,40 @@ SGGeodesy::inverse(const SGGeod& p1, const SGGeod& p2, double& course1,
   return ret == 0;
 }
 
+double
+SGGeodesy::courseDeg(const SGGeod& p1, const SGGeod& p2)
+{
+  double course1, course2, distance;
+  int r = _geo_inverse_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+                                p2.getLatitudeDeg(), p2.getLongitudeDeg(),
+                                &course1, &course2, &distance);
+  if (r != 0) {
+    throw sg_exception("SGGeodesy::courseDeg, unable to compute course");
+  }
+  
+  return course1;
+}
+
+double
+SGGeodesy::distanceM(const SGGeod& p1, const SGGeod& p2)
+{
+  double course1, course2, distance;
+  int r = _geo_inverse_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+                                p2.getLatitudeDeg(), p2.getLongitudeDeg(),
+                                &course1, &course2, &distance);
+  if (r != 0) {
+    throw sg_exception("SGGeodesy::distanceM, unable to compute distance");
+  }
+  
+  return distance;
+}
+
+double
+SGGeodesy::distanceNm(const SGGeod& from, const SGGeod& to)
+{
+  return distanceM(from, to) * SG_METER_TO_NM;
+}
+
 /// Geocentric routines
 
 void
@@ -486,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))
@@ -496,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;
 }