ceph/src/boost/boost/geometry/strategies/spherical/intersection.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.
   4
   5 // Copyright (c) 2016-2017, Oracle and/or its affiliates.
   6 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
   7
   8 // Use, modification and distribution is subject to the Boost Software License,
   9 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  10 // http://www.boost.org/LICENSE_1_0.txt)
  11
  12 #ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP
  13 #define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP
  14
  15 #include <algorithm>
  16
  17 #include <boost/geometry/core/cs.hpp>
  18 #include <boost/geometry/core/access.hpp>
  19 #include <boost/geometry/core/radian_access.hpp>
  20 #include <boost/geometry/core/tags.hpp>
  21
  22 #include <boost/geometry/algorithms/detail/assign_values.hpp>
  23 #include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
  24 #include <boost/geometry/algorithms/detail/equals/point_point.hpp>
  25 #include <boost/geometry/algorithms/detail/recalculate.hpp>
  26
  27 #include <boost/geometry/arithmetic/arithmetic.hpp>
  28 #include <boost/geometry/arithmetic/cross_product.hpp>
  29 #include <boost/geometry/arithmetic/dot_product.hpp>
  30 #include <boost/geometry/arithmetic/normalize.hpp>
  31 #include <boost/geometry/formulas/spherical.hpp>
  32
  33 #include <boost/geometry/geometries/concepts/point_concept.hpp>
  34 #include <boost/geometry/geometries/concepts/segment_concept.hpp>
  35
  36 #include <boost/geometry/policies/robustness/segment_ratio.hpp>
  37
  38 #include <boost/geometry/strategies/covered_by.hpp>
  39 #include <boost/geometry/strategies/intersection.hpp>
  40 #include <boost/geometry/strategies/intersection_result.hpp>
  41 #include <boost/geometry/strategies/side.hpp>
  42 #include <boost/geometry/strategies/side_info.hpp>
  43 #include <boost/geometry/strategies/spherical/area.hpp>
  44 #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
  45 #include <boost/geometry/strategies/spherical/envelope_segment.hpp>
  46 #include <boost/geometry/strategies/spherical/point_in_poly_winding.hpp>
  47 #include <boost/geometry/strategies/spherical/ssf.hpp>
  48 #include <boost/geometry/strategies/within.hpp>
  49
  50 #include <boost/geometry/util/math.hpp>
  51 #include <boost/geometry/util/select_calculation_type.hpp>
  52
  53
  54 namespace boost { namespace geometry
  55 {
  56
  57 namespace strategy { namespace intersection
  58 {
  59
  60 // NOTE:
  61 // The coordinates of crossing IP may be calculated with small precision in some cases.
  62 // For double, near the equator noticed error ~1e-9 so far greater than
  63 // machine epsilon which is ~1e-16. This error is ~0.04m.
  64 // E.g. consider two cases, one near the origin and the second one rotated by 90 deg around Z or SN axis.
  65 // After the conversion from spherical degrees to cartesian 3d the following coordinates
  66 // are calculated:
  67 // for sph (-1 -1,  1 1) deg cart3d ys are -0.017449748351250485 and  0.017449748351250485
  68 // for sph (89 -1, 91 1) deg cart3d xs are  0.017449748351250571 and -0.017449748351250450
  69 // During the conversion degrees must first be converted to radians and then radians
  70 // are passed into trigonometric functions. The error may have several causes:
  71 // 1. Radians cannot represent exactly the same angles as degrees.
  72 // 2. Different longitudes are passed into sin() for x, corresponding to cos() for y,
  73 //    and for different angle the error of the result may be different.
  74 // 3. These non-corresponding cartesian coordinates are used in calculation,
  75 //    e.g. multiplied several times in cross and dot products.
  76 // If it was a problem this strategy could e.g. "normalize" longitudes before the conversion using the source units
  77 // by rotating the globe around Z axis, so moving longitudes always the same way towards the origin,
  78 // assuming this could help which is not clear.
  79 // For now, intersection points near the endpoints are checked explicitly if needed (if the IP is near the endpoint)
  80 // to generate precise result for them. Only the crossing (i) case may suffer from lower precision.
  81
  82 template
  83 <
  84     typename CalcPolicy,
  85     typename CalculationType = void
  86 >
  87 struct ecef_segments
  88 {
  89     typedef side::spherical_side_formula<CalculationType> side_strategy_type;
  90
  91     static inline side_strategy_type get_side_strategy()
  92     {
  93         return side_strategy_type();
  94     }
  95
  96     template <typename Geometry1, typename Geometry2>
  97     struct point_in_geometry_strategy
  98     {
  99         typedef strategy::within::spherical_winding
 100             <
 101                 typename point_type<Geometry1>::type,
 102                 typename point_type<Geometry2>::type,
 103                 CalculationType
 104             > type;
 105     };
 106
 107     template <typename Geometry1, typename Geometry2>
 108     static inline typename point_in_geometry_strategy<Geometry1, Geometry2>::type
 109         get_point_in_geometry_strategy()
 110     {
 111         typedef typename point_in_geometry_strategy
 112             <
 113                 Geometry1, Geometry2
 114             >::type strategy_type;
 115         return strategy_type();
 116     }
 117
 118     template <typename Geometry>
 119     struct area_strategy
 120     {
 121         typedef area::spherical
 122             <
 123                 typename coordinate_type<Geometry>::type,
 124                 CalculationType
 125             > type;
 126     };
 127
 128     template <typename Geometry>
 129     static inline typename area_strategy<Geometry>::type get_area_strategy()
 130     {
 131         typedef typename area_strategy<Geometry>::type strategy_type;
 132         return strategy_type();
 133     }
 134
 135     template <typename Geometry>
 136     struct distance_strategy
 137     {
 138         typedef distance::haversine
 139             <
 140                 typename coordinate_type<Geometry>::type,
 141                 CalculationType
 142             > type;
 143     };
 144
 145     template <typename Geometry>
 146     static inline typename distance_strategy<Geometry>::type get_distance_strategy()
 147     {
 148         typedef typename distance_strategy<Geometry>::type strategy_type;
 149         return strategy_type();
 150     }
 151
 152     typedef envelope::spherical_segment<CalculationType>
 153         envelope_strategy_type;
 154
 155     static inline envelope_strategy_type get_envelope_strategy()
 156     {
 157         return envelope_strategy_type();
 158     }
 159
 160     enum intersection_point_flag { ipi_inters = 0, ipi_at_a1, ipi_at_a2, ipi_at_b1, ipi_at_b2 };
 161
 162     // segment_intersection_info cannot outlive relate_ecef_segments
 163     template <typename CoordinateType, typename SegmentRatio, typename Vector3d>
 164     struct segment_intersection_info
 165     {
 166         segment_intersection_info(CalcPolicy const& calc)
 167             : calc_policy(calc)
 168         {}
 169
 170         template <typename Point, typename Segment1, typename Segment2>
 171         void calculate(Point& point, Segment1 const& a, Segment2 const& b) const
 172         {
 173             if (ip_flag == ipi_inters)
 174             {
 175                 // TODO: assign the rest of coordinates
 176                 point = calc_policy.template from_cart3d<Point>(intersection_point);
 177             }
 178             else if (ip_flag == ipi_at_a1)
 179             {
 180                 detail::assign_point_from_index<0>(a, point);
 181             }
 182             else if (ip_flag == ipi_at_a2)
 183             {
 184                 detail::assign_point_from_index<1>(a, point);
 185             }
 186             else if (ip_flag == ipi_at_b1)
 187             {
 188                 detail::assign_point_from_index<0>(b, point);
 189             }
 190             else // ip_flag == ipi_at_b2
 191             {
 192                 detail::assign_point_from_index<1>(b, point);
 193             }
 194         }
 195
 196         Vector3d intersection_point;
 197         SegmentRatio robust_ra;
 198         SegmentRatio robust_rb;
 199         intersection_point_flag ip_flag;
 200
 201         CalcPolicy const& calc_policy;
 202     };
 203
 204     // Relate segments a and b
 205     template
 206     <
 207         typename Segment1,
 208         typename Segment2,
 209         typename Policy,
 210         typename RobustPolicy
 211     >
 212     static inline typename Policy::return_type
 213         apply(Segment1 const& a, Segment2 const& b,
 214               Policy const& policy, RobustPolicy const& robust_policy)
 215     {
 216         typedef typename point_type<Segment1>::type point1_t;
 217         typedef typename point_type<Segment2>::type point2_t;
 218         point1_t a1, a2;
 219         point2_t b1, b2;
 220
 221         // TODO: use indexed_point_view if possible?
 222         detail::assign_point_from_index<0>(a, a1);
 223         detail::assign_point_from_index<1>(a, a2);
 224         detail::assign_point_from_index<0>(b, b1);
 225         detail::assign_point_from_index<1>(b, b2);
 226
 227         return apply(a, b, policy, robust_policy, a1, a2, b1, b2);
 228     }
 229
 230     // Relate segments a and b
 231     template
 232     <
 233         typename Segment1,
 234         typename Segment2,
 235         typename Policy,
 236         typename RobustPolicy,
 237         typename Point1,
 238         typename Point2
 239     >
 240     static inline typename Policy::return_type
 241         apply(Segment1 const& a, Segment2 const& b,
 242               Policy const&, RobustPolicy const&,
 243               Point1 const& a1, Point1 const& a2, Point2 const& b1, Point2 const& b2)
 244     {
 245         // For now create it using default constructor. In the future it could
 246         //  be stored in strategy. However then apply() wouldn't be static and
 247         //  all relops and setops would have to take the strategy or model.
 248         // Initialize explicitly to prevent compiler errors in case of PoD type
 249         CalcPolicy const calc_policy = CalcPolicy();
 250
 251         BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment1>) );
 252         BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment2>) );
 253
 254         // TODO: check only 2 first coordinates here?
 255         using geometry::detail::equals::equals_point_point;
 256         bool a_is_point = equals_point_point(a1, a2);
 257         bool b_is_point = equals_point_point(b1, b2);
 258
 259         if(a_is_point && b_is_point)
 260         {
 261             return equals_point_point(a1, b2)
 262                 ? Policy::degenerate(a, true)
 263                 : Policy::disjoint()
 264                 ;
 265         }
 266
 267         typedef typename select_calculation_type
 268             <Segment1, Segment2, CalculationType>::type calc_t;
 269
 270         calc_t const c0 = 0;
 271         calc_t const c1 = 1;
 272
 273         typedef model::point<calc_t, 3, cs::cartesian> vec3d_t;
 274
 275         vec3d_t const a1v = calc_policy.template to_cart3d<vec3d_t>(a1);
 276         vec3d_t const a2v = calc_policy.template to_cart3d<vec3d_t>(a2);
 277         vec3d_t const b1v = calc_policy.template to_cart3d<vec3d_t>(b1);
 278         vec3d_t const b2v = calc_policy.template to_cart3d<vec3d_t>(b2);
 279
 280         bool degen_neq_coords = false;
 281         side_info sides;
 282
 283         typename CalcPolicy::template plane<vec3d_t>
 284             plane2 = calc_policy.get_plane(b1v, b2v);
 285
 286         calc_t dist_b1_b2 = 0;
 287         if (! b_is_point)
 288         {
 289             calculate_dist(b1v, b2v, plane2, dist_b1_b2);
 290             if (math::equals(dist_b1_b2, c0))
 291             {
 292                 degen_neq_coords = true;
 293                 b_is_point = true;
 294                 dist_b1_b2 = 0;
 295             }
 296             else
 297             {
 298                 // not normalized normals, the same as in side strategy
 299                 sides.set<0>(plane2.side_value(a1v), plane2.side_value(a2v));
 300                 if (sides.same<0>())
 301                 {
 302                     // Both points are at same side of other segment, we can leave
 303                     return Policy::disjoint();
 304                 }
 305             }
 306         }
 307
 308         typename CalcPolicy::template plane<vec3d_t>
 309             plane1 = calc_policy.get_plane(a1v, a2v);
 310
 311         calc_t dist_a1_a2 = 0;
 312         if (! a_is_point)
 313         {
 314             calculate_dist(a1v, a2v, plane1, dist_a1_a2);
 315             if (math::equals(dist_a1_a2, c0))
 316             {
 317                 degen_neq_coords = true;
 318                 a_is_point = true;
 319                 dist_a1_a2 = 0;
 320             }
 321             else
 322             {
 323                 // not normalized normals, the same as in side strategy
 324                 sides.set<1>(plane1.side_value(b1v), plane1.side_value(b2v));
 325                 if (sides.same<1>())
 326                 {
 327                     // Both points are at same side of other segment, we can leave
 328                     return Policy::disjoint();
 329                 }
 330             }
 331         }
 332
 333         // NOTE: at this point the segments may still be disjoint
 334
 335         calc_t len1 = 0;
 336         // point or opposite sides of a sphere/spheroid, assume point
 337         if (! a_is_point && ! detail::vec_normalize(plane1.normal, len1))
 338         {
 339             a_is_point = true;
 340             if (sides.get<0, 0>() == 0 || sides.get<0, 1>() == 0)
 341             {
 342                 sides.set<0>(0, 0);
 343             }
 344         }
 345
 346         calc_t len2 = 0;
 347         if (! b_is_point && ! detail::vec_normalize(plane2.normal, len2))
 348         {
 349             b_is_point = true;
 350             if (sides.get<1, 0>() == 0 || sides.get<1, 1>() == 0)
 351             {
 352                 sides.set<1>(0, 0);
 353             }
 354         }
 355
 356         // check both degenerated once more
 357         if (a_is_point && b_is_point)
 358         {
 359             return equals_point_point(a1, b2)
 360                 ? Policy::degenerate(a, true)
 361                 : Policy::disjoint()
 362                 ;
 363         }
 364
 365         // NOTE: at this point the segments may still be disjoint
 366         // NOTE: at this point one of the segments may be degenerated
 367
 368         bool collinear = sides.collinear();
 369
 370         if (! collinear)
 371         {
 372             // NOTE: for some approximations it's possible that both points may lie
 373             // on the same geodesic but still some of the sides may be != 0.
 374             // This is e.g. true for long segments represented as elliptic arcs
 375             // with origin different than the center of the coordinate system.
 376             // So make the sides consistent
 377
 378             // WARNING: the side strategy doesn't have the info about the other
 379             // segment so it may return results inconsistent with this intersection
 380             // strategy, as it checks both segments for consistency
 381
 382             if (sides.get<0, 0>() == 0 && sides.get<0, 1>() == 0)
 383             {
 384                 collinear = true;
 385                 sides.set<1>(0, 0);
 386             }
 387             else if (sides.get<1, 0>() == 0 && sides.get<1, 1>() == 0)
 388             {
 389                 collinear = true;
 390                 sides.set<0>(0, 0);
 391             }
 392         }
 393
 394         calc_t dot_n1n2 = dot_product(plane1.normal, plane2.normal);
 395
 396         // NOTE: this is technically not needed since theoretically above sides
 397         //       are calculated, but just in case check the normals.
 398         //       Have in mind that SSF side strategy doesn't check this.
 399         // collinear if normals are equal or opposite: cos(a) in {-1, 1}
 400         if (! collinear && math::equals(math::abs(dot_n1n2), c1))
 401         {
 402             collinear = true;
 403             sides.set<0>(0, 0);
 404             sides.set<1>(0, 0);
 405         }
 406
 407         if (collinear)
 408         {
 409             if (a_is_point)
 410             {
 411                 return collinear_one_degenerated<Policy, calc_t>(a, true, b1, b2, a1, a2, b1v, b2v,
 412                                                                  plane2, a1v, a2v, dist_b1_b2, degen_neq_coords);
 413             }
 414             else if (b_is_point)
 415             {
 416                 // b2 used to be consistent with (degenerated) checks above (is it needed?)
 417                 return collinear_one_degenerated<Policy, calc_t>(b, false, a1, a2, b1, b2, a1v, a2v,
 418                                                                  plane1, b1v, b2v, dist_a1_a2, degen_neq_coords);
 419             }
 420             else
 421             {
 422                 calc_t dist_a1_b1, dist_a1_b2;
 423                 calc_t dist_b1_a1, dist_b1_a2;
 424                 // use shorter segment
 425                 if (len1 <= len2)
 426                 {
 427                     calculate_collinear_data(a1, a2, b1, b2, a1v, a2v, plane1, b1v, b2v, dist_a1_a2, dist_a1_b1);
 428                     calculate_collinear_data(a1, a2, b1, b2, a1v, a2v, plane1, b2v, b1v, dist_a1_a2, dist_a1_b2);
 429                     dist_b1_b2 = dist_a1_b2 - dist_a1_b1;
 430                     dist_b1_a1 = -dist_a1_b1;
 431                     dist_b1_a2 = dist_a1_a2 - dist_a1_b1;
 432                 }
 433                 else
 434                 {
 435                     calculate_collinear_data(b1, b2, a1, a2, b1v, b2v, plane2, a1v, a2v, dist_b1_b2, dist_b1_a1);
 436                     calculate_collinear_data(b1, b2, a1, a2, b1v, b2v, plane2, a2v, a1v, dist_b1_b2, dist_b1_a2);
 437                     dist_a1_a2 = dist_b1_a2 - dist_b1_a1;
 438                     dist_a1_b1 = -dist_b1_a1;
 439                     dist_a1_b2 = dist_b1_b2 - dist_b1_a1;
 440                 }
 441
 442                 segment_ratio<calc_t> ra_from(dist_b1_a1, dist_b1_b2);
 443                 segment_ratio<calc_t> ra_to(dist_b1_a2, dist_b1_b2);
 444                 segment_ratio<calc_t> rb_from(dist_a1_b1, dist_a1_a2);
 445                 segment_ratio<calc_t> rb_to(dist_a1_b2, dist_a1_a2);
 446
 447                 // NOTE: this is probably not needed
 448                 int const a1_wrt_b = position_value(c0, dist_a1_b1, dist_a1_b2);
 449                 int const a2_wrt_b = position_value(dist_a1_a2, dist_a1_b1, dist_a1_b2);
 450                 int const b1_wrt_a = position_value(c0, dist_b1_a1, dist_b1_a2);
 451                 int const b2_wrt_a = position_value(dist_b1_b2, dist_b1_a1, dist_b1_a2);
 452
 453                 if (a1_wrt_b == 1)
 454                 {
 455                     ra_from.assign(0, dist_b1_b2);
 456                     rb_from.assign(0, dist_a1_a2);
 457                 }
 458                 else if (a1_wrt_b == 3)
 459                 {
 460                     ra_from.assign(dist_b1_b2, dist_b1_b2);
 461                     rb_to.assign(0, dist_a1_a2);
 462                 }
 463
 464                 if (a2_wrt_b == 1)
 465                 {
 466                     ra_to.assign(0, dist_b1_b2);
 467                     rb_from.assign(dist_a1_a2, dist_a1_a2);
 468                 }
 469                 else if (a2_wrt_b == 3)
 470                 {
 471                     ra_to.assign(dist_b1_b2, dist_b1_b2);
 472                     rb_to.assign(dist_a1_a2, dist_a1_a2);
 473                 }
 474
 475                 if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3))
 476                 {
 477                     return Policy::disjoint();
 478                 }
 479
 480                 bool const opposite = dot_n1n2 < c0;
 481
 482                 return Policy::segments_collinear(a, b, opposite,
 483                     a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a,
 484                     ra_from, ra_to, rb_from, rb_to);
 485             }
 486         }
 487         else // crossing
 488         {
 489             if (a_is_point || b_is_point)
 490             {
 491                 return Policy::disjoint();
 492             }
 493
 494             vec3d_t i1;
 495             intersection_point_flag ip_flag;
 496             calc_t dist_a1_i1, dist_b1_i1;
 497             if (calculate_ip_data(a1, a2, b1, b2, a1v, a2v, b1v, b2v,
 498                                   plane1, plane2, calc_policy,
 499                                   sides, dist_a1_a2, dist_b1_b2,
 500                                   i1, dist_a1_i1, dist_b1_i1, ip_flag))
 501             {
 502                 // intersects
 503                 segment_intersection_info
 504                     <
 505                         calc_t,
 506                         segment_ratio<calc_t>,
 507                         vec3d_t
 508                     > sinfo(calc_policy);
 509
 510                 sinfo.robust_ra.assign(dist_a1_i1, dist_a1_a2);
 511                 sinfo.robust_rb.assign(dist_b1_i1, dist_b1_b2);
 512                 sinfo.intersection_point = i1;
 513                 sinfo.ip_flag = ip_flag;
 514
 515                 return Policy::segments_crosses(sides, sinfo, a, b);
 516             }
 517             else
 518             {
 519                 return Policy::disjoint();
 520             }
 521         }
 522     }
 523
 524 private:
 525     template <typename Policy, typename CalcT, typename Segment, typename Point1, typename Point2, typename Vec3d, typename Plane>
 526     static inline typename Policy::return_type
 527         collinear_one_degenerated(Segment const& segment, bool degenerated_a,
 528                                   Point1 const& a1, Point1 const& a2,
 529                                   Point2 const& b1, Point2 const& b2,
 530                                   Vec3d const& a1v, Vec3d const& a2v,
 531                                   Plane const& plane,
 532                                   Vec3d const& b1v, Vec3d const& b2v,
 533                                   CalcT const& dist_1_2,
 534                                   bool degen_neq_coords)
 535     {
 536         CalcT dist_1_o;
 537         return ! calculate_collinear_data(a1, a2, b1, b2, a1v, a2v, plane, b1v, b2v, dist_1_2, dist_1_o, degen_neq_coords)
 538                 ? Policy::disjoint()
 539                 : Policy::one_degenerate(segment, segment_ratio<CalcT>(dist_1_o, dist_1_2), degenerated_a);
 540     }
 541
 542     template <typename Point1, typename Point2, typename Vec3d, typename Plane, typename CalcT>
 543     static inline bool calculate_collinear_data(Point1 const& a1, Point1 const& a2, // in
 544                                                 Point2 const& b1, Point2 const& b2, // in
 545                                                 Vec3d const& a1v,                   // in
 546                                                 Vec3d const& a2v,                   // in
 547                                                 Plane const& plane1,                // in
 548                                                 Vec3d const& b1v,                   // in
 549                                                 Vec3d const& b2v,                   // in
 550                                                 CalcT const& dist_a1_a2,            // in
 551                                                 CalcT& dist_a1_i1,                  // out
 552                                                 bool degen_neq_coords = false)      // in
 553     {
 554         // calculate dist_a1_a2 and dist_a1_i1
 555         calculate_dist(a1v, a2v, plane1, b1v, dist_a1_i1);
 556
 557         // if i1 is close to a1 and b1 or b2 is equal to a1
 558         if (is_endpoint_equal(dist_a1_i1, a1, b1, b2))
 559         {
 560             dist_a1_i1 = 0;
 561             return true;
 562         }
 563         // or i1 is close to a2 and b1 or b2 is equal to a2
 564         else if (is_endpoint_equal(dist_a1_a2 - dist_a1_i1, a2, b1, b2))
 565         {
 566             dist_a1_i1 = dist_a1_a2;
 567             return true;
 568         }
 569
 570         // check the other endpoint of a very short segment near the pole
 571         if (degen_neq_coords)
 572         {
 573             static CalcT const c0 = 0;
 574
 575             CalcT dist_a1_i2 = 0;
 576             calculate_dist(a1v, a2v, plane1, b2v, dist_a1_i2);
 577
 578             if (math::equals(dist_a1_i2, c0))
 579             {
 580                 dist_a1_i1 = 0;
 581                 return true;
 582             }
 583             else if (math::equals(dist_a1_a2 - dist_a1_i2, c0))
 584             {
 585                 dist_a1_i1 = dist_a1_a2;
 586                 return true;
 587             }
 588         }
 589
 590         // or i1 is on b
 591         return segment_ratio<CalcT>(dist_a1_i1, dist_a1_a2).on_segment();
 592     }
 593
 594     template <typename Point1, typename Point2, typename Vec3d, typename Plane, typename CalcT>
 595     static inline bool calculate_ip_data(Point1 const& a1, Point1 const& a2, // in
 596                                          Point2 const& b1, Point2 const& b2, // in
 597                                          Vec3d const& a1v, Vec3d const& a2v, // in
 598                                          Vec3d const& b1v, Vec3d const& b2v, // in
 599                                          Plane const& plane1,                // in
 600                                          Plane const& plane2,                // in
 601                                          CalcPolicy const& calc_policy,      // in
 602                                          side_info const& sides,             // in
 603                                          CalcT const& dist_a1_a2,            // in
 604                                          CalcT const& dist_b1_b2,            // in
 605                                          Vec3d & ip,                         // out
 606                                          CalcT& dist_a1_ip,                  // out
 607                                          CalcT& dist_b1_ip,                  // out
 608                                          intersection_point_flag& ip_flag)   // out
 609     {
 610         Vec3d ip1, ip2;
 611         calc_policy.intersection_points(plane1, plane2, ip1, ip2);
 612
 613         calculate_dist(a1v, a2v, plane1, ip1, dist_a1_ip);
 614         ip = ip1;
 615
 616         // choose the opposite side of the globe if the distance is shorter
 617         {
 618             CalcT const d = abs_distance(dist_a1_a2, dist_a1_ip);
 619             if (d > CalcT(0))
 620             {
 621                 // TODO: this should be ok not only for sphere
 622                 //       but requires more investigation
 623                 CalcT const dist_a1_i2 = dist_of_i2(dist_a1_ip);
 624                 CalcT const d2 = abs_distance(dist_a1_a2, dist_a1_i2);
 625                 if (d2 < d)
 626                 {
 627                     dist_a1_ip = dist_a1_i2;
 628                     ip = ip2;
 629                 }
 630             }
 631         }
 632
 633         bool is_on_a = false, is_near_a1 = false, is_near_a2 = false;
 634         if (! is_potentially_crossing(dist_a1_a2, dist_a1_ip, is_on_a, is_near_a1, is_near_a2))
 635         {
 636             return false;
 637         }
 638
 639         calculate_dist(b1v, b2v, plane2, ip, dist_b1_ip);
 640
 641         bool is_on_b = false, is_near_b1 = false, is_near_b2 = false;
 642         if (! is_potentially_crossing(dist_b1_b2, dist_b1_ip, is_on_b, is_near_b1, is_near_b2))
 643         {
 644             return false;
 645         }
 646
 647         // reassign the IP if some endpoints overlap
 648         using geometry::detail::equals::equals_point_point;
 649         if (is_near_a1)
 650         {
 651             if (is_near_b1 && equals_point_point(a1, b1))
 652             {
 653                 dist_a1_ip = 0;
 654                 dist_b1_ip = 0;
 655                 //i1 = a1v;
 656                 ip_flag = ipi_at_a1;
 657                 return true;
 658             }
 659
 660             if (is_near_b2 && equals_point_point(a1, b2))
 661             {
 662                 dist_a1_ip = 0;
 663                 dist_b1_ip = dist_b1_b2;
 664                 //i1 = a1v;
 665                 ip_flag = ipi_at_a1;
 666                 return true;
 667             }
 668         }
 669
 670         if (is_near_a2)
 671         {
 672             if (is_near_b1 && equals_point_point(a2, b1))
 673             {
 674                 dist_a1_ip = dist_a1_a2;
 675                 dist_b1_ip = 0;
 676                 //i1 = a2v;
 677                 ip_flag = ipi_at_a2;
 678                 return true;
 679             }
 680
 681             if (is_near_b2 && equals_point_point(a2, b2))
 682             {
 683                 dist_a1_ip = dist_a1_a2;
 684                 dist_b1_ip = dist_b1_b2;
 685                 //i1 = a2v;
 686                 ip_flag = ipi_at_a2;
 687                 return true;
 688             }
 689         }
 690
 691         // at this point we know that the endpoints doesn't overlap
 692         // reassign IP and distance if the IP is on a segment and one of
 693         //   the endpoints of the other segment lies on the former segment
 694         if (is_on_a)
 695         {
 696             if (is_near_b1 && sides.template get<1, 0>() == 0) // b1 wrt a
 697             {
 698                 dist_b1_ip = 0;
 699                 //i1 = b1v;
 700                 ip_flag = ipi_at_b1;
 701                 return true;
 702             }
 703
 704             if (is_near_b2 && sides.template get<1, 1>() == 0) // b2 wrt a
 705             {
 706                 dist_b1_ip = dist_b1_b2;
 707                 //i1 = b2v;
 708                 ip_flag = ipi_at_b2;
 709                 return true;
 710             }
 711         }
 712
 713         if (is_on_b)
 714         {
 715             if (is_near_a1 && sides.template get<0, 0>() == 0) // a1 wrt b
 716             {
 717                 dist_a1_ip = 0;
 718                 //i1 = a1v;
 719                 ip_flag = ipi_at_a1;
 720                 return true;
 721             }
 722
 723             if (is_near_a2 && sides.template get<0, 1>() == 0) // a2 wrt b
 724             {
 725                 dist_a1_ip = dist_a1_a2;
 726                 //i1 = a2v;
 727                 ip_flag = ipi_at_a2;
 728                 return true;
 729             }
 730         }
 731
 732         ip_flag = ipi_inters;
 733
 734         return is_on_a && is_on_b;
 735     }
 736
 737     template <typename Vec3d, typename Plane, typename CalcT>
 738     static inline void calculate_dist(Vec3d const& a1v,    // in
 739                                       Vec3d const& a2v,    // in
 740                                       Plane const& plane1, // in
 741                                       CalcT& dist_a1_a2)   // out
 742     {
 743         static CalcT const c1 = 1;
 744         CalcT const cos_a1_a2 = plane1.cos_angle_between(a1v, a2v);
 745         dist_a1_a2 = -cos_a1_a2 + c1; // [1, -1] -> [0, 2] representing [0, pi]
 746     }
 747
 748     template <typename Vec3d, typename Plane, typename CalcT>
 749     static inline void calculate_dist(Vec3d const& a1v,     // in
 750                                       Vec3d const& /*a2v*/, // in
 751                                       Plane const& plane1,  // in
 752                                       Vec3d const& i1,      // in
 753                                       CalcT& dist_a1_i1)    // out
 754     {
 755         static CalcT const c1 = 1;
 756         static CalcT const c2 = 2;
 757         static CalcT const c4 = 4;
 758
 759         bool is_forward = true;
 760         CalcT cos_a1_i1 = plane1.cos_angle_between(a1v, i1, is_forward);
 761         dist_a1_i1 = -cos_a1_i1 + c1; // [0, 2] representing [0, pi]
 762         if (! is_forward) // left or right of a1 on a
 763         {
 764             dist_a1_i1 = -dist_a1_i1; // [0, 2] -> [0, -2] representing [0, -pi]
 765         }
 766         if (dist_a1_i1 <= -c2) // <= -pi
 767         {
 768             dist_a1_i1 += c4; // += 2pi
 769         }
 770     }
 771     /*
 772     template <typename Vec3d, typename Plane, typename CalcT>
 773     static inline void calculate_dists(Vec3d const& a1v,    // in
 774                                        Vec3d const& a2v,    // in
 775                                        Plane const& plane1, // in
 776                                        Vec3d const& i1,     // in
 777                                        CalcT& dist_a1_a2, // out
 778                                        CalcT& dist_a1_i1) // out
 779     {
 780         calculate_dist(a1v, a2v, plane1, dist_a1_a2);
 781         calculate_dist(a1v, a2v, plane1, i1, dist_a1_i1);
 782     }
 783     */
 784     // the dist of the ip on the other side of the sphere
 785     template <typename CalcT>
 786     static inline CalcT dist_of_i2(CalcT const& dist_a1_i1)
 787     {
 788         CalcT const c2 = 2;
 789         CalcT const c4 = 4;
 790
 791         CalcT dist_a1_i2 = dist_a1_i1 - c2; // dist_a1_i2 = dist_a1_i1 - pi;
 792         if (dist_a1_i2 <= -c2)          // <= -pi
 793         {
 794             dist_a1_i2 += c4;           // += 2pi;
 795         }
 796         return dist_a1_i2;
 797     }
 798
 799     template <typename CalcT>
 800     static inline CalcT abs_distance(CalcT const& dist_a1_a2, CalcT const& dist_a1_i1)
 801     {
 802         if (dist_a1_i1 < CalcT(0))
 803             return -dist_a1_i1;
 804         else if (dist_a1_i1 > dist_a1_a2)
 805             return dist_a1_i1 - dist_a1_a2;
 806         else
 807             return CalcT(0);
 808     }
 809
 810     template <typename CalcT>
 811     static inline bool is_potentially_crossing(CalcT const& dist_a1_a2, CalcT const& dist_a1_i1, // in
 812                                                bool& is_on_a, bool& is_near_a1, bool& is_near_a2) // out
 813     {
 814         is_on_a = segment_ratio<CalcT>(dist_a1_i1, dist_a1_a2).on_segment();
 815         is_near_a1 = is_near(dist_a1_i1);
 816         is_near_a2 = is_near(dist_a1_a2 - dist_a1_i1);
 817         return is_on_a || is_near_a1 || is_near_a2;
 818     }
 819
 820     template <typename CalcT, typename P1, typename P2>
 821     static inline bool is_endpoint_equal(CalcT const& dist,
 822                                          P1 const& ai, P2 const& b1, P2 const& b2)
 823     {
 824         static CalcT const c0 = 0;
 825         using geometry::detail::equals::equals_point_point;
 826         return is_near(dist) && (equals_point_point(ai, b1) || equals_point_point(ai, b2) || math::equals(dist, c0));
 827     }
 828
 829     template <typename CalcT>
 830     static inline bool is_near(CalcT const& dist)
 831     {
 832         CalcT const small_number = CalcT(boost::is_same<CalcT, float>::value ? 0.0001 : 0.00000001);
 833         return math::abs(dist) <= small_number;
 834     }
 835
 836     template <typename ProjCoord1, typename ProjCoord2>
 837     static inline int position_value(ProjCoord1 const& ca1,
 838                                      ProjCoord2 const& cb1,
 839                                      ProjCoord2 const& cb2)
 840     {
 841         // S1x  0   1    2     3   4
 842         // S2       |---------->
 843         return math::equals(ca1, cb1) ? 1
 844              : math::equals(ca1, cb2) ? 3
 845              : cb1 < cb2 ?
 846                 ( ca1 < cb1 ? 0
 847                 : ca1 > cb2 ? 4
 848                 : 2 )
 849               : ( ca1 > cb1 ? 0
 850                 : ca1 < cb2 ? 4
 851                 : 2 );
 852     }
 853 };
 854
 855 struct spherical_segments_calc_policy
 856 {
 857     template <typename Point, typename Point3d>
 858     static Point from_cart3d(Point3d const& point_3d)
 859     {
 860         return formula::cart3d_to_sph<Point>(point_3d);
 861     }
 862
 863     template <typename Point3d, typename Point>
 864     static Point3d to_cart3d(Point const& point)
 865     {
 866         return formula::sph_to_cart3d<Point3d>(point);
 867     }
 868
 869     template <typename Point3d>
 870     struct plane
 871     {
 872         typedef typename coordinate_type<Point3d>::type coord_t;
 873
 874         // not normalized
 875         plane(Point3d const& p1, Point3d const& p2)
 876             : normal(cross_product(p1, p2))
 877         {}
 878
 879         int side_value(Point3d const& pt) const
 880         {
 881             return formula::sph_side_value(normal, pt);
 882         }
 883
 884         static coord_t cos_angle_between(Point3d const& p1, Point3d const& p2)
 885         {
 886             return dot_product(p1, p2);
 887         }
 888
 889         coord_t cos_angle_between(Point3d const& p1, Point3d const& p2, bool & is_forward) const
 890         {
 891             coord_t const c0 = 0;
 892             is_forward = dot_product(normal, cross_product(p1, p2)) >= c0;
 893             return dot_product(p1, p2);
 894         }
 895
 896         Point3d normal;
 897     };
 898
 899     template <typename Point3d>
 900     static plane<Point3d> get_plane(Point3d const& p1, Point3d const& p2)
 901     {
 902         return plane<Point3d>(p1, p2);
 903     }
 904
 905     template <typename Point3d>
 906     static bool intersection_points(plane<Point3d> const& plane1,
 907                                     plane<Point3d> const& plane2,
 908                                     Point3d & ip1, Point3d & ip2)
 909     {
 910         typedef typename coordinate_type<Point3d>::type coord_t;
 911
 912         ip1 = cross_product(plane1.normal, plane2.normal);
 913         // NOTE: the length should be greater than 0 at this point
 914         //       if the normals were not normalized and their dot product
 915         //       not checked before this function is called the length
 916         //       should be checked here (math::equals(len, c0))
 917         coord_t const len = math::sqrt(dot_product(ip1, ip1));
 918         divide_value(ip1, len); // normalize i1
 919
 920         ip2 = ip1;
 921         multiply_value(ip2, coord_t(-1));
 922
 923         return true;
 924     }
 925 };
 926
 927
 928 template
 929 <
 930     typename CalculationType = void
 931 >
 932 struct spherical_segments
 933     : ecef_segments
 934         <
 935             spherical_segments_calc_policy,
 936             CalculationType
 937         >
 938 {};
 939
 940
 941 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
 942 namespace services
 943 {
 944
 945 /*template <typename CalculationType>
 946 struct default_strategy<spherical_polar_tag, CalculationType>
 947 {
 948     typedef spherical_segments<CalculationType> type;
 949 };*/
 950
 951 template <typename CalculationType>
 952 struct default_strategy<spherical_equatorial_tag, CalculationType>
 953 {
 954     typedef spherical_segments<CalculationType> type;
 955 };
 956
 957 template <typename CalculationType>
 958 struct default_strategy<geographic_tag, CalculationType>
 959 {
 960     // NOTE: Spherical strategy returns the same result as the geographic one
 961     // representing segments as great elliptic arcs. If the elliptic arcs are
 962     // not great elliptic arcs (the origin not in the center of the coordinate
 963     // system) then there may be problems with consistency of the side and
 964     // intersection strategies.
 965     typedef spherical_segments<CalculationType> type;
 966 };
 967
 968 } // namespace services
 969 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
 970
 971
 972 }} // namespace strategy::intersection
 973
 974
 975 namespace strategy
 976 {
 977
 978 namespace within { namespace services
 979 {
 980
 981 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
 982 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, spherical_tag, spherical_tag>
 983 {
 984     typedef strategy::intersection::spherical_segments<> type;
 985 };
 986
 987 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
 988 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, spherical_tag, spherical_tag>
 989 {
 990     typedef strategy::intersection::spherical_segments<> type;
 991 };
 992
 993 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
 994 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, spherical_tag, spherical_tag>
 995 {
 996     typedef strategy::intersection::spherical_segments<> type;
 997 };
 998
 999 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
1000 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, spherical_tag, spherical_tag>
1001 {
1002     typedef strategy::intersection::spherical_segments<> type;
1003 };
1004
1005 }} // within::services
1006
1007 namespace covered_by { namespace services
1008 {
1009
1010 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
1011 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, spherical_tag, spherical_tag>
1012 {
1013     typedef strategy::intersection::spherical_segments<> type;
1014 };
1015
1016 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
1017 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, spherical_tag, spherical_tag>
1018 {
1019     typedef strategy::intersection::spherical_segments<> type;
1020 };
1021
1022 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
1023 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, spherical_tag, spherical_tag>
1024 {
1025     typedef strategy::intersection::spherical_segments<> type;
1026 };
1027
1028 template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
1029 struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, spherical_tag, spherical_tag>
1030 {
1031     typedef strategy::intersection::spherical_segments<> type;
1032 };
1033
1034 }} // within::services
1035
1036 } // strategy
1037
1038
1039 }} // namespace boost::geometry
1040
1041
1042 #endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP