From dd044844e52e939af0c4e979f25583862123bb7a Mon Sep 17 00:00:00 2001 From: James Turner Date: Mon, 14 Jun 2010 10:15:44 +0100 Subject: [PATCH] Add radial intersection code to SGGeodesy, and test coverage for geocentric helpers. --- simgear/math/Makefile.am | 14 ++++++- simgear/math/SGGeodesy.cxx | 75 +++++++++++++++++++++++++++++++++++++ simgear/math/SGGeodesy.hxx | 10 +++++ simgear/math/SGMathTest.cxx | 23 ++++++++++++ 4 files changed, 120 insertions(+), 2 deletions(-) diff --git a/simgear/math/Makefile.am b/simgear/math/Makefile.am index 466bb790..5474f297 100644 --- a/simgear/math/Makefile.am +++ b/simgear/math/Makefile.am @@ -4,11 +4,21 @@ check_PROGRAMS = SGMathTest SGGeometryTest TESTS = $(check_PROGRAMS) SGMathTest_SOURCES = SGMathTest.cxx -SGMathTest_LDADD = libsgmath.a -lsgstructure $(base_LIBS) +SGMathTest_LDADD = \ + libsgmath.a \ + $(top_builddir)/simgear/debug/libsgdebug.a \ + $(top_builddir)/simgear/structure/libsgstructure.a \ + $(base_LIBS) + SGGeometryTest_SOURCES = SGGeometryTest.cxx -SGGeometryTest_LDADD = libsgmath.a -lsgstructure $(base_LIBS) +SGGeometryTest_LDADD = \ + libsgmath.a \ + $(top_builddir)/simgear/debug/libsgdebug.a \ + $(top_builddir)/simgear/structure/libsgstructure.a \ + $(base_LIBS) + lib_LIBRARIES = libsgmath.a include_HEADERS = \ diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx index 1d71b919..d86cf0e3 100644 --- a/simgear/math/SGGeodesy.cxx +++ b/simgear/math/SGGeodesy.cxx @@ -22,6 +22,8 @@ #include #include +#include + #include "SGMath.hxx" // These are hard numbers from the WGS84 standard. DON'T MODIFY @@ -560,3 +562,76 @@ SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to) { return distanceRad(from, to) * SG_RAD_TO_NM * SG_NM_TO_METER; } + +bool +SGGeodesy::radialIntersection(const SGGeoc& a, double r1, + const SGGeoc& b, double r2, SGGeoc& result) +{ + // implementation of + // http://williams.best.vwh.net/avform.htm#Intersection + + 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 crs12 = SGGeodesy::courseRad(a, b), + crs21 = SGGeodesy::courseRad(b, a); + + double sinLat1 = sin(a.getLatitudeRad()); + double cosLat1 = cos(a.getLatitudeRad()); + double sinDst12 = sin(dst12); + double cosDst12 = cos(dst12); + + double ang1 = SGMiscd::normalizeAngle2(crs13-crs12); + double ang2 = SGMiscd::normalizeAngle2(crs21-crs23); + + if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { + SG_LOG(SG_GENERAL, SG_WARN, "SGGeodesy::radialIntersection: infinity of intersections"); + return false; + } + + if ((sin(ang1)*sin(ang2))<0.0) { + SG_LOG(SG_GENERAL, SG_WARN, "SGGeodesy::radialIntersection: intersection ambiguous"); + 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)); + 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()); + return true; +} + +bool +SGGeodesy::radialIntersection(const SGGeod& a, double aRadial, + const SGGeod& b, double bRadial, SGGeod& result) +{ + SGGeoc r; + bool ok = radialIntersection(SGGeoc::fromGeod(a), aRadial, + SGGeoc::fromGeod(b), bRadial, r); + if (!ok) { + return false; + } + + result = SGGeod::fromGeoc(r); + return true; +} diff --git a/simgear/math/SGGeodesy.hxx b/simgear/math/SGGeodesy.hxx index ef1be21f..060bf4d3 100644 --- a/simgear/math/SGGeodesy.hxx +++ b/simgear/math/SGGeodesy.hxx @@ -63,6 +63,16 @@ public: static double courseRad(const SGGeoc& from, const SGGeoc& to); static double distanceRad(const SGGeoc& from, const SGGeoc& to); static double distanceM(const SGGeoc& from, const SGGeoc& to); + + /** + * compute the intersection of two (true) radials (in degrees), or return false + * if no intersection culd be computed. + */ + static bool radialIntersection(const SGGeoc& a, double aRadial, + const SGGeoc& b, double bRadial, SGGeoc& result); + + static bool radialIntersection(const SGGeod& a, double aRadial, + const SGGeod& b, double bRadial, SGGeod& result); }; #endif diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx index 010f9995..f4e5b69f 100644 --- a/simgear/math/SGMathTest.cxx +++ b/simgear/math/SGMathTest.cxx @@ -271,6 +271,29 @@ GeodesyTest(void) if (!equivalent(cart0, cart1)) return false; + // test course / advance routines + // uses examples from Williams aviation formulary + SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0); + SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0); + + double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM; + std::cout << "distance is " << distNm << std::endl; + if (0.5 < fabs(distNm - 2144)) // 2144 nm + return false; + + double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES; + std::cout << "course is " << crsDeg << std::endl; + if (0.5 < fabs(crsDeg - 66)) // 66 degrees + return false; + + SGGeoc adv; + SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv); + std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl; + + if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) || + 0.01 < fabs(adv.getLatitudeRad() - 0.604180)) + return false; + return true; } -- 2.39.5