X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGGeodesy.cxx;h=9971829af315ce299bd8ca95abce973de0196069;hb=6a7c2000027cd22eea603e936ddbad1a5bfc8b04;hp=819acdd029cbc5db83827f50307afd67cf9aa965;hpb=30529ccdf5ccd880dc68d9f83953584ba50eddfc;p=simgear.git diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx index 819acdd0..9971829a 100644 --- a/simgear/math/SGGeodesy.cxx +++ b/simgear/math/SGGeodesy.cxx @@ -21,6 +21,7 @@ #include +#include #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; @@ -83,6 +84,14 @@ SGGeodesy::SGCartToGeod(const SGVec3& cart, SGGeod& geod) 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 +323,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 +432,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