boost/geometry/strategies/spherical/intersection.hpp

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