diff --git a/include/boost/geometry/strategies/geographic/geodesic_intersection.hpp b/include/boost/geometry/strategies/geographic/geodesic_intersection.hpp new file mode 100644 index 000000000..7d502978c --- /dev/null +++ b/include/boost/geometry/strategies/geographic/geodesic_intersection.hpp @@ -0,0 +1,703 @@ +// Boost.Geometry + +// Copyright (c) 2016, Oracle and/or its affiliates. +// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle + +// Use, modification and distribution is subject to the Boost Software License, +// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) + +#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_GEODESIC_INTERSECTION_HPP +#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_GEODESIC_INTERSECTION_HPP + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + + +namespace boost { namespace geometry +{ + +namespace strategy { namespace intersection +{ + +//TODO: Improve the robustness/accuracy/repeatability by +// moving all segments to 0 longitude +// switching the segments +// switching the endpoints +// BECAUSE: the result calculated by sjoberg intersection strategy +// is not consistent when the above are altered +// worse than that, the error is a lot greater either for lon or lat +// so make it consistently greater for lat + +template +< + typename Policy, + typename Spheroid = srs::spheroid, + template class Inverse = formula::andoyer_inverse, + unsigned int Order = 2, + typename CalculationType = void +> +struct relate_geodesic_segments +{ + typedef typename Policy::return_type return_type; + + enum intersection_point_flag { ipi_inters = 0, ipi_at_a1, ipi_at_a2, ipi_at_b1, ipi_at_b2 }; + + // segment_intersection_info cannot outlive relate_ecef_segments + template + struct segment_intersection_info + { + typedef typename select_most_precise + < + CoordinateType, double + >::type promoted_type; + + promoted_type comparable_length_a() const + { + return robust_ra.denominator(); + } + + promoted_type comparable_length_b() const + { + return robust_rb.denominator(); + } + + template + void assign_a(Point& point, Segment1 const& a, Segment2 const& b) const + { + assign(point, a, b); + } + template + void assign_b(Point& point, Segment1 const& a, Segment2 const& b) const + { + assign(point, a, b); + } + + template + void assign(Point& point, Segment1 const& a, Segment2 const& b) const + { + if (ip_flag == ipi_inters) + { + // TODO: assign the rest of coordinates + set_from_radian<0>(point, lon); + set_from_radian<1>(point, lat); + } + else if (ip_flag == ipi_at_a1) + { + detail::assign_point_from_index<0>(a, point); + } + else if (ip_flag == ipi_at_a2) + { + detail::assign_point_from_index<1>(a, point); + } + else if (ip_flag == ipi_at_b1) + { + detail::assign_point_from_index<0>(b, point); + } + else // ip_flag == ipi_at_b2 + { + detail::assign_point_from_index<1>(b, point); + } + } + + CoordinateType lon; + CoordinateType lat; + SegmentRatio robust_ra; + SegmentRatio robust_rb; + intersection_point_flag ip_flag; + }; + + // Relate segments a and b + template + static inline return_type apply(Segment1 const& a, Segment2 const& b, + RobustPolicy const& robust_policy) + { + typedef typename point_type::type point1_t; + typedef typename point_type::type point2_t; + point1_t a1, a2; + point2_t b1, b2; + + // TODO: use indexed_point_view if possible? + detail::assign_point_from_index<0>(a, a1); + detail::assign_point_from_index<1>(a, a2); + detail::assign_point_from_index<0>(b, b1); + detail::assign_point_from_index<1>(b, b2); + + return apply(a, b, robust_policy, a1, a2, b1, b2); + } + + // Relate segments a and b + template + static inline return_type apply(Segment1 const& a, Segment2 const& b, + RobustPolicy const&, + Point1 const& a1, Point1 const& a2, Point2 const& b1, Point2 const& b2) + { + BOOST_CONCEPT_ASSERT( (concepts::ConstSegment) ); + BOOST_CONCEPT_ASSERT( (concepts::ConstSegment) ); + + typedef typename select_calculation_type + ::type calc_t; + + // For now create it using default constructor. In the future it could + // be stored in strategy. However then apply() wouldn't be static and + // all relops and setops would have to take the strategy or model. + Spheroid spheroid_in; + // normalized spheroid + srs::spheroid spheroid(calc_t(1), + calc_t(get_radius<2>(spheroid_in)) // b/a + / calc_t(get_radius<0>(spheroid_in))); + + // TODO: check only 2 first coordinates here? + using geometry::detail::equals::equals_point_point; + bool a_is_point = equals_point_point(a1, a2); + bool b_is_point = equals_point_point(b1, b2); + + if(a_is_point && b_is_point) + { + return equals_point_point(a1, b2) + ? Policy::degenerate(a, true) + : Policy::disjoint() + ; + } + + calc_t const a1_lon = get_as_radian<0>(a1); + calc_t const a1_lat = get_as_radian<1>(a1); + calc_t const a2_lon = get_as_radian<0>(a2); + calc_t const a2_lat = get_as_radian<1>(a2); + calc_t const b1_lon = get_as_radian<0>(b1); + calc_t const b1_lat = get_as_radian<1>(b1); + calc_t const b2_lon = get_as_radian<0>(b2); + calc_t const b2_lat = get_as_radian<1>(b2); + + side_info sides; + + // NOTE: potential optimization, don't calculate distance at this point + // this would require to reimplement inverse strategy to allow + // calculation of distance if needed, probably also storing intermediate + // results somehow inside an object. + typedef Inverse inverse_dist_azi; + typedef typename inverse_dist_azi::result_type inverse_result; + + // TODO: no need to call inverse formula if we know that the points are equal + // distance can be set to 0 in this case and azimuth may be not calculated + bool const is_equal_a1_b1 = equals_point_point(a1, b1); + bool const is_equal_a2_b1 = equals_point_point(a2, b1); + + inverse_result res_b1_b2 = inverse_dist_azi::apply(b1_lon, b1_lat, b2_lon, b2_lat, spheroid); + inverse_result res_b1_a1 = inverse_dist_azi::apply(b1_lon, b1_lat, a1_lon, a1_lat, spheroid); + inverse_result res_b1_a2 = inverse_dist_azi::apply(b1_lon, b1_lat, a2_lon, a2_lat, spheroid); + sides.set<0>(is_equal_a1_b1 ? 0 : formula::azimuth_side_value(res_b1_a1.azimuth, res_b1_b2.azimuth), + is_equal_a2_b1 ? 0 : formula::azimuth_side_value(res_b1_a2.azimuth, res_b1_b2.azimuth)); + if (sides.same<0>()) + { + // Both points are at the same side of other segment, we can leave + return Policy::disjoint(); + } + + bool const is_equal_a1_b2 = equals_point_point(a1, b2); + + inverse_result res_a1_a2 = inverse_dist_azi::apply(a1_lon, a1_lat, a2_lon, a2_lat, spheroid); + inverse_result res_a1_b1 = inverse_dist_azi::apply(a1_lon, a1_lat, b1_lon, b1_lat, spheroid); + inverse_result res_a1_b2 = inverse_dist_azi::apply(a1_lon, a1_lat, b2_lon, b2_lat, spheroid); + sides.set<1>(is_equal_a1_b1 ? 0 : formula::azimuth_side_value(res_a1_b1.azimuth, res_a1_a2.azimuth), + is_equal_a1_b2 ? 0 : formula::azimuth_side_value(res_a1_b2.azimuth, res_a1_a2.azimuth)); + if (sides.same<1>()) + { + // Both points are at the same side of other segment, we can leave + return Policy::disjoint(); + } + + // NOTE: at this point the segments may still be disjoint + // NOTE: at this point one of the segments may be degenerated + + bool collinear = sides.collinear(); + + if (! collinear) + { + // WARNING: the side strategy doesn't have the info about the other + // segment so it may return results inconsistent with this intersection + // strategy, as it checks both segments for consistency + + if (sides.get<0, 0>() == 0 && sides.get<0, 1>() == 0) + { + collinear = true; + sides.set<1>(0, 0); + } + else if (sides.get<1, 0>() == 0 && sides.get<1, 1>() == 0) + { + collinear = true; + sides.set<0>(0, 0); + } + } + + if (collinear) + { + if (a_is_point) + { + return collinear_one_degenerted(a, true, b1, b2, a1, a2, res_b1_b2, res_b1_a1); + } + else if (b_is_point) + { + return collinear_one_degenerted(b, false, a1, a2, b1, b2, res_a1_a2, res_a1_b1); + } + else + { + calc_t dist_a1_a2, dist_a1_b1, dist_a1_b2; + calc_t dist_b1_b2, dist_b1_a1, dist_b1_a2; + // use shorter segment + if (res_a1_a2.distance <= res_b1_b2.distance) + { + calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_b1, dist_a1_a2, dist_a1_b1); + calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_b2, dist_a1_a2, dist_a1_b2); + dist_b1_b2 = dist_a1_b2 - dist_a1_b1; + dist_b1_a1 = -dist_a1_b1; + dist_b1_a2 = dist_a1_a2 - dist_a1_b1; + } + else + { + calculate_collinear_data(b1, b2, a1, a2, res_b1_b2, res_b1_a1, dist_b1_b2, dist_b1_a1); + calculate_collinear_data(b1, b2, a1, a2, res_b1_b2, res_b1_a2, dist_b1_b2, dist_b1_a2); + dist_a1_a2 = dist_b1_a2 - dist_b1_a1; + dist_a1_b1 = -dist_b1_a1; + dist_a1_b2 = dist_b1_b2 - dist_b1_a1; + } + + segment_ratio ra_from(dist_b1_a1, dist_b1_b2); + segment_ratio ra_to(dist_b1_a2, dist_b1_b2); + segment_ratio rb_from(dist_a1_b1, dist_a1_a2); + segment_ratio rb_to(dist_a1_b2, dist_a1_a2); + + calc_t const c0 = 0; + + // NOTE: this is probably not needed + int const a1_wrt_b = position_value(c0, dist_a1_b1, dist_a1_b2); + int const a2_wrt_b = position_value(dist_a1_a2, dist_a1_b1, dist_a1_b2); + int const b1_wrt_a = position_value(c0, dist_b1_a1, dist_b1_a2); + int const b2_wrt_a = position_value(dist_b1_b2, dist_b1_a1, dist_b1_a2); + + if (a1_wrt_b == 1) + { + ra_from.assign(0, dist_b1_b2); + rb_from.assign(0, dist_a1_a2); + } + else if (a1_wrt_b == 3) + { + ra_from.assign(dist_b1_b2, dist_b1_b2); + rb_to.assign(0, dist_a1_a2); + } + + if (a2_wrt_b == 1) + { + ra_to.assign(0, dist_b1_b2); + rb_from.assign(dist_a1_a2, dist_a1_a2); + } + else if (a2_wrt_b == 3) + { + ra_to.assign(dist_b1_b2, dist_b1_b2); + rb_to.assign(dist_a1_a2, dist_a1_a2); + } + + if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3)) + { + return Policy::disjoint(); + } + + bool const opposite = ! same_direction(res_a1_a2.azimuth, res_b1_b2.azimuth); + + return Policy::segments_collinear(a, b, opposite, + a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a, + ra_from, ra_to, rb_from, rb_to); + } + } + else // crossing or touching + { + if (a_is_point || b_is_point) + { + return Policy::disjoint(); + } + + calc_t lon = 0, lat = 0; + intersection_point_flag ip_flag; + calc_t dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1; + if (calculate_ip_data(a1, a2, b1, b2, + a1_lon, a1_lat, a2_lon, a2_lat, + b1_lon, b1_lat, b2_lon, b2_lat, + res_a1_a2, res_a1_b1, res_a1_b2, + res_b1_b2, res_b1_a1, res_b1_a2, + sides, spheroid, + lon, lat, + dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1, + ip_flag)) + { + // intersects + segment_intersection_info + < + calc_t, + segment_ratio + > sinfo; + + sinfo.lon = lon; + sinfo.lat = lat; + sinfo.robust_ra.assign(dist_a1_i1, dist_a1_a2); + sinfo.robust_rb.assign(dist_b1_i1, dist_b1_b2); + sinfo.ip_flag = ip_flag; + + return Policy::segments_crosses(sides, sinfo, a, b); + } + else + { + return Policy::disjoint(); + } + } + } + +private: + template + static inline return_type collinear_one_degenerted(Segment const& segment, bool degenerated_a, + Point1 const& a1, Point1 const& a2, + Point2 const& b1, Point2 const& b2, + ResultInverse const& res_a1_a2, + ResultInverse const& res_a1_bi) + { + CalcT dist_1_2, dist_1_o; + return ! calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_bi, dist_1_2, dist_1_o) + ? Policy::disjoint() + : Policy::one_degenerate(segment, segment_ratio(dist_1_o, dist_1_2), degenerated_a); + } + + // TODO: instead of the code below test bi against a1 and a2 here? + template + static inline bool calculate_collinear_data(Point1 const& a1, Point1 const& a2, // in + Point2 const& b1, Point2 const& b2, // in + ResultInverse const& res_a1_a2, // in + ResultInverse const& res_a1_bi, // in + CalcT& dist_a1_a2, CalcT& dist_a1_bi) // out + { + dist_a1_a2 = res_a1_a2.distance; + + dist_a1_bi = res_a1_bi.distance; + if (! same_direction(res_a1_bi.azimuth, res_a1_a2.azimuth)) + { + dist_a1_bi = -dist_a1_bi; + } + + // if i1 is close to a1 and b1 or b2 is equal to a1 + if (is_endpoint_equal(dist_a1_bi, a1, b1, b2)) + { + dist_a1_bi = 0; + return true; + } + // or i1 is close to a2 and b1 or b2 is equal to a2 + else if (is_endpoint_equal(dist_a1_a2 - dist_a1_bi, a2, b1, b2)) + { + dist_a1_bi = dist_a1_a2; + return true; + } + + // or i1 is on b + return segment_ratio(dist_a1_bi, dist_a1_a2).on_segment(); + } + + template + static inline bool calculate_ip_data(Point1 const& a1, Point1 const& a2, // in + Point2 const& b1, Point2 const& b2, // in + CalcT const& a1_lon, CalcT const& a1_lat, // in + CalcT const& a2_lon, CalcT const& a2_lat, // in + CalcT const& b1_lon, CalcT const& b1_lat, // in + CalcT const& b2_lon, CalcT const& b2_lat, // in + ResultInverse const& res_a1_a2, // in + ResultInverse const& res_a1_b1, // in + ResultInverse const& res_a1_b2, // in + ResultInverse const& res_b1_b2, // in + ResultInverse const& res_b1_a1, // in + ResultInverse const& res_b1_a2, // in + side_info const& sides, // in + Spheroid_ const& spheroid, // in + CalcT & lon, CalcT & lat, // out + CalcT& dist_a1_a2, CalcT& dist_a1_ip, // out + CalcT& dist_b1_b2, CalcT& dist_b1_ip, // out + intersection_point_flag& ip_flag) // out + { + dist_a1_a2 = res_a1_a2.distance; + dist_b1_b2 = res_b1_b2.distance; + + // assign the IP if some endpoints overlap + using geometry::detail::equals::equals_point_point; + if (equals_point_point(a1, b1)) + { + lon = a1_lon; + lat = a1_lat; + dist_a1_ip = 0; + dist_b1_ip = 0; + ip_flag = ipi_at_a1; + return true; + } + else if (equals_point_point(a1, b2)) + { + lon = a1_lon; + lat = a1_lat; + dist_a1_ip = 0; + dist_b1_ip = dist_b1_b2; + ip_flag = ipi_at_a1; + return true; + } + else if (equals_point_point(a2, b1)) + { + lon = a2_lon; + lat = a2_lat; + dist_a1_ip = dist_a1_a2; + dist_b1_ip = 0; + ip_flag = ipi_at_a2; + return true; + } + else if (equals_point_point(a2, b2)) + { + lon = a2_lon; + lat = a2_lat; + dist_a1_ip = dist_a1_a2; + dist_b1_ip = dist_b1_b2; + ip_flag = ipi_at_a2; + return true; + } + + // at this point we know that the endpoints doesn't overlap + // check cases when an endpoint lies on the other geodesic + if (sides.template get<0, 0>() == 0) // a1 wrt b + { + if (res_b1_a1.distance <= res_b1_b2.distance + && same_direction(res_b1_a1.azimuth, res_b1_b2.azimuth)) + { + lon = a1_lon; + lat = a1_lat; + dist_a1_ip = 0; + dist_b1_ip = res_b1_a1.distance; + ip_flag = ipi_at_a1; + return true; + } + else + { + return false; + } + } + else if (sides.template get<0, 1>() == 0) // a2 wrt b + { + if (res_b1_a2.distance <= res_b1_b2.distance + && same_direction(res_b1_a2.azimuth, res_b1_b2.azimuth)) + { + lon = a2_lon; + lat = a2_lat; + dist_a1_ip = res_a1_a2.distance; + dist_b1_ip = res_b1_a2.distance; + ip_flag = ipi_at_a2; + return true; + } + else + { + return false; + } + } + else if (sides.template get<1, 0>() == 0) // b1 wrt a + { + if (res_a1_b1.distance <= res_a1_a2.distance + && same_direction(res_a1_b1.azimuth, res_a1_a2.azimuth)) + { + lon = b1_lon; + lat = b1_lat; + dist_a1_ip = res_a1_b1.distance; + dist_b1_ip = 0; + ip_flag = ipi_at_b1; + return true; + } + else + { + return false; + } + } + else if (sides.template get<1, 1>() == 0) // b2 wrt a + { + if (res_a1_b2.distance <= res_a1_a2.distance + && same_direction(res_a1_b2.azimuth, res_a1_a2.azimuth)) + { + lon = b2_lon; + lat = b2_lat; + dist_a1_ip = res_a1_b2.distance; + dist_b1_ip = res_b1_b2.distance; + ip_flag = ipi_at_b2; + return true; + } + else + { + return false; + } + } + + // At this point neither the endpoints overlaps + // nor any andpoint lies on the other geodesic + // So the endpoints should lie on the opposite sides of both geodesics + + bool const ok = formula::sjoberg_intersection::apply( + a1_lon, a1_lat, a2_lon, a2_lat, res_a1_a2.azimuth, + b1_lon, b1_lat, b2_lon, b2_lat, res_b1_b2.azimuth, + lon, lat, spheroid); + + if (! ok) + { + return false; + } + + // TODO: it's theoretically possible to generate the IP outside the segment, + // i.e. the azimuth of the IP should be "smaller" than the azimuth of the endpoints + // i.e. the sides has to match, e.g. if one side is -1 and the other is 0 + // then the side of the IP has to be -1 or 0 as well + + // TODO: consider swapping points at the beginning if lon1 > lon2 + + typedef Inverse inverse_dist_azi; + typedef typename inverse_dist_azi::result_type inverse_result; + + inverse_result const res_a1_ip = inverse_dist_azi::apply(a1_lon, a1_lat, lon, lat, spheroid); + dist_a1_ip = res_a1_ip.distance; + if (! same_direction(res_a1_ip.azimuth, res_a1_a2.azimuth)) + { + dist_a1_ip = -dist_a1_ip; + } + + bool is_on_a = segment_ratio(dist_a1_ip, dist_a1_a2).on_segment(); + // NOTE: not fully consistent with equals_point_point() since radians are always used. + bool is_on_a1 = math::equals(lon, a1_lon) && math::equals(lat, a1_lat); + bool is_on_a2 = math::equals(lon, a2_lon) && math::equals(lat, a2_lat); + + if (! (is_on_a || is_on_a1 || is_on_a2)) + { + return false; + } + + inverse_result const res_b1_ip = inverse_dist_azi::apply(b1_lon, b1_lat, lon, lat, spheroid); + dist_b1_ip = res_b1_ip.distance; + if (! same_direction(res_b1_ip.azimuth, res_b1_b2.azimuth)) + { + dist_b1_ip = -dist_b1_ip; + } + + bool is_on_b = segment_ratio(dist_b1_ip, dist_b1_b2).on_segment(); + // NOTE: not fully consistent with equals_point_point() since radians are always used. + bool is_on_b1 = math::equals(lon, b1_lon) && math::equals(lat, b1_lat); + bool is_on_b2 = math::equals(lon, b2_lon) && math::equals(lat, b2_lat); + + if (! (is_on_b || is_on_b1 || is_on_b2)) + { + return false; + } + + ip_flag = ipi_inters; + + if (is_on_b1) + { + lon = b1_lon; + lat = b1_lat; + dist_b1_ip = 0; + ip_flag = ipi_at_b1; + } + else if (is_on_b2) + { + lon = b2_lon; + lat = b2_lat; + dist_b1_ip = res_b1_b2.distance; + ip_flag = ipi_at_b2; + } + + if (is_on_a1) + { + lon = a1_lon; + lat = a1_lat; + dist_a1_ip = 0; + ip_flag = ipi_at_a1; + } + else if (is_on_a2) + { + lon = a2_lon; + lat = a2_lat; + dist_a1_ip = res_a1_a2.distance; + ip_flag = ipi_at_a2; + } + + return true; + } + + template + static inline bool is_endpoint_equal(CalcT const& dist, + P1 const& ai, P2 const& b1, P2 const& b2) + { + using geometry::detail::equals::equals_point_point; + return is_near(dist) && (equals_point_point(ai, b1) || equals_point_point(ai, b2)); + } + + template + static inline bool is_near(CalcT const& dist) + { + // NOTE: This strongly depends on the Inverse method + CalcT const small_number = CalcT(boost::is_same::value ? 0.0001 : 0.00000001); + return math::abs(dist) <= small_number; + } + + template + static inline int position_value(ProjCoord1 const& ca1, + ProjCoord2 const& cb1, + ProjCoord2 const& cb2) + { + // S1x 0 1 2 3 4 + // S2 |----------> + return math::equals(ca1, cb1) ? 1 + : math::equals(ca1, cb2) ? 3 + : cb1 < cb2 ? + ( ca1 < cb1 ? 0 + : ca1 > cb2 ? 4 + : 2 ) + : ( ca1 > cb1 ? 0 + : ca1 < cb2 ? 4 + : 2 ); + } + + template + static inline bool same_direction(CalcT const& azimuth1, CalcT const& azimuth2) + { + // distance between two angles normalized to (-180, 180] + CalcT const angle_diff = math::longitude_distance_signed(azimuth1, azimuth2); + return math::abs(angle_diff) <= math::half_pi(); + } +}; + + +}} // namespace strategy::intersection + +}} // namespace boost::geometry + + +#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_GEODESIC_INTERSECTION_HPP