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