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 get_side_strategyboost::geometry::strategy::intersection::ecef_segments97 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 get_point_in_geometry_strategyboost::geometry::strategy::intersection::ecef_segments115 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> get_area_strategyboost::geometry::strategy::intersection::ecef_segments135 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> get_distance_strategyboost::geometry::strategy::intersection::ecef_segments152 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 get_envelope_strategyboost::geometry::strategy::intersection::ecef_segments161 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 get_expand_strategyboost::geometry::strategy::intersection::ecef_segments169 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 get_point_in_point_strategyboost::geometry::strategy::intersection::ecef_segments176 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 get_equals_point_point_strategyboost::geometry::strategy::intersection::ecef_segments183 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 get_disjoint_box_box_strategyboost::geometry::strategy::intersection::ecef_segments190 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 get_disjoint_segment_box_strategyboost::geometry::strategy::intersection::ecef_segments197 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 { segment_intersection_infoboost::geometry::strategy::intersection::ecef_segments::segment_intersection_info214 segment_intersection_info(CalcPolicy const& calc) 215 : calc_policy(calc) 216 {} 217 218 template <typename Point, typename Segment1, typename Segment2> calculateboost::geometry::strategy::intersection::ecef_segments::segment_intersection_info219 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 applyboost::geometry::strategy::intersection::ecef_segments260 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 collinear_one_degeneratedboost::geometry::strategy::intersection::ecef_segments553 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> calculate_collinear_databoost::geometry::strategy::intersection::ecef_segments569 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> calculate_ip_databoost::geometry::strategy::intersection::ecef_segments621 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> calculate_distboost::geometry::strategy::intersection::ecef_segments767 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> calculate_distboost::geometry::strategy::intersection::ecef_segments778 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> dist_of_i2boost::geometry::strategy::intersection::ecef_segments815 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> abs_distanceboost::geometry::strategy::intersection::ecef_segments829 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> is_potentially_crossingboost::geometry::strategy::intersection::ecef_segments840 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> is_endpoint_equalboost::geometry::strategy::intersection::ecef_segments850 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> is_nearboost::geometry::strategy::intersection::ecef_segments858 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> position_valueboost::geometry::strategy::intersection::ecef_segments865 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> equals_point_pointboost::geometry::strategy::intersection::ecef_segments883 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> from_cart3dboost::geometry::strategy::intersection::spherical_segments_calc_policy893 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> to_cart3dboost::geometry::strategy::intersection::spherical_segments_calc_policy899 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 planeboost::geometry::strategy::intersection::spherical_segments_calc_policy::plane910 plane(Point3d const& p1, Point3d const& p2) 911 : normal(cross_product(p1, p2)) 912 {} 913 side_valueboost::geometry::strategy::intersection::spherical_segments_calc_policy::plane914 int side_value(Point3d const& pt) const 915 { 916 return formula::sph_side_value(normal, pt); 917 } 918 cos_angle_betweenboost::geometry::strategy::intersection::spherical_segments_calc_policy::plane919 static coord_t cos_angle_between(Point3d const& p1, Point3d const& p2) 920 { 921 return dot_product(p1, p2); 922 } 923 cos_angle_betweenboost::geometry::strategy::intersection::spherical_segments_calc_policy::plane924 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> get_planeboost::geometry::strategy::intersection::spherical_segments_calc_policy935 static plane<Point3d> get_plane(Point3d const& p1, Point3d const& p2) 936 { 937 return plane<Point3d>(p1, p2); 938 } 939 940 template <typename Point3d> intersection_pointsboost::geometry::strategy::intersection::spherical_segments_calc_policy941 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 1078