From 545b347a1629a13f6e096fae9d9be2fa17bdd427 Mon Sep 17 00:00:00 2001 From: James Turner Date: Thu, 10 Dec 2015 15:06:33 -0600 Subject: [PATCH] Relocate implementation of geocRadialIntersection --- src/Instrumentation/rnav_waypt_controller.cxx | 67 +------------------ src/Navaids/routePath.cxx | 67 ++++++++++++++++++- 2 files changed, 68 insertions(+), 66 deletions(-) diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index d783a206b..560cf9e12 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -32,71 +32,8 @@ extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const S namespace flightgear { -// implementation of -// http://williams.best.vwh.net/avform.htm#Intersection -bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result) -{ - double crs13 = r1 * SG_DEGREES_TO_RADIANS; - double crs23 = r2 * SG_DEGREES_TO_RADIANS; - double dst12 = SGGeodesy::distanceRad(a, b); - - //IF sin(lon2-lon1)<0 - // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) - // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) - // ELSE - // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) - // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) - // ENDIF - - double sinLat1 = sin(a.getLatitudeRad()); - double cosLat1 = cos(a.getLatitudeRad()); - double sinDst12 = sin(dst12); - double cosDst12 = cos(dst12); - - double crs12 = SGGeodesy::courseRad(a, b), - crs21 = SGGeodesy::courseRad(b, a); - - - // normalise to -pi .. pi range - double ang1 = SGMiscd::normalizeAngle(crs13-crs12); - double ang2 = SGMiscd::normalizeAngle(crs21-crs23); - - if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { - SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections"); - return false; - } - - if ((sin(ang1)*sin(ang2))<0.0) { - // SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:" - // << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2)); - return false; - } - - ang1 = fabs(ang1); - ang2 = fabs(ang2); - - //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12)) - //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3)) - //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13)) - - //lon3=mod(lon1-dlon+pi,2*pi)-pi - - double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12); - double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3)); - - SGGeoc pt3; - SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3); - - double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13)); - - //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3)) - double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3))); - double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon); - - result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM()); - //result = pt3; - return true; -} +// declared in routePath.cxx +bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result); /** * Helper function Cross track error: diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 721c6d26a..cae2fd711 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -16,7 +16,72 @@ #include namespace flightgear { - bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result); + + // implementation of + // http://williams.best.vwh.net/avform.htm#Intersection + bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result) + { + double crs13 = r1 * SG_DEGREES_TO_RADIANS; + double crs23 = r2 * SG_DEGREES_TO_RADIANS; + double dst12 = SGGeodesy::distanceRad(a, b); + + //IF sin(lon2-lon1)<0 + // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) + // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) + // ELSE + // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) + // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) + // ENDIF + + double sinLat1 = sin(a.getLatitudeRad()); + double cosLat1 = cos(a.getLatitudeRad()); + double sinDst12 = sin(dst12); + double cosDst12 = cos(dst12); + + double crs12 = SGGeodesy::courseRad(a, b), + crs21 = SGGeodesy::courseRad(b, a); + + + // normalise to -pi .. pi range + double ang1 = SGMiscd::normalizeAngle(crs13-crs12); + double ang2 = SGMiscd::normalizeAngle(crs21-crs23); + + if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { + SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections"); + return false; + } + + if ((sin(ang1)*sin(ang2))<0.0) { + // SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:" + // << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2)); + return false; + } + + ang1 = fabs(ang1); + ang2 = fabs(ang2); + + //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12)) + //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3)) + //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13)) + + //lon3=mod(lon1-dlon+pi,2*pi)-pi + + double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12); + double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3)); + + SGGeoc pt3; + SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3); + + double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13)); + + //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3)) + double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3))); + double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon); + + result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM()); + //result = pt3; + return true; + } } using namespace flightgear; -- 2.39.5