diff --git a/include/boost/geometry/strategies/closest_points/spherical.hpp b/include/boost/geometry/strategies/closest_points/spherical.hpp index 69f2090c6..781059468 100644 --- a/include/boost/geometry/strategies/closest_points/spherical.hpp +++ b/include/boost/geometry/strategies/closest_points/spherical.hpp @@ -19,7 +19,7 @@ //#include //#include -//#include +#include #include #include @@ -36,16 +36,54 @@ namespace boost { namespace geometry namespace strategies { namespace closest_points { -template -struct spherical - : public strategies::distance::spherical +#ifndef DOXYGEN_NO_DETAIL +namespace detail { - //template - //static auto closest_points(Geometry1 const&, Geometry2 const&, - // distance::detail::enable_if_ps_t * = nullptr) - //{ - // return strategy::closest_points::projected_point(); - //} + +template +class spherical + : public strategies::distance::detail::spherical +{ + using base_t = strategies::distance::detail::spherical; + +public: + spherical() = default; + + template + explicit spherical(RadiusOrSphere const& radius_or_sphere) + : base_t(radius_or_sphere) + {} + + template + static auto closest_points(Geometry1 const&, Geometry2 const&, + distance::detail::enable_if_ps_t * = nullptr) + { + return strategy::closest_points::cross_track(); + } + +}; + + +} // namespace detail +#endif // DOXYGEN_NO_DETAIL + +template +< + typename RadiusTypeOrSphere = double, + typename CalculationType = void +> +class spherical + : public strategies::closest_points::detail::spherical +{ + using base_t = strategies::closest_points::detail::spherical; + +public: + spherical() = default; + + template + explicit spherical(RadiusOrSphere const& radius_or_sphere) + : base_t(radius_or_sphere) + {} }; diff --git a/include/boost/geometry/strategies/spherical/closest_points_pt_seg.hpp b/include/boost/geometry/strategies/spherical/closest_points_pt_seg.hpp new file mode 100644 index 000000000..b8f058bc8 --- /dev/null +++ b/include/boost/geometry/strategies/spherical/closest_points_pt_seg.hpp @@ -0,0 +1,201 @@ +// Boost.Geometry + +// Copyright (c) 2021, Oracle and/or its affiliates. + +// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle + +// Licensed under the Boost Software License version 1.0. +// http://www.boost.org/users/license.html + +#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_CLOSEST_POINTS_CROSS_TRACK_HPP +#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_CLOSEST_POINTS_CROSS_TRACK_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK +# include +#endif + + +namespace boost { namespace geometry +{ + +namespace strategy { namespace closest_points +{ + +template +< + typename CalculationType = void, + typename Strategy = distance::comparable::haversine +> +class cross_track +{ +public: + template + struct calculation_type + : promote_floating_point + < + typename select_calculation_type + < + Point, + PointOfSegment, + CalculationType + >::type + > + {}; + + using radius_type = typename Strategy::radius_type; + + cross_track() = default; + + explicit inline cross_track(typename Strategy::radius_type const& r) + : m_strategy(r) + {} + + inline cross_track(Strategy const& s) + : m_strategy(s) + {} + + template + inline auto apply(Point const& p, + PointOfSegment const& sp1, + PointOfSegment const& sp2) const + { + + using CT = typename calculation_type::type; + + // http://williams.best.vwh.net/avform.htm#XTE + CT d3 = m_strategy.apply(sp1, sp2); + + if (geometry::math::equals(d3, 0.0)) + { + // "Degenerate" segment, return either d1 or d2 + return sp1; + } + + CT d1 = m_strategy.apply(sp1, p); + CT d2 = m_strategy.apply(sp2, p); + + auto d_crs_pair = distance::detail::compute_cross_track_pair::apply( + p, sp1, sp2); + + // d1, d2, d3 are in principle not needed, only the sign matters + CT projection1 = cos(d_crs_pair.first) * d1 / d3; + CT projection2 = cos(d_crs_pair.second) * d2 / d3; + +#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK + std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " " + << crs_AD * geometry::math::r2d() << std::endl; + std::cout << "Course " << dsv(sp1) << " to " << dsv(sp2) << " " + << crs_AB * geometry::math::r2d() << std::endl; + std::cout << "Course " << dsv(sp2) << " to " << dsv(sp1) << " " + << crs_BA * geometry::math::r2d() << std::endl; + std::cout << "Course " << dsv(sp2) << " to " << dsv(p) << " " + << crs_BD * geometry::math::r2d() << std::endl; + std::cout << "Projection AD-AB " << projection1 << " : " + << d_crs1 * geometry::math::r2d() << std::endl; + std::cout << "Projection BD-BA " << projection2 << " : " + << d_crs2 * geometry::math::r2d() << std::endl; + std::cout << " d1: " << (d1 ) + << " d2: " << (d2 ) + << std::endl; +#endif + + if (projection1 > 0.0 && projection2 > 0.0) + { +#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK + CT XTD = radius() * geometry::math::abs( asin( sin( d1 ) * sin( d_crs1 ) )); + + std::cout << "Projection ON the segment" << std::endl; + std::cout << "XTD: " << XTD + << " d1: " << (d1 * radius()) + << " d2: " << (d2 * radius()) + << std::endl; +#endif + auto distance = distance::detail::compute_cross_track_distance::apply( + d_crs_pair.first, d1); + + CT lon1 = geometry::get_as_radian<0>(sp1); + CT lat1 = geometry::get_as_radian<1>(sp1); + CT lon2 = geometry::get_as_radian<0>(sp2); + CT lat2 = geometry::get_as_radian<1>(sp2); + + CT dist = CT(2) * asin(math::sqrt(distance)) * m_strategy.radius(); + CT dist_d1 = CT(2) * asin(math::sqrt(d1)) * m_strategy.radius(); + + // Note: this is similar to spherical computation in geographic + // point_segment_distance formula + CT earth_radius = m_strategy.radius(); + CT cos_frac = cos(dist_d1 / earth_radius) / cos(dist / earth_radius); + CT s14_sph = cos_frac >= 1 + ? CT(0) : cos_frac <= -1 ? math::pi() * earth_radius + : acos(cos_frac) * earth_radius; + + CT a12 = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2); + auto res_direct = geometry::formula::spherical_direct + < + true, + false + >(lon1, lat1, s14_sph, a12, srs::sphere(earth_radius)); + + model::point + < + CT, + dimension::value, + typename coordinate_system::type + > cp; + + geometry::set_from_radian<0>(cp, res_direct.lon2); + geometry::set_from_radian<1>(cp, res_direct.lat2); + + return cp; + } + else + { +#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK + std::cout << "Projection OUTSIDE the segment" << std::endl; +#endif + return d1 < d2 ? sp1 : sp2; + } + } + + template + inline radius_type vertical_or_meridian(T1 lat1, T2 lat2) const + { + return m_strategy.radius() * (lat1 - lat2); + } + + inline typename Strategy::radius_type radius() const + { return m_strategy.radius(); } + +private : + Strategy m_strategy; +}; + +}} // namespace strategy::closest_points + +}} // namespace boost::geometry + +#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_CLOSEST_POINTS_CROSS_TRACK_HPP diff --git a/include/boost/geometry/strategies/spherical/distance_cross_track.hpp b/include/boost/geometry/strategies/spherical/distance_cross_track.hpp index 16bc70823..21b303679 100644 --- a/include/boost/geometry/strategies/spherical/distance_cross_track.hpp +++ b/include/boost/geometry/strategies/spherical/distance_cross_track.hpp @@ -50,6 +50,82 @@ namespace boost { namespace geometry namespace strategy { namespace distance { +#ifndef DOXYGEN_NO_DETAIL +namespace detail +{ + template + struct compute_cross_track_pair + { + template + static inline auto apply(Point const& p, + PointOfSegment const& sp1, + PointOfSegment const& sp2) + { + CalculationType lon1 = geometry::get_as_radian<0>(sp1); + CalculationType lat1 = geometry::get_as_radian<1>(sp1); + CalculationType lon2 = geometry::get_as_radian<0>(sp2); + CalculationType lat2 = geometry::get_as_radian<1>(sp2); + CalculationType lon = geometry::get_as_radian<0>(p); + CalculationType lat = geometry::get_as_radian<1>(p); + + CalculationType crs_AD = geometry::formula::spherical_azimuth + < + CalculationType, + false + >(lon1, lat1, lon, lat).azimuth; + + auto result = geometry::formula::spherical_azimuth + < + CalculationType, + true + >(lon1, lat1, lon2, lat2); + + CalculationType crs_AB = result.azimuth; + CalculationType crs_BA = result.reverse_azimuth - + geometry::math::pi(); + + CalculationType crs_BD = geometry::formula::spherical_azimuth + < + CalculationType, + false + >(lon2, lat2, lon, lat).azimuth; + + CalculationType d_crs1 = crs_AD - crs_AB; + CalculationType d_crs2 = crs_BD - crs_BA; + + return std::pair(d_crs1, d_crs2); + } + }; + + struct compute_cross_track_distance + { + template + static inline auto apply(CalculationType const& d_crs1, + CalculationType const& d1) + { + CalculationType const half(0.5); + CalculationType const quarter(0.25); + + CalculationType sin_d_crs1 = sin(d_crs1); + /* + This is the straightforward obvious way to continue: + + return_type discriminant + = 1.0 - 4.0 * (d1 - d1 * d1) * sin_d_crs1 * sin_d_crs1; + return 0.5 - 0.5 * math::sqrt(discriminant); + + Below we optimize the number of arithmetic operations + and account for numerical robustness: + */ + CalculationType d1_x_sin = d1 * sin_d_crs1; + CalculationType d = d1_x_sin * (sin_d_crs1 - d1_x_sin); + return d / (half + math::sqrt(quarter - d)); + } + }; + +} +#endif // DOXYGEN_NO_DETAIL + namespace comparable { @@ -343,7 +419,7 @@ public: > {}; - typedef typename Strategy::radius_type radius_type; + using radius_type = typename Strategy::radius_type; cross_track() = default; @@ -372,7 +448,7 @@ public: ); #endif - typedef typename return_type::type return_type; + using return_type = typename return_type::type; // http://williams.best.vwh.net/avform.htm#XTE return_type d1 = m_strategy.apply(sp1, p); @@ -386,31 +462,12 @@ public: return_type d2 = m_strategy.apply(sp2, p); - return_type lon1 = geometry::get_as_radian<0>(sp1); - return_type lat1 = geometry::get_as_radian<1>(sp1); - return_type lon2 = geometry::get_as_radian<0>(sp2); - return_type lat2 = geometry::get_as_radian<1>(sp2); - return_type lon = geometry::get_as_radian<0>(p); - return_type lat = geometry::get_as_radian<1>(p); - - return_type crs_AD = geometry::formula::spherical_azimuth - (lon1, lat1, lon, lat).azimuth; - - geometry::formula::result_spherical result = - geometry::formula::spherical_azimuth - (lon1, lat1, lon2, lat2); - return_type crs_AB = result.azimuth; - return_type crs_BA = result.reverse_azimuth - geometry::math::pi(); - - return_type crs_BD = geometry::formula::spherical_azimuth - (lon2, lat2, lon, lat).azimuth; - - return_type d_crs1 = crs_AD - crs_AB; - return_type d_crs2 = crs_BD - crs_BA; + auto d_crs_pair = detail::compute_cross_track_pair::apply( + p, sp1, sp2); // d1, d2, d3 are in principle not needed, only the sign matters - return_type projection1 = cos( d_crs1 ) * d1 / d3; - return_type projection2 = cos( d_crs2 ) * d2 / d3; + return_type projection1 = cos(d_crs_pair.first) * d1 / d3; + return_type projection2 = cos(d_crs_pair.second) * d2 / d3; #ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " " @@ -441,30 +498,14 @@ public: << " d2: " << (d2 * radius()) << std::endl; #endif - return_type const half(0.5); - return_type const quarter(0.25); - - return_type sin_d_crs1 = sin(d_crs1); - /* - This is the straightforward obvious way to continue: - - return_type discriminant - = 1.0 - 4.0 * (d1 - d1 * d1) * sin_d_crs1 * sin_d_crs1; - return 0.5 - 0.5 * math::sqrt(discriminant); - - Below we optimize the number of arithmetic operations - and account for numerical robustness: - */ - return_type d1_x_sin = d1 * sin_d_crs1; - return_type d = d1_x_sin * (sin_d_crs1 - d1_x_sin); - return d / (half + math::sqrt(quarter - d)); + return detail::compute_cross_track_distance::apply( + d_crs_pair.first, d1); } else { #ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK std::cout << "Projection OUTSIDE the segment" << std::endl; #endif - // Return shortest distance, project either on point sp1 or sp2 return return_type( (std::min)( d1 , d2 ) ); } @@ -508,22 +549,22 @@ template class cross_track { public : - typedef within::spherical_point_point equals_point_point_strategy_type; + using equals_point_point_strategy_type = within::spherical_point_point; - typedef intersection::spherical_segments + using relate_segment_segment_strategy_type = intersection::spherical_segments < CalculationType - > relate_segment_segment_strategy_type; + >; static inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy() { return relate_segment_segment_strategy_type(); } - typedef within::spherical_winding + using point_in_geometry_strategy_type = within::spherical_winding < void, void, CalculationType - > point_in_geometry_strategy_type; + >; static inline point_in_geometry_strategy_type get_point_in_geometry_strategy() { @@ -543,7 +584,7 @@ public : > {}; - typedef typename Strategy::radius_type radius_type; + using radius_type = typename Strategy::radius_type; inline cross_track() {} @@ -562,8 +603,9 @@ public : template - inline typename return_type::type - apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const + inline auto apply(Point const& p, + PointOfSegment const& sp1, + PointOfSegment const& sp2) const { #if !defined(BOOST_MSVC) @@ -572,13 +614,13 @@ public : (concepts::PointDistanceStrategy) ); #endif - typedef typename return_type::type return_type; - typedef cross_track this_type; + using return_type = typename return_type::type; + using this_type = cross_track; - typedef typename services::comparable_type + using comparable_type = typename services::comparable_type < this_type - >::type comparable_type; + >::type; comparable_type cstrategy = services::get_comparable::apply(m_strategy); @@ -611,7 +653,7 @@ namespace services template struct tag > { - typedef strategy_tag_distance_point_segment type; + using type = strategy_tag_distance_point_segment; }; @@ -624,10 +666,10 @@ struct return_type, P, PS> template struct comparable_type > { - typedef comparable::cross_track + using type = comparable::cross_track < CalculationType, typename comparable_type::type - > type; + > ; }; @@ -638,10 +680,10 @@ template > struct get_comparable > { - typedef typename comparable_type + using comparable_type = typename comparable_type < cross_track - >::type comparable_type; + >::type; public : static inline comparable_type apply(cross_track const& strategy) @@ -661,10 +703,10 @@ template struct result_from_distance, P, PS> { private : - typedef typename cross_track + using return_type = typename cross_track < CalculationType, Strategy - >::template return_type::type return_type; + >::template return_type::type; public : template static inline return_type @@ -679,7 +721,7 @@ public : template struct tag > { - typedef strategy_tag_distance_point_segment type; + using type = strategy_tag_distance_point_segment; }; @@ -701,7 +743,7 @@ struct return_type, P, PS> template struct comparable_type > { - typedef comparable::cross_track type; + using type = comparable::cross_track; }; @@ -709,7 +751,7 @@ template struct get_comparable > { private : - typedef comparable::cross_track this_type; + using this_type = comparable::cross_track; public : static inline this_type apply(this_type const& input) { @@ -731,8 +773,8 @@ struct result_from_distance > { private : - typedef comparable::cross_track strategy_type; - typedef typename return_type::type return_type; + using strategy_type = comparable::cross_track; + using return_type = typename return_type::type; public : template static inline return_type apply(strategy_type const& strategy, @@ -783,7 +825,7 @@ struct default_strategy Strategy > { - typedef cross_track + using type = cross_track < void, std::conditional_t @@ -796,7 +838,7 @@ struct default_strategy >::type, Strategy > - > type; + >; }; @@ -808,12 +850,12 @@ struct default_strategy Strategy > { - typedef typename default_strategy + using type = typename default_strategy < point_tag, segment_tag, Point, PointOfSegment, spherical_equatorial_tag, spherical_equatorial_tag, Strategy - >::type type; + >::type; }; diff --git a/test/algorithms/closest_points/ar_ar.cpp b/test/algorithms/closest_points/ar_ar.cpp index 3307c98d7..eb4f667ac 100644 --- a/test/algorithms/closest_points/ar_ar.cpp +++ b/test/algorithms/closest_points/ar_ar.cpp @@ -455,12 +455,9 @@ BOOST_AUTO_TEST_CASE( test_all_areal_areal ) { test_all_ar_ar(cartesian()); - //double radius = bg::formula::mean_radius(bg::srs::spheroid()); - - //test_all_ar_ar(spherical_ps(), spherical_bb(), spherical_sb()); - //test_all_ar_ar(spherical_ps(radius), - // spherical_bb(radius), - // spherical_sb(radius)); + test_all_ar_ar(spherical()); + test_all_ar_ar(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); //test_all_ar_ar(andoyer_ps(), andoyer_bb(), andoyer_sb()); //test_all_ar_ar(thomas_ps(), thomas_bb(), thomas_sb()); diff --git a/test/algorithms/closest_points/l_ar.cpp b/test/algorithms/closest_points/l_ar.cpp index 98c4356cd..b5d8a3506 100644 --- a/test/algorithms/closest_points/l_ar.cpp +++ b/test/algorithms/closest_points/l_ar.cpp @@ -975,11 +975,10 @@ BOOST_AUTO_TEST_CASE( test_all_linear_areal ) { test_all_l_ar(cartesian()); - //double radius = bg::formula::mean_radius(bg::srs::spheroid()); - - //test_all_l_ar(spherical_ps(), spherical_sb()); - //test_all_l_ar(spherical_ps(radius), spherical_sb(radius)); - + test_all_l_ar(spherical()); + test_all_l_ar(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); + //test_all_l_ar(andoyer_ps(), andoyer_sb()); //test_all_l_ar(thomas_ps(), thomas_sb()); //test_all_l_ar(vincenty_ps(), vincenty_sb()); diff --git a/test/algorithms/closest_points/l_l.cpp b/test/algorithms/closest_points/l_l.cpp index 87e75947e..b88c35da1 100644 --- a/test/algorithms/closest_points/l_l.cpp +++ b/test/algorithms/closest_points/l_l.cpp @@ -286,10 +286,9 @@ BOOST_AUTO_TEST_CASE( test_all_linear_linear ) { test_all_l_l(cartesian()); - //double radius = bg::formula::mean_radius(bg::srs::spheroid()); - - //test_all_l_l(spherical_ps()); - //test_all_l_l(spherical_ps(radius)); + test_all_l_l(spherical()); + test_all_l_l(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); //test_all_l_l(andoyer_ps()); //test_all_l_l(thomas_ps()); diff --git a/test/algorithms/closest_points/pl_ar.cpp b/test/algorithms/closest_points/pl_ar.cpp index b511767e7..9dc009880 100644 --- a/test/algorithms/closest_points/pl_ar.cpp +++ b/test/algorithms/closest_points/pl_ar.cpp @@ -338,10 +338,9 @@ BOOST_AUTO_TEST_CASE( test_all_pointlike_areal ) { test_all_pl_ar(cartesian()); - //double radius = bg::formula::mean_radius(bg::srs::spheroid()); - - //test_all_pl_ar(spherical_ps(), spherical_pb()); - //test_all_pl_ar(spherical_ps(radius), spherical_pb(radius)); + test_all_pl_ar(spherical()); + test_all_pl_ar(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); //test_all_pl_ar(andoyer_ps(), andoyer_pb()); //test_all_pl_ar(thomas_ps(), thomas_pb()); diff --git a/test/algorithms/closest_points/pl_l.cpp b/test/algorithms/closest_points/pl_l.cpp index fd725ce51..dac765c68 100644 --- a/test/algorithms/closest_points/pl_l.cpp +++ b/test/algorithms/closest_points/pl_l.cpp @@ -276,10 +276,10 @@ BOOST_AUTO_TEST_CASE( test_all_pointlike_linear ) { test_all_pl_l(cartesian()); - //test_all_pl_l(spherical_ps()); - //test_all_pl_l(spherical_ps(bg::formula::mean_radius - // (bg::srs::spheroid()))); - + test_all_pl_l(spherical()); + test_all_pl_l(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); + //test_all_pl_l(andoyer_ps()); //test_all_pl_l(thomas_ps()); //test_all_pl_l(vincenty_ps()); diff --git a/test/algorithms/closest_points/pl_pl.cpp b/test/algorithms/closest_points/pl_pl.cpp index 5cde92d35..4e1768d08 100644 --- a/test/algorithms/closest_points/pl_pl.cpp +++ b/test/algorithms/closest_points/pl_pl.cpp @@ -148,8 +148,8 @@ BOOST_AUTO_TEST_CASE( test_all_pointlike_pointlike ) test_all_pl_pl(cartesian()); test_all_pl_pl(spherical()); - //test_all_pl_pl(spherical_pp(bg::formula::mean_radius - // (bg::srs::spheroid()))); + test_all_pl_pl(spherical( + bg::formula::mean_radius(bg::srs::spheroid()))); //test_all_pl_pl(andoyer_pp()); //test_all_pl_pl(thomas_pp());