#include <cmath>
+#include <simgear/structure/exception.hxx>
#include "SGMath.hxx"
// These are hard numbers from the WGS84 standard. DON'T MODIFY
#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;
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);
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;
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