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