diff options
Diffstat (limited to 'boost/geometry/strategies/geographic/distance_cross_track.hpp')
| -rw-r--r-- | boost/geometry/strategies/geographic/distance_cross_track.hpp | 134 |
1 files changed, 83 insertions, 51 deletions
diff --git a/boost/geometry/strategies/geographic/distance_cross_track.hpp b/boost/geometry/strategies/geographic/distance_cross_track.hpp index be930a3fd4..f84bb4134f 100644 --- a/boost/geometry/strategies/geographic/distance_cross_track.hpp +++ b/boost/geometry/strategies/geographic/distance_cross_track.hpp @@ -13,6 +13,8 @@ #include <algorithm> +#include <boost/tuple/tuple.hpp> +#include <boost/algorithm/minmax.hpp> #include <boost/config.hpp> #include <boost/concept_check.hpp> #include <boost/mpl/if.hpp> @@ -27,6 +29,7 @@ #include <boost/geometry/strategies/concepts/distance_concept.hpp> #include <boost/geometry/strategies/spherical/distance_haversine.hpp> #include <boost/geometry/strategies/geographic/azimuth.hpp> +#include <boost/geometry/strategies/geographic/distance.hpp> #include <boost/geometry/strategies/geographic/parameters.hpp> #include <boost/geometry/formulas/vincenty_direct.hpp> @@ -94,17 +97,6 @@ public : > {}; - struct distance_strategy - { - typedef geographic<FormulaPolicy, Spheroid, CalculationType> type; - }; - - inline typename distance_strategy::type get_distance_strategy() const - { - typedef typename distance_strategy::type distance_type; - return distance_type(m_spheroid); - } - explicit geographic_cross_track(Spheroid const& spheroid = Spheroid()) : m_spheroid(spheroid) {} @@ -121,6 +113,20 @@ public : m_spheroid)).distance; } + // points on a meridian not crossing poles + template <typename CT> + inline CT vertical_or_meridian(CT lat1, CT lat2) const + { + typedef typename formula::meridian_inverse + < + CT, + strategy::default_order<FormulaPolicy>::value + > meridian_inverse; + + return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2, + m_spheroid); + } + private : template <typename CT> @@ -171,21 +177,22 @@ private : if (g4 < -1.25*pi)//close to -270 { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "g4=" << g4 << ", close to -270" << std::endl; + std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -270" << std::endl; #endif return g4 + 1.5 * pi; } else if (g4 > 1.25*pi)//close to 270 { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "g4=" << g4 << ", close to 270" << std::endl; + std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to 270" << std::endl; #endif + der = -der; return - g4 + 1.5 * pi; } else if (g4 < 0 && g4 > -0.75*pi)//close to -90 { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "g4=" << g4 << ", close to -90" << std::endl; + std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -90" << std::endl; #endif der = -der; return -g4 - pi/2; @@ -233,21 +240,30 @@ private : std::swap(lat1, lat2); } +#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK + std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>(); + std::cout << "," << lat1 * math::r2d<CT>(); + std::cout << "),(" << lon2 * math::r2d<CT>(); + std::cout << "," << lat2 * math::r2d<CT>(); + std::cout << ")\np=(" << lon3 * math::r2d<CT>(); + std::cout << "," << lat3 * math::r2d<CT>(); + std::cout << ")" << std::endl; +#endif + //segment on equator //Note: antipodal points on equator does not define segment on equator //but pass by the pole CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2); - typedef typename formula::elliptic_arc_length<CT> elliptic_arc_length; + typedef typename formula::meridian_inverse<CT> + meridian_inverse; bool meridian_not_crossing_pole = - elliptic_arc_length::meridian_not_crossing_pole(lat1, lat2, diff); + meridian_inverse::meridian_not_crossing_pole + (lat1, lat2, diff); bool meridian_crossing_pole = - elliptic_arc_length::meridian_crossing_pole(diff); - - //bool meridian_crossing_pole = math::equals(math::abs(diff), pi); - //bool meridian_not_crossing_pole = math::equals(math::abs(diff), c0); + meridian_inverse::meridian_crossing_pole(diff); if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole) { @@ -271,18 +287,28 @@ private : return non_iterative_case(lon3, lat1, lon3, lat3, spheroid); } - if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && lat1 > lat2) + if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && std::abs(lat1) > std::abs(lat2)) { +#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK + std::cout << "Meridian segment not crossing pole" << std::endl; +#endif std::swap(lat1,lat2); } if (meridian_crossing_pole) { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "Meridian segment" << std::endl; + std::cout << "Meridian segment crossing pole" << std::endl; #endif - result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid); - result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid); + CT sign_non_zero = lat3 >= c0 ? 1 : -1; + result_distance_point_segment<CT> d1 = + apply<geometry::radian>(lon1, lat1, + lon1, half_pi * sign_non_zero, + lon3, lat3, spheroid); + result_distance_point_segment<CT> d2 = + apply<geometry::radian>(lon2, lat2, + lon2, half_pi * sign_non_zero, + lon3, lat3, spheroid); if (d1.distance < d2.distance) { return d1; @@ -319,29 +345,31 @@ private : CT a312 = a13 - a12; - if (geometry::math::equals(a312, c0)) + // TODO: meridian case optimization + if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole) { + boost::tuple<CT,CT> minmax_elem = boost::minmax(lat1, lat2); + if (lat3 >= minmax_elem.template get<0>() && + lat3 <= minmax_elem.template get<1>()) + { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "point on segment" << std::endl; + std::cout << "Point on meridian segment" << std::endl; #endif - return non_iterative_case(lon3, lat3, c0); + return non_iterative_case(lon3, lat3, c0); + } } CT projection1 = cos( a312 ) * d1 / d3; #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK - std::cout << "segment=(" << lon1 * math::r2d<CT>(); - std::cout << "," << lat1 * math::r2d<CT>(); - std::cout << "),(" << lon2 * math::r2d<CT>(); - std::cout << "," << lat2 * math::r2d<CT>(); - std::cout << ")\np=(" << lon3 * math::r2d<CT>(); - std::cout << "," << lat3 * math::r2d<CT>(); - std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl; + std::cout << "a1=" << a12 * math::r2d<CT>() << std::endl; std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl; std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl; std::cout << "cos(a312)=" << cos(a312) << std::endl; + std::cout << "projection 1=" << projection1 << std::endl; #endif - if (projection1 < 0.0) + + if (projection1 < c0) { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK std::cout << "projection closer to p1" << std::endl; @@ -356,15 +384,17 @@ private : CT a321 = a23 - a21; + CT projection2 = cos( a321 ) * d2 / d3; + #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl; std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl; std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl; std::cout << "cos(a321)=" << cos(a321) << std::endl; + std::cout << "projection 2=" << projection2 << std::endl; #endif - CT projection2 = cos( a321 ) * d2 / d3; - if (projection2 < 0.0) + if (projection2 < c0) { #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK std::cout << "projection closer to p2" << std::endl; @@ -400,13 +430,15 @@ private : #endif // Update s14 (using Newton method) - CT prev_distance = 0; + CT prev_distance; geometry::formula::result_direct<CT> res14; geometry::formula::result_inverse<CT> res34; + res34.distance = -1; int counter = 0; // robustness CT g4; CT delta_g4; + bool dist_improve = true; do{ prev_distance = res34.distance; @@ -423,23 +455,27 @@ private : lon3, lat3, spheroid); g4 = res34.azimuth - a4; - - CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit CT m34 = res34.reduced_length; CT der = (M43 / m34) * sin(g4); - // normalize (g4 - pi/2) + //normalize delta_g4 = normalize(g4, der); - s14 = s14 - delta_g4 / der; + result.distance = res34.distance; + + dist_improve = prev_distance > res34.distance || prev_distance == -1; + if (!dist_improve) + { + result.distance = prev_distance; + } #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK std::cout << "p4=" << res14.lon2 * math::r2d<CT>() << "," << res14.lat2 * math::r2d<CT>() << std::endl; std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl; std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl; - std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl; + std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl; std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>() << std::endl; std::cout << "der=" << der << std::endl; std::cout << "M43=" << M43 << std::endl; @@ -448,15 +484,11 @@ private : std::cout << "new_s14=" << s14 << std::endl; std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl; std::cout << "---------end of step " << counter << std::endl<< std::endl; -#endif - result.distance = prev_distance; - -#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK if (g4 == half_pi) { std::cout << "Stop msg: g4 == half_pi" << std::endl; } - if (res34.distance >= prev_distance && prev_distance != 0) + if (!dist_improve) { std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl; } @@ -464,16 +496,16 @@ private : { std::cout << "Stop msg: delta_g4 == 0" << std::endl; } - if (counter == 19) + if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS) { std::cout << "Stop msg: counter" << std::endl; } #endif } while (g4 != half_pi - && (prev_distance > res34.distance || prev_distance == 0) + && dist_improve && delta_g4 != 0 - && ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ; + && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS); #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK std::cout << "distance=" << res34.distance << std::endl; |