]>
Commit | Line | Data |
---|---|---|
b32b8144 FG |
1 | // Boost.Geometry (aka GGL, Generic Geometry Library) |
2 | ||
20effc67 | 3 | // Copyright (c) 2016-2020, Oracle and/or its affiliates. |
b32b8144 FG |
4 | |
5 | // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle | |
92f5a8d4 | 6 | // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle |
b32b8144 FG |
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_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP | |
13 | #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP | |
14 | ||
15 | #include <algorithm> | |
16 | ||
92f5a8d4 TL |
17 | #include <boost/tuple/tuple.hpp> |
18 | #include <boost/algorithm/minmax.hpp> | |
b32b8144 FG |
19 | #include <boost/config.hpp> |
20 | #include <boost/concept_check.hpp> | |
b32b8144 FG |
21 | |
22 | #include <boost/geometry/core/cs.hpp> | |
23 | #include <boost/geometry/core/access.hpp> | |
24 | #include <boost/geometry/core/radian_access.hpp> | |
25 | #include <boost/geometry/core/tags.hpp> | |
26 | ||
27 | #include <boost/geometry/strategies/distance.hpp> | |
28 | #include <boost/geometry/strategies/concepts/distance_concept.hpp> | |
92f5a8d4 | 29 | #include <boost/geometry/strategies/spherical/distance_cross_track.hpp> |
b32b8144 | 30 | #include <boost/geometry/strategies/spherical/distance_haversine.hpp> |
92f5a8d4 | 31 | #include <boost/geometry/strategies/spherical/point_in_point.hpp> |
b32b8144 | 32 | #include <boost/geometry/strategies/geographic/azimuth.hpp> |
92f5a8d4 | 33 | #include <boost/geometry/strategies/geographic/distance.hpp> |
b32b8144 | 34 | #include <boost/geometry/strategies/geographic/parameters.hpp> |
92f5a8d4 | 35 | #include <boost/geometry/strategies/geographic/intersection.hpp> |
b32b8144 FG |
36 | |
37 | #include <boost/geometry/formulas/vincenty_direct.hpp> | |
38 | ||
39 | #include <boost/geometry/util/math.hpp> | |
40 | #include <boost/geometry/util/promote_floating_point.hpp> | |
41 | #include <boost/geometry/util/select_calculation_type.hpp> | |
42 | #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp> | |
43 | ||
44 | #include <boost/geometry/formulas/result_direct.hpp> | |
45 | #include <boost/geometry/formulas/mean_radius.hpp> | |
92f5a8d4 | 46 | #include <boost/geometry/formulas/spherical.hpp> |
b32b8144 FG |
47 | |
48 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
11fdf7f2 | 49 | #include <boost/geometry/io/dsv/write.hpp> |
b32b8144 FG |
50 | #endif |
51 | ||
52 | #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS | |
53 | #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100 | |
54 | #endif | |
55 | ||
56 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
57 | #include <iostream> | |
58 | #endif | |
59 | ||
60 | namespace boost { namespace geometry | |
61 | { | |
62 | ||
63 | namespace strategy { namespace distance | |
64 | { | |
65 | ||
92f5a8d4 TL |
66 | namespace detail |
67 | { | |
b32b8144 FG |
68 | /*! |
69 | \brief Strategy functor for distance point to segment calculation on ellipsoid | |
70 | Algorithm uses direct and inverse geodesic problems as subroutines. | |
71 | The algorithm approximates the distance by an iterative Newton method. | |
72 | \ingroup strategies | |
73 | \details Class which calculates the distance of a point to a segment, for points | |
74 | on the ellipsoid | |
75 | \see C.F.F.Karney - Geodesics on an ellipsoid of revolution, | |
76 | https://arxiv.org/abs/1102.1215 | |
77 | \tparam FormulaPolicy underlying point-point distance strategy | |
78 | \tparam Spheroid is the spheroidal model used | |
79 | \tparam CalculationType \tparam_calculation | |
80 | \tparam EnableClosestPoint computes the closest point on segment if true | |
81 | */ | |
82 | template | |
83 | < | |
84 | typename FormulaPolicy = strategy::andoyer, | |
85 | typename Spheroid = srs::spheroid<double>, | |
86 | typename CalculationType = void, | |
92f5a8d4 | 87 | bool Bisection = false, |
b32b8144 FG |
88 | bool EnableClosestPoint = false |
89 | > | |
90 | class geographic_cross_track | |
91 | { | |
92 | public : | |
92f5a8d4 TL |
93 | typedef within::spherical_point_point equals_point_point_strategy_type; |
94 | ||
95 | typedef intersection::geographic_segments | |
96 | < | |
97 | FormulaPolicy, | |
98 | strategy::default_order<FormulaPolicy>::value, | |
99 | Spheroid, | |
100 | CalculationType | |
101 | > relate_segment_segment_strategy_type; | |
102 | ||
103 | inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy() const | |
104 | { | |
105 | return relate_segment_segment_strategy_type(m_spheroid); | |
106 | } | |
107 | ||
108 | typedef within::geographic_winding | |
109 | < | |
110 | void, void, FormulaPolicy, Spheroid, CalculationType | |
111 | > point_in_geometry_strategy_type; | |
112 | ||
113 | inline point_in_geometry_strategy_type get_point_in_geometry_strategy() const | |
114 | { | |
115 | return point_in_geometry_strategy_type(m_spheroid); | |
116 | } | |
117 | ||
b32b8144 FG |
118 | template <typename Point, typename PointOfSegment> |
119 | struct return_type | |
120 | : promote_floating_point | |
121 | < | |
122 | typename select_calculation_type | |
123 | < | |
124 | Point, | |
125 | PointOfSegment, | |
126 | CalculationType | |
127 | >::type | |
128 | > | |
129 | {}; | |
130 | ||
b32b8144 FG |
131 | explicit geographic_cross_track(Spheroid const& spheroid = Spheroid()) |
132 | : m_spheroid(spheroid) | |
133 | {} | |
134 | ||
135 | template <typename Point, typename PointOfSegment> | |
136 | inline typename return_type<Point, PointOfSegment>::type | |
137 | apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const | |
138 | { | |
92f5a8d4 | 139 | typedef typename geometry::detail::cs_angular_units<Point>::type units_type; |
b32b8144 | 140 | |
92f5a8d4 TL |
141 | return (apply<units_type>(get_as_radian<0>(sp1), get_as_radian<1>(sp1), |
142 | get_as_radian<0>(sp2), get_as_radian<1>(sp2), | |
143 | get_as_radian<0>(p), get_as_radian<1>(p), | |
b32b8144 FG |
144 | m_spheroid)).distance; |
145 | } | |
146 | ||
92f5a8d4 TL |
147 | // points on a meridian not crossing poles |
148 | template <typename CT> | |
149 | inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const | |
150 | { | |
151 | typedef typename formula::meridian_inverse | |
152 | < | |
153 | CT, | |
154 | strategy::default_order<FormulaPolicy>::value | |
155 | > meridian_inverse; | |
156 | ||
157 | return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2, | |
158 | m_spheroid); | |
159 | } | |
160 | ||
161 | Spheroid const& model() const | |
162 | { | |
163 | return m_spheroid; | |
164 | } | |
165 | ||
b32b8144 FG |
166 | private : |
167 | ||
168 | template <typename CT> | |
169 | struct result_distance_point_segment | |
170 | { | |
171 | result_distance_point_segment() | |
172 | : distance(0) | |
173 | , closest_point_lon(0) | |
174 | , closest_point_lat(0) | |
175 | {} | |
176 | ||
177 | CT distance; | |
178 | CT closest_point_lon; | |
179 | CT closest_point_lat; | |
180 | }; | |
181 | ||
182 | template <typename CT> | |
183 | result_distance_point_segment<CT> | |
92f5a8d4 | 184 | static inline non_iterative_case(CT const& lon, CT const& lat, CT const& distance) |
b32b8144 FG |
185 | { |
186 | result_distance_point_segment<CT> result; | |
187 | result.distance = distance; | |
188 | ||
189 | if (EnableClosestPoint) | |
190 | { | |
191 | result.closest_point_lon = lon; | |
192 | result.closest_point_lat = lat; | |
193 | } | |
194 | return result; | |
195 | } | |
196 | ||
197 | template <typename CT> | |
198 | result_distance_point_segment<CT> | |
92f5a8d4 TL |
199 | static inline non_iterative_case(CT const& lon1, CT const& lat1, //p1 |
200 | CT const& lon2, CT const& lat2, //p2 | |
b32b8144 FG |
201 | Spheroid const& spheroid) |
202 | { | |
203 | CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT> | |
204 | ::apply(lon1, lat1, lon2, lat2, spheroid); | |
205 | ||
206 | return non_iterative_case(lon1, lat1, distance); | |
207 | } | |
208 | ||
209 | template <typename CT> | |
92f5a8d4 | 210 | CT static inline normalize(CT const& g4, CT& der) |
b32b8144 FG |
211 | { |
212 | CT const pi = math::pi<CT>(); | |
11fdf7f2 | 213 | if (g4 < -1.25*pi)//close to -270 |
b32b8144 | 214 | { |
11fdf7f2 | 215 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
92f5a8d4 | 216 | std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -270" << std::endl; |
11fdf7f2 | 217 | #endif |
b32b8144 FG |
218 | return g4 + 1.5 * pi; |
219 | } | |
11fdf7f2 | 220 | else if (g4 > 1.25*pi)//close to 270 |
b32b8144 | 221 | { |
11fdf7f2 | 222 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
92f5a8d4 | 223 | std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to 270" << std::endl; |
11fdf7f2 | 224 | #endif |
92f5a8d4 | 225 | der = -der; |
b32b8144 FG |
226 | return - g4 + 1.5 * pi; |
227 | } | |
11fdf7f2 | 228 | else if (g4 < 0 && g4 > -0.75*pi)//close to -90 |
b32b8144 | 229 | { |
11fdf7f2 | 230 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
92f5a8d4 | 231 | std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -90" << std::endl; |
11fdf7f2 TL |
232 | #endif |
233 | der = -der; | |
b32b8144 FG |
234 | return -g4 - pi/2; |
235 | } | |
236 | return g4 - pi/2; | |
237 | } | |
238 | ||
92f5a8d4 TL |
239 | template <typename CT> |
240 | static void bisection(CT const& lon1, CT const& lat1, //p1 | |
241 | CT const& lon2, CT const& lat2, //p2 | |
242 | CT const& lon3, CT const& lat3, //query point p3 | |
243 | Spheroid const& spheroid, | |
244 | CT const& s14_start, CT const& a12, | |
245 | result_distance_point_segment<CT>& result) | |
246 | { | |
247 | typedef typename FormulaPolicy::template direct<CT, true, false, false, false> | |
248 | direct_distance_type; | |
249 | typedef typename FormulaPolicy::template inverse<CT, true, false, false, false, false> | |
250 | inverse_distance_type; | |
251 | ||
252 | geometry::formula::result_direct<CT> res14; | |
253 | ||
254 | int counter = 0; // robustness | |
255 | bool dist_improve = true; | |
256 | ||
257 | CT pl_lon = lon1; | |
258 | CT pl_lat = lat1; | |
259 | CT pr_lon = lon2; | |
260 | CT pr_lat = lat2; | |
261 | ||
262 | CT s14 = s14_start; | |
263 | ||
264 | do{ | |
265 | // Solve the direct problem to find p4 (GEO) | |
266 | res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid); | |
267 | ||
268 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
269 | std::cout << "dist(pl,p3)=" | |
270 | << inverse_distance_type::apply(lon3, lat3, | |
271 | pr_lon, pr_lat, | |
272 | spheroid).distance | |
273 | << std::endl; | |
274 | std::cout << "dist(pr,p3)=" | |
275 | << inverse_distance_type::apply(lon3, lat3, | |
276 | pr_lon, pr_lat, | |
277 | spheroid).distance | |
278 | << std::endl; | |
279 | #endif | |
280 | if (inverse_distance_type::apply(lon3, lat3, pl_lon, pl_lat, spheroid).distance | |
281 | < inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance) | |
282 | { | |
283 | s14 -= inverse_distance_type::apply(res14.lon2, res14.lat2, pl_lon, pl_lat, | |
284 | spheroid).distance/2; | |
285 | pr_lon = res14.lon2; | |
286 | pr_lat = res14.lat2; | |
287 | } | |
288 | else | |
289 | { | |
290 | s14 += inverse_distance_type::apply(res14.lon2, res14.lat2, pr_lon, pr_lat, | |
291 | spheroid).distance/2; | |
292 | pl_lon = res14.lon2; | |
293 | pl_lat = res14.lat2; | |
294 | } | |
295 | ||
296 | CT new_distance = inverse_distance_type::apply(lon3, lat3, | |
297 | res14.lon2, res14.lat2, | |
298 | spheroid).distance; | |
299 | ||
300 | dist_improve = new_distance != result.distance; | |
301 | ||
302 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
303 | std::cout << "p4=" << res14.lon2 * math::r2d<CT>() << | |
304 | "," << res14.lat2 * math::r2d<CT>() << std::endl; | |
305 | std::cout << "pl=" << pl_lon * math::r2d<CT>() << "," << pl_lat * math::r2d<CT>()<< std::endl; | |
306 | std::cout << "pr=" << pr_lon * math::r2d<CT>() << "," << pr_lat * math::r2d<CT>() << std::endl; | |
307 | std::cout << "new_s14=" << s14 << std::endl; | |
308 | std::cout << std::setprecision(16) << "result.distance =" << result.distance << std::endl; | |
309 | std::cout << std::setprecision(16) << "new_distance =" << new_distance << std::endl; | |
310 | std::cout << "---------end of step " << counter << std::endl<< std::endl; | |
311 | if (!dist_improve) | |
312 | { | |
313 | std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl; | |
314 | } | |
315 | if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS) | |
316 | { | |
317 | std::cout << "Stop msg: counter" << std::endl; | |
318 | } | |
319 | #endif | |
320 | ||
321 | result.distance = new_distance; | |
322 | ||
323 | } while (dist_improve | |
324 | && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS); | |
325 | } | |
326 | ||
327 | template <typename CT> | |
328 | static void newton(CT const& lon1, CT const& lat1, //p1 | |
329 | CT const& lon2, CT const& lat2, //p2 | |
330 | CT const& lon3, CT const& lat3, //query point p3 | |
331 | Spheroid const& spheroid, | |
332 | CT const& s14_start, CT const& a12, | |
333 | result_distance_point_segment<CT>& result) | |
b32b8144 | 334 | { |
11fdf7f2 TL |
335 | typedef typename FormulaPolicy::template inverse<CT, true, true, false, true, true> |
336 | inverse_distance_azimuth_quantities_type; | |
b32b8144 | 337 | typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false> |
92f5a8d4 | 338 | inverse_dist_azimuth_type; |
b32b8144 FG |
339 | typedef typename FormulaPolicy::template direct<CT, true, false, false, false> |
340 | direct_distance_type; | |
341 | ||
92f5a8d4 TL |
342 | CT const half_pi = math::pi<CT>() / CT(2); |
343 | CT prev_distance; | |
344 | geometry::formula::result_direct<CT> res14; | |
345 | geometry::formula::result_inverse<CT> res34; | |
346 | res34.distance = -1; | |
347 | ||
348 | int counter = 0; // robustness | |
349 | CT g4; | |
350 | CT delta_g4 = 0; | |
351 | bool dist_improve = true; | |
352 | CT s14 = s14_start; | |
353 | ||
354 | do{ | |
355 | prev_distance = res34.distance; | |
356 | ||
357 | // Solve the direct problem to find p4 (GEO) | |
358 | res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid); | |
359 | ||
360 | // Solve an inverse problem to find g4 | |
361 | // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO) | |
362 | ||
363 | CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2, | |
364 | lon2, lat2, spheroid).azimuth; | |
365 | res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2, | |
366 | lon3, lat3, spheroid); | |
367 | g4 = res34.azimuth - a4; | |
368 | ||
369 | CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit | |
370 | CT m34 = res34.reduced_length; | |
371 | ||
372 | if (m34 != 0) | |
373 | { | |
374 | CT der = (M43 / m34) * sin(g4); | |
375 | delta_g4 = normalize(g4, der); | |
376 | s14 -= der != 0 ? delta_g4 / der : 0; | |
377 | } | |
378 | ||
379 | result.distance = res34.distance; | |
380 | ||
381 | dist_improve = prev_distance > res34.distance || prev_distance == -1; | |
382 | if (!dist_improve) | |
383 | { | |
384 | result.distance = prev_distance; | |
385 | } | |
386 | ||
387 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
388 | std::cout << "p4=" << res14.lon2 * math::r2d<CT>() << | |
389 | "," << res14.lat2 * math::r2d<CT>() << std::endl; | |
390 | std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl; | |
391 | std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl; | |
392 | std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl; | |
393 | std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>() << std::endl; | |
394 | std::cout << "der=" << der << std::endl; | |
395 | std::cout << "M43=" << M43 << std::endl; | |
396 | std::cout << "m34=" << m34 << std::endl; | |
397 | std::cout << "new_s14=" << s14 << std::endl; | |
398 | std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl; | |
399 | std::cout << "---------end of step " << counter << std::endl<< std::endl; | |
400 | if (g4 == half_pi) | |
401 | { | |
402 | std::cout << "Stop msg: g4 == half_pi" << std::endl; | |
403 | } | |
404 | if (!dist_improve) | |
405 | { | |
406 | std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl; | |
407 | } | |
408 | if (delta_g4 == 0) | |
409 | { | |
410 | std::cout << "Stop msg: delta_g4 == 0" << std::endl; | |
411 | } | |
412 | if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS) | |
413 | { | |
414 | std::cout << "Stop msg: counter" << std::endl; | |
415 | } | |
416 | #endif | |
417 | ||
418 | } while (g4 != half_pi | |
419 | && dist_improve | |
420 | && delta_g4 != 0 | |
421 | && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS); | |
422 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
423 | std::cout << "distance=" << res34.distance << std::endl; | |
424 | ||
425 | std::cout << "s34(geo) =" | |
426 | << inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2, lon3, lat3, spheroid).distance | |
427 | << ", p4=(" << res14.lon2 * math::r2d<double>() << "," | |
428 | << res14.lat2 * math::r2d<double>() << ")" | |
429 | << std::endl; | |
430 | ||
431 | CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance; | |
432 | CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance; | |
433 | ||
434 | CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2, lon2, lat2, spheroid).azimuth; | |
435 | geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(res14.lon2, res14.lat2, .04, a4, spheroid); | |
436 | CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance; | |
437 | ||
438 | geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid); | |
439 | CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance; | |
440 | ||
441 | std::cout << "s31=" << s31 << "\ns32=" << s32 | |
442 | << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")" | |
443 | << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")" | |
444 | << std::endl; | |
445 | ||
446 | if (res34.distance <= p4_plus && res34.distance <= p4_minus) | |
447 | { | |
448 | std::cout << "Closest point computed" << std::endl; | |
449 | } | |
450 | else | |
451 | { | |
452 | std::cout << "There is a closer point nearby" << std::endl; | |
453 | } | |
454 | #endif | |
455 | } | |
456 | ||
457 | template <typename Units, typename CT> | |
458 | result_distance_point_segment<CT> | |
459 | static inline apply(CT const& lo1, CT const& la1, //p1 | |
460 | CT const& lo2, CT const& la2, //p2 | |
461 | CT const& lo3, CT const& la3, //query point p3 | |
462 | Spheroid const& spheroid) | |
463 | { | |
464 | typedef typename FormulaPolicy::template inverse<CT, true, true, false, false, false> | |
465 | inverse_dist_azimuth_type; | |
466 | typedef typename FormulaPolicy::template inverse<CT, true, true, true, false, false> | |
467 | inverse_dist_azimuth_reverse_type; | |
468 | ||
b32b8144 FG |
469 | CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid); |
470 | ||
471 | result_distance_point_segment<CT> result; | |
472 | ||
473 | // Constants | |
11fdf7f2 | 474 | //CT const f = geometry::formula::flattening<CT>(spheroid); |
b32b8144 FG |
475 | CT const pi = math::pi<CT>(); |
476 | CT const half_pi = pi / CT(2); | |
477 | CT const c0 = CT(0); | |
478 | ||
92f5a8d4 TL |
479 | CT lon1 = lo1; |
480 | CT lat1 = la1; | |
481 | CT lon2 = lo2; | |
482 | CT lat2 = la2; | |
483 | CT lon3 = lo3; | |
484 | CT lat3 = la3; | |
b32b8144 FG |
485 | |
486 | if (lon1 > lon2) | |
487 | { | |
488 | std::swap(lon1, lon2); | |
489 | std::swap(lat1, lat2); | |
490 | } | |
491 | ||
92f5a8d4 TL |
492 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
493 | std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>(); | |
494 | std::cout << "," << lat1 * math::r2d<CT>(); | |
495 | std::cout << "),(" << lon2 * math::r2d<CT>(); | |
496 | std::cout << "," << lat2 * math::r2d<CT>(); | |
497 | std::cout << ")\np=(" << lon3 * math::r2d<CT>(); | |
498 | std::cout << "," << lat3 * math::r2d<CT>(); | |
499 | std::cout << ")" << std::endl; | |
500 | #endif | |
501 | ||
b32b8144 FG |
502 | //segment on equator |
503 | //Note: antipodal points on equator does not define segment on equator | |
504 | //but pass by the pole | |
505 | CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2); | |
11fdf7f2 | 506 | |
92f5a8d4 TL |
507 | typedef typename formula::meridian_inverse<CT> |
508 | meridian_inverse; | |
11fdf7f2 TL |
509 | |
510 | bool meridian_not_crossing_pole = | |
92f5a8d4 TL |
511 | meridian_inverse::meridian_not_crossing_pole |
512 | (lat1, lat2, diff); | |
11fdf7f2 TL |
513 | |
514 | bool meridian_crossing_pole = | |
92f5a8d4 | 515 | meridian_inverse::meridian_crossing_pole(diff); |
11fdf7f2 TL |
516 | |
517 | if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole) | |
b32b8144 FG |
518 | { |
519 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
520 | std::cout << "Equatorial segment" << std::endl; | |
11fdf7f2 TL |
521 | std::cout << "segment=(" << lon1 * math::r2d<CT>(); |
522 | std::cout << "," << lat1 * math::r2d<CT>(); | |
523 | std::cout << "),(" << lon2 * math::r2d<CT>(); | |
524 | std::cout << "," << lat2 * math::r2d<CT>(); | |
525 | std::cout << ")\np=(" << lon3 * math::r2d<CT>(); | |
526 | std::cout << "," << lat3 * math::r2d<CT>() << ")\n"; | |
b32b8144 FG |
527 | #endif |
528 | if (lon3 <= lon1) | |
529 | { | |
530 | return non_iterative_case(lon1, lat1, lon3, lat3, spheroid); | |
531 | } | |
532 | if (lon3 >= lon2) | |
533 | { | |
534 | return non_iterative_case(lon2, lat2, lon3, lat3, spheroid); | |
535 | } | |
536 | return non_iterative_case(lon3, lat1, lon3, lat3, spheroid); | |
537 | } | |
538 | ||
92f5a8d4 | 539 | if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && std::abs(lat1) > std::abs(lat2)) |
11fdf7f2 | 540 | { |
92f5a8d4 TL |
541 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
542 | std::cout << "Meridian segment not crossing pole" << std::endl; | |
543 | #endif | |
11fdf7f2 TL |
544 | std::swap(lat1,lat2); |
545 | } | |
546 | ||
547 | if (meridian_crossing_pole) | |
b32b8144 FG |
548 | { |
549 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
92f5a8d4 | 550 | std::cout << "Meridian segment crossing pole" << std::endl; |
b32b8144 | 551 | #endif |
92f5a8d4 TL |
552 | CT sign_non_zero = lat3 >= c0 ? 1 : -1; |
553 | result_distance_point_segment<CT> res13 = | |
554 | apply<geometry::radian>(lon1, lat1, | |
555 | lon1, half_pi * sign_non_zero, | |
556 | lon3, lat3, spheroid); | |
557 | result_distance_point_segment<CT> res23 = | |
558 | apply<geometry::radian>(lon2, lat2, | |
559 | lon2, half_pi * sign_non_zero, | |
560 | lon3, lat3, spheroid); | |
561 | if (res13.distance < res23.distance) | |
b32b8144 | 562 | { |
92f5a8d4 | 563 | return res13; |
b32b8144 FG |
564 | } |
565 | else | |
566 | { | |
92f5a8d4 | 567 | return res23; |
b32b8144 FG |
568 | } |
569 | } | |
570 | ||
92f5a8d4 TL |
571 | geometry::formula::result_inverse<CT> res12 = |
572 | inverse_dist_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid); | |
573 | geometry::formula::result_inverse<CT> res13 = | |
574 | inverse_dist_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid); | |
b32b8144 | 575 | |
92f5a8d4 | 576 | if (geometry::math::equals(res12.distance, c0)) |
b32b8144 FG |
577 | { |
578 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
579 | std::cout << "Degenerate segment" << std::endl; | |
92f5a8d4 | 580 | std::cout << "distance between points=" << res13.distance << std::endl; |
b32b8144 | 581 | #endif |
92f5a8d4 TL |
582 | typename meridian_inverse::result res = |
583 | meridian_inverse::apply(lon1, lat1, lon3, lat3, spheroid); | |
b32b8144 | 584 | |
92f5a8d4 TL |
585 | return non_iterative_case(lon1, lat2, |
586 | res.meridian ? res.distance : res13.distance); | |
587 | } | |
b32b8144 FG |
588 | |
589 | // Compute a12 (GEO) | |
92f5a8d4 | 590 | CT a312 = res13.azimuth - res12.azimuth; |
b32b8144 | 591 | |
92f5a8d4 TL |
592 | // TODO: meridian case optimization |
593 | if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole) | |
b32b8144 | 594 | { |
92f5a8d4 TL |
595 | boost::tuple<CT,CT> minmax_elem = boost::minmax(lat1, lat2); |
596 | if (lat3 >= minmax_elem.template get<0>() && | |
597 | lat3 <= minmax_elem.template get<1>()) | |
598 | { | |
b32b8144 | 599 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK |
92f5a8d4 | 600 | std::cout << "Point on meridian segment" << std::endl; |
b32b8144 | 601 | #endif |
92f5a8d4 TL |
602 | return non_iterative_case(lon3, lat3, c0); |
603 | } | |
b32b8144 FG |
604 | } |
605 | ||
92f5a8d4 | 606 | CT projection1 = cos( a312 ) * res13.distance / res12.distance; |
b32b8144 FG |
607 | |
608 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
92f5a8d4 TL |
609 | std::cout << "a1=" << res12.azimuth * math::r2d<CT>() << std::endl; |
610 | std::cout << "a13=" << res13.azimuth * math::r2d<CT>() << std::endl; | |
b32b8144 FG |
611 | std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl; |
612 | std::cout << "cos(a312)=" << cos(a312) << std::endl; | |
92f5a8d4 | 613 | std::cout << "projection 1=" << projection1 << std::endl; |
b32b8144 | 614 | #endif |
92f5a8d4 TL |
615 | |
616 | if (projection1 < c0) | |
b32b8144 FG |
617 | { |
618 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
619 | std::cout << "projection closer to p1" << std::endl; | |
620 | #endif | |
621 | // projection of p3 on geodesic spanned by segment (p1,p2) fall | |
622 | // outside of segment on the side of p1 | |
92f5a8d4 | 623 | |
b32b8144 FG |
624 | return non_iterative_case(lon1, lat1, lon3, lat3, spheroid); |
625 | } | |
626 | ||
92f5a8d4 TL |
627 | geometry::formula::result_inverse<CT> res23 = |
628 | inverse_dist_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid); | |
b32b8144 | 629 | |
92f5a8d4 TL |
630 | CT a321 = res23.azimuth - res12.reverse_azimuth + pi; |
631 | CT projection2 = cos( a321 ) * res23.distance / res12.distance; | |
b32b8144 FG |
632 | |
633 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
92f5a8d4 TL |
634 | std::cout << "a21=" << res12.reverse_azimuth * math::r2d<CT>() << std::endl; |
635 | std::cout << "a23=" << res23.azimuth * math::r2d<CT>() << std::endl; | |
b32b8144 FG |
636 | std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl; |
637 | std::cout << "cos(a321)=" << cos(a321) << std::endl; | |
92f5a8d4 | 638 | std::cout << "projection 2=" << projection2 << std::endl; |
b32b8144 | 639 | #endif |
b32b8144 | 640 | |
92f5a8d4 | 641 | if (projection2 < c0) |
b32b8144 FG |
642 | { |
643 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
644 | std::cout << "projection closer to p2" << std::endl; | |
645 | #endif | |
646 | // projection of p3 on geodesic spanned by segment (p1,p2) fall | |
647 | // outside of segment on the side of p2 | |
648 | return non_iterative_case(lon2, lat2, lon3, lat3, spheroid); | |
649 | } | |
650 | ||
92f5a8d4 | 651 | // Guess s14 (SPHERICAL) aka along-track distance |
b32b8144 FG |
652 | typedef geometry::model::point |
653 | < | |
654 | CT, 2, | |
655 | geometry::cs::spherical_equatorial<geometry::radian> | |
656 | > point; | |
657 | ||
11fdf7f2 TL |
658 | point p1 = point(lon1, lat1); |
659 | point p2 = point(lon2, lat2); | |
660 | point p3 = point(lon3, lat3); | |
b32b8144 FG |
661 | |
662 | geometry::strategy::distance::cross_track<CT> cross_track(earth_radius); | |
92f5a8d4 | 663 | CT s34_sph = cross_track.apply(p3, p1, p2); |
b32b8144 FG |
664 | |
665 | geometry::strategy::distance::haversine<CT> str(earth_radius); | |
92f5a8d4 | 666 | CT s13_sph = str.apply(p1, p3); |
b32b8144 | 667 | |
92f5a8d4 TL |
668 | //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius; |
669 | CT cos_frac = cos(s13_sph / earth_radius) / cos(s34_sph / earth_radius); | |
670 | CT s14_sph = cos_frac >= 1 ? CT(0) | |
671 | : cos_frac <= -1 ? pi * earth_radius | |
672 | : acos(cos_frac) * earth_radius; | |
11fdf7f2 | 673 | |
92f5a8d4 | 674 | CT a12_sph = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2); |
b32b8144 | 675 | |
92f5a8d4 TL |
676 | geometry::formula::result_direct<CT> res |
677 | = geometry::formula::spherical_direct<true, false> | |
678 | (lon1, lat1, s14_sph, a12_sph, srs::sphere<CT>(earth_radius)); | |
11fdf7f2 | 679 | |
92f5a8d4 TL |
680 | // this is what postgis (version 2.5) returns |
681 | // geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT> | |
682 | // ::apply(lon3, lat3, res.lon2, res.lat2, spheroid); | |
b32b8144 FG |
683 | |
684 | #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK | |
92f5a8d4 TL |
685 | std::cout << "s34=" << s34_sph << std::endl; |
686 | std::cout << "s13=" << res13.distance << std::endl; | |
687 | std::cout << "s14=" << s14_sph << std::endl; | |
688 | std::cout << "===============" << std::endl; | |
b32b8144 FG |
689 | #endif |
690 | ||
92f5a8d4 | 691 | // Update s14 (using Newton method) |
b32b8144 | 692 | |
92f5a8d4 | 693 | if (Bisection) |
b32b8144 | 694 | { |
92f5a8d4 TL |
695 | bisection<CT>(lon1, lat1, lon2, lat2, lon3, lat3, |
696 | spheroid, res12.distance/2, res12.azimuth, result); | |
b32b8144 FG |
697 | } |
698 | else | |
699 | { | |
92f5a8d4 TL |
700 | CT s14_start = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT> |
701 | ::apply(lon1, lat1, res.lon2, res.lat2, spheroid); | |
702 | ||
703 | newton<CT>(lon1, lat1, lon2, lat2, lon3, lat3, | |
704 | spheroid, s14_start, res12.azimuth, result); | |
b32b8144 | 705 | } |
b32b8144 FG |
706 | |
707 | return result; | |
708 | } | |
709 | ||
710 | Spheroid m_spheroid; | |
711 | }; | |
712 | ||
92f5a8d4 | 713 | } // namespace detail |
b32b8144 | 714 | |
92f5a8d4 TL |
715 | template |
716 | < | |
717 | typename FormulaPolicy = strategy::andoyer, | |
718 | typename Spheroid = srs::spheroid<double>, | |
719 | typename CalculationType = void | |
720 | > | |
721 | class geographic_cross_track | |
722 | : public detail::geographic_cross_track | |
723 | < | |
724 | FormulaPolicy, | |
725 | Spheroid, | |
726 | CalculationType, | |
727 | false, | |
728 | false | |
729 | > | |
730 | { | |
731 | public : | |
732 | explicit geographic_cross_track(Spheroid const& spheroid = Spheroid()) | |
733 | : | |
734 | detail::geographic_cross_track< | |
735 | FormulaPolicy, | |
736 | Spheroid, | |
737 | CalculationType, | |
738 | false, | |
739 | false | |
740 | >(spheroid) | |
741 | {} | |
742 | ||
743 | }; | |
b32b8144 FG |
744 | |
745 | #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS | |
746 | namespace services | |
747 | { | |
748 | ||
749 | //tags | |
750 | template <typename FormulaPolicy> | |
751 | struct tag<geographic_cross_track<FormulaPolicy> > | |
752 | { | |
753 | typedef strategy_tag_distance_point_segment type; | |
754 | }; | |
755 | ||
756 | template | |
757 | < | |
758 | typename FormulaPolicy, | |
759 | typename Spheroid | |
760 | > | |
761 | struct tag<geographic_cross_track<FormulaPolicy, Spheroid> > | |
762 | { | |
763 | typedef strategy_tag_distance_point_segment type; | |
764 | }; | |
765 | ||
766 | template | |
767 | < | |
768 | typename FormulaPolicy, | |
769 | typename Spheroid, | |
770 | typename CalculationType | |
771 | > | |
772 | struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> > | |
773 | { | |
774 | typedef strategy_tag_distance_point_segment type; | |
775 | }; | |
776 | ||
92f5a8d4 TL |
777 | template |
778 | < | |
779 | typename FormulaPolicy, | |
780 | typename Spheroid, | |
781 | typename CalculationType, | |
782 | bool Bisection | |
783 | > | |
784 | struct tag<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> > | |
785 | { | |
786 | typedef strategy_tag_distance_point_segment type; | |
787 | }; | |
b32b8144 FG |
788 | |
789 | //return types | |
790 | template <typename FormulaPolicy, typename P, typename PS> | |
791 | struct return_type<geographic_cross_track<FormulaPolicy>, P, PS> | |
792 | : geographic_cross_track<FormulaPolicy>::template return_type<P, PS> | |
793 | {}; | |
794 | ||
795 | template | |
796 | < | |
797 | typename FormulaPolicy, | |
798 | typename Spheroid, | |
799 | typename P, | |
800 | typename PS | |
801 | > | |
802 | struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS> | |
803 | : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS> | |
804 | {}; | |
805 | ||
806 | template | |
807 | < | |
808 | typename FormulaPolicy, | |
809 | typename Spheroid, | |
810 | typename CalculationType, | |
811 | typename P, | |
812 | typename PS | |
813 | > | |
814 | struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS> | |
815 | : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS> | |
816 | {}; | |
817 | ||
92f5a8d4 TL |
818 | template |
819 | < | |
820 | typename FormulaPolicy, | |
821 | typename Spheroid, | |
822 | typename CalculationType, | |
823 | bool Bisection, | |
824 | typename P, | |
825 | typename PS | |
826 | > | |
827 | struct return_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection>, P, PS> | |
828 | : detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection>::template return_type<P, PS> | |
829 | {}; | |
830 | ||
b32b8144 FG |
831 | //comparable types |
832 | template | |
833 | < | |
834 | typename FormulaPolicy, | |
835 | typename Spheroid, | |
836 | typename CalculationType | |
837 | > | |
838 | struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> > | |
839 | { | |
840 | typedef geographic_cross_track | |
841 | < | |
842 | FormulaPolicy, Spheroid, CalculationType | |
843 | > type; | |
844 | }; | |
845 | ||
92f5a8d4 TL |
846 | |
847 | template | |
848 | < | |
849 | typename FormulaPolicy, | |
850 | typename Spheroid, | |
851 | typename CalculationType, | |
852 | bool Bisection | |
853 | > | |
854 | struct comparable_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> > | |
855 | { | |
856 | typedef detail::geographic_cross_track | |
857 | < | |
858 | FormulaPolicy, Spheroid, CalculationType, Bisection | |
859 | > type; | |
860 | }; | |
861 | ||
b32b8144 FG |
862 | template |
863 | < | |
864 | typename FormulaPolicy, | |
865 | typename Spheroid, | |
866 | typename CalculationType | |
867 | > | |
868 | struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> > | |
869 | { | |
b32b8144 | 870 | public : |
92f5a8d4 TL |
871 | static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> |
872 | apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy) | |
873 | { | |
874 | return strategy; | |
875 | } | |
876 | }; | |
877 | ||
878 | template | |
879 | < | |
880 | typename FormulaPolicy, | |
881 | typename Spheroid, | |
882 | typename CalculationType, | |
883 | bool Bisection | |
884 | > | |
885 | struct get_comparable<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> > | |
886 | { | |
887 | public : | |
888 | static inline detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> | |
889 | apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> const& strategy) | |
b32b8144 | 890 | { |
92f5a8d4 | 891 | return strategy; |
b32b8144 FG |
892 | } |
893 | }; | |
894 | ||
895 | ||
896 | template | |
897 | < | |
898 | typename FormulaPolicy, | |
899 | typename P, | |
900 | typename PS | |
901 | > | |
902 | struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS> | |
903 | { | |
904 | private : | |
905 | typedef typename geographic_cross_track | |
906 | < | |
907 | FormulaPolicy | |
908 | >::template return_type<P, PS>::type return_type; | |
909 | public : | |
910 | template <typename T> | |
911 | static inline return_type | |
912 | apply(geographic_cross_track<FormulaPolicy> const& , T const& distance) | |
913 | { | |
914 | return distance; | |
915 | } | |
916 | }; | |
917 | ||
11fdf7f2 TL |
918 | template |
919 | < | |
920 | typename FormulaPolicy, | |
921 | typename Spheroid, | |
922 | typename CalculationType, | |
923 | typename P, | |
924 | typename PS | |
925 | > | |
926 | struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS> | |
927 | { | |
928 | private : | |
929 | typedef typename geographic_cross_track | |
930 | < | |
931 | FormulaPolicy, Spheroid, CalculationType | |
932 | >::template return_type<P, PS>::type return_type; | |
933 | public : | |
934 | template <typename T> | |
935 | static inline return_type | |
936 | apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance) | |
937 | { | |
938 | return distance; | |
939 | } | |
940 | }; | |
941 | ||
b32b8144 FG |
942 | |
943 | template <typename Point, typename PointOfSegment> | |
944 | struct default_strategy | |
945 | < | |
946 | point_tag, segment_tag, Point, PointOfSegment, | |
947 | geographic_tag, geographic_tag | |
948 | > | |
949 | { | |
950 | typedef geographic_cross_track<> type; | |
951 | }; | |
952 | ||
953 | ||
954 | template <typename PointOfSegment, typename Point> | |
955 | struct default_strategy | |
956 | < | |
957 | segment_tag, point_tag, PointOfSegment, Point, | |
958 | geographic_tag, geographic_tag | |
959 | > | |
960 | { | |
961 | typedef typename default_strategy | |
962 | < | |
963 | point_tag, segment_tag, Point, PointOfSegment, | |
964 | geographic_tag, geographic_tag | |
965 | >::type type; | |
966 | }; | |
967 | ||
968 | } // namespace services | |
969 | #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS | |
970 | ||
971 | }} // namespace strategy::distance | |
972 | ||
973 | }} // namespace boost::geometry | |
974 | #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP |