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