1 // Boost.Geometry (aka GGL, Generic Geometry Library)
3 // Copyright (c) 2016-2017, Oracle and/or its affiliates.
5 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
7 // Use, modification and distribution is subject to the Boost Software License,
8 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
9 // http://www.boost.org/LICENSE_1_0.txt)
11 #ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
12 #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
16 #include <boost/config.hpp>
17 #include <boost/concept_check.hpp>
18 #include <boost/mpl/if.hpp>
19 #include <boost/type_traits/is_void.hpp>
21 #include <boost/geometry/core/cs.hpp>
22 #include <boost/geometry/core/access.hpp>
23 #include <boost/geometry/core/radian_access.hpp>
24 #include <boost/geometry/core/tags.hpp>
26 #include <boost/geometry/strategies/distance.hpp>
27 #include <boost/geometry/strategies/concepts/distance_concept.hpp>
28 #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
29 #include <boost/geometry/strategies/geographic/azimuth.hpp>
30 #include <boost/geometry/strategies/geographic/parameters.hpp>
32 #include <boost/geometry/formulas/vincenty_direct.hpp>
34 #include <boost/geometry/util/math.hpp>
35 #include <boost/geometry/util/promote_floating_point.hpp>
36 #include <boost/geometry/util/select_calculation_type.hpp>
37 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
39 #include <boost/geometry/formulas/result_direct.hpp>
40 #include <boost/geometry/formulas/mean_radius.hpp>
42 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
43 #include <boost/geometry/io/dsv/write.hpp>
46 #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
47 #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
50 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
54 namespace boost { namespace geometry
57 namespace strategy { namespace distance
61 \brief Strategy functor for distance point to segment calculation on ellipsoid
62 Algorithm uses direct and inverse geodesic problems as subroutines.
63 The algorithm approximates the distance by an iterative Newton method.
65 \details Class which calculates the distance of a point to a segment, for points
67 \see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
68 https://arxiv.org/abs/1102.1215
69 \tparam FormulaPolicy underlying point-point distance strategy
70 \tparam Spheroid is the spheroidal model used
71 \tparam CalculationType \tparam_calculation
72 \tparam EnableClosestPoint computes the closest point on segment if true
76 typename FormulaPolicy = strategy::andoyer,
77 typename Spheroid = srs::spheroid<double>,
78 typename CalculationType = void,
79 bool EnableClosestPoint = false
81 class geographic_cross_track
84 template <typename Point, typename PointOfSegment>
86 : promote_floating_point
88 typename select_calculation_type
97 struct distance_strategy
99 typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
102 inline typename distance_strategy::type get_distance_strategy() const
104 typedef typename distance_strategy::type distance_type;
105 return distance_type(m_spheroid);
108 explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
109 : m_spheroid(spheroid)
112 template <typename Point, typename PointOfSegment>
113 inline typename return_type<Point, PointOfSegment>::type
114 apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
116 typedef typename coordinate_system<Point>::type::units units_type;
118 return (apply<units_type>(get<0>(sp1), get<1>(sp1),
119 get<0>(sp2), get<1>(sp2),
120 get<0>(p), get<1>(p),
121 m_spheroid)).distance;
126 template <typename CT>
127 struct result_distance_point_segment
129 result_distance_point_segment()
131 , closest_point_lon(0)
132 , closest_point_lat(0)
136 CT closest_point_lon;
137 CT closest_point_lat;
140 template <typename CT>
141 result_distance_point_segment<CT>
142 static inline non_iterative_case(CT lon, CT lat, CT distance)
144 result_distance_point_segment<CT> result;
145 result.distance = distance;
147 if (EnableClosestPoint)
149 result.closest_point_lon = lon;
150 result.closest_point_lat = lat;
155 template <typename CT>
156 result_distance_point_segment<CT>
157 static inline non_iterative_case(CT lon1, CT lat1, //p1
158 CT lon2, CT lat2, //p2
159 Spheroid const& spheroid)
161 CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
162 ::apply(lon1, lat1, lon2, lat2, spheroid);
164 return non_iterative_case(lon1, lat1, distance);
167 template <typename CT>
168 CT static inline normalize(CT g4, CT& der)
170 CT const pi = math::pi<CT>();
171 if (g4 < -1.25*pi)//close to -270
173 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
174 std::cout << "g4=" << g4 << ", close to -270" << std::endl;
176 return g4 + 1.5 * pi;
178 else if (g4 > 1.25*pi)//close to 270
180 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
181 std::cout << "g4=" << g4 << ", close to 270" << std::endl;
183 return - g4 + 1.5 * pi;
185 else if (g4 < 0 && g4 > -0.75*pi)//close to -90
187 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
188 std::cout << "g4=" << g4 << ", close to -90" << std::endl;
196 template <typename Units, typename CT>
197 result_distance_point_segment<CT>
198 static inline apply(CT lon1, CT lat1, //p1
199 CT lon2, CT lat2, //p2
200 CT lon3, CT lat3, //query point p3
201 Spheroid const& spheroid)
203 typedef typename FormulaPolicy::template inverse<CT, true, true, false, true, true>
204 inverse_distance_azimuth_quantities_type;
205 typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
206 inverse_azimuth_type;
207 typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
208 inverse_azimuth_reverse_type;
209 typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
210 direct_distance_type;
212 CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
214 result_distance_point_segment<CT> result;
217 //CT const f = geometry::formula::flattening<CT>(spheroid);
218 CT const pi = math::pi<CT>();
219 CT const half_pi = pi / CT(2);
222 // Convert to radians
223 lon1 = math::as_radian<Units>(lon1);
224 lat1 = math::as_radian<Units>(lat1);
225 lon2 = math::as_radian<Units>(lon2);
226 lat2 = math::as_radian<Units>(lat2);
227 lon3 = math::as_radian<Units>(lon3);
228 lat3 = math::as_radian<Units>(lat3);
232 std::swap(lon1, lon2);
233 std::swap(lat1, lat2);
237 //Note: antipodal points on equator does not define segment on equator
238 //but pass by the pole
239 CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
241 typedef typename formula::elliptic_arc_length<CT> elliptic_arc_length;
243 bool meridian_not_crossing_pole =
244 elliptic_arc_length::meridian_not_crossing_pole(lat1, lat2, diff);
246 bool meridian_crossing_pole =
247 elliptic_arc_length::meridian_crossing_pole(diff);
249 //bool meridian_crossing_pole = math::equals(math::abs(diff), pi);
250 //bool meridian_not_crossing_pole = math::equals(math::abs(diff), c0);
252 if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole)
254 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
255 std::cout << "Equatorial segment" << std::endl;
256 std::cout << "segment=(" << lon1 * math::r2d<CT>();
257 std::cout << "," << lat1 * math::r2d<CT>();
258 std::cout << "),(" << lon2 * math::r2d<CT>();
259 std::cout << "," << lat2 * math::r2d<CT>();
260 std::cout << ")\np=(" << lon3 * math::r2d<CT>();
261 std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
265 return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
269 return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
271 return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
274 if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && lat1 > lat2)
276 std::swap(lat1,lat2);
279 if (meridian_crossing_pole)
281 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
282 std::cout << "Meridian segment" << std::endl;
284 result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid);
285 result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid);
286 if (d1.distance < d2.distance)
296 CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
297 ::apply(lon1, lat1, lon3, lat3, spheroid);
299 CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
300 ::apply(lon1, lat1, lon2, lat2, spheroid);
302 if (geometry::math::equals(d3, c0))
304 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
305 std::cout << "Degenerate segment" << std::endl;
306 std::cout << "distance between points=" << d1 << std::endl;
308 return non_iterative_case(lon1, lat2, d1);
311 CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
312 ::apply(lon2, lat2, lon3, lat3, spheroid);
315 geometry::formula::result_inverse<CT> res12 =
316 inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
317 CT a12 = res12.azimuth;
318 CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;
322 if (geometry::math::equals(a312, c0))
324 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
325 std::cout << "point on segment" << std::endl;
327 return non_iterative_case(lon3, lat3, c0);
330 CT projection1 = cos( a312 ) * d1 / d3;
332 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
333 std::cout << "segment=(" << lon1 * math::r2d<CT>();
334 std::cout << "," << lat1 * math::r2d<CT>();
335 std::cout << "),(" << lon2 * math::r2d<CT>();
336 std::cout << "," << lat2 * math::r2d<CT>();
337 std::cout << ")\np=(" << lon3 * math::r2d<CT>();
338 std::cout << "," << lat3 * math::r2d<CT>();
339 std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl;
340 std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
341 std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
342 std::cout << "cos(a312)=" << cos(a312) << std::endl;
344 if (projection1 < 0.0)
346 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
347 std::cout << "projection closer to p1" << std::endl;
349 // projection of p3 on geodesic spanned by segment (p1,p2) fall
350 // outside of segment on the side of p1
351 return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
354 CT a21 = res12.reverse_azimuth - pi;
355 CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;
359 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
360 std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
361 std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
362 std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
363 std::cout << "cos(a321)=" << cos(a321) << std::endl;
365 CT projection2 = cos( a321 ) * d2 / d3;
367 if (projection2 < 0.0)
369 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
370 std::cout << "projection closer to p2" << std::endl;
372 // projection of p3 on geodesic spanned by segment (p1,p2) fall
373 // outside of segment on the side of p2
374 return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
377 // Guess s14 (SPHERICAL)
378 typedef geometry::model::point
381 geometry::cs::spherical_equatorial<geometry::radian>
384 point p1 = point(lon1, lat1);
385 point p2 = point(lon2, lat2);
386 point p3 = point(lon3, lat3);
388 geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
389 CT s34 = cross_track.apply(p3, p1, p2);
391 geometry::strategy::distance::haversine<CT> str(earth_radius);
392 CT s13 = str.apply(p1, p3);
393 CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
395 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
396 std::cout << "s34=" << s34 << std::endl;
397 std::cout << "s13=" << s13 << std::endl;
398 std::cout << "s14=" << s14 << std::endl;
399 std::cout << "===============" << std::endl;
402 // Update s14 (using Newton method)
403 CT prev_distance = 0;
404 geometry::formula::result_direct<CT> res14;
405 geometry::formula::result_inverse<CT> res34;
407 int counter = 0; // robustness
412 prev_distance = res34.distance;
414 // Solve the direct problem to find p4 (GEO)
415 res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
417 // Solve an inverse problem to find g4
418 // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)
420 CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
421 lon2, lat2, spheroid).azimuth;
422 res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2,
423 lon3, lat3, spheroid);
424 g4 = res34.azimuth - a4;
428 CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
429 CT m34 = res34.reduced_length;
430 CT der = (M43 / m34) * sin(g4);
432 // normalize (g4 - pi/2)
433 delta_g4 = normalize(g4, der);
435 s14 = s14 - delta_g4 / der;
437 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
438 std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
439 "," << res14.lat2 * math::r2d<CT>() << std::endl;
440 std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
441 std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
442 std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl;
443 std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>() << std::endl;
444 std::cout << "der=" << der << std::endl;
445 std::cout << "M43=" << M43 << std::endl;
446 std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
447 std::cout << "m34=" << m34 << std::endl;
448 std::cout << "new_s14=" << s14 << std::endl;
449 std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl;
450 std::cout << "---------end of step " << counter << std::endl<< std::endl;
452 result.distance = prev_distance;
454 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
457 std::cout << "Stop msg: g4 == half_pi" << std::endl;
459 if (res34.distance >= prev_distance && prev_distance != 0)
461 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
465 std::cout << "Stop msg: delta_g4 == 0" << std::endl;
469 std::cout << "Stop msg: counter" << std::endl;
473 } while (g4 != half_pi
474 && (prev_distance > res34.distance || prev_distance == 0)
476 && ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ;
478 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
479 std::cout << "distance=" << res34.distance << std::endl;
481 point p4(res14.lon2, res14.lat2);
482 CT s34_sph = str.apply(p4, p3);
484 std::cout << "s34(sph) =" << s34_sph << std::endl;
485 std::cout << "s34(geo) ="
486 << inverse_distance_azimuth_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
487 << ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
488 << get<1>(p4) * math::r2d<double>() << ")"
491 CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
492 CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;
494 CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
495 geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
496 CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;
498 geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
499 CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;
501 std::cout << "s31=" << s31 << "\ns32=" << s32
502 << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
503 << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
506 if (res34.distance <= p4_plus && res34.distance <= p4_minus)
508 std::cout << "Closest point computed" << std::endl;
512 std::cout << "There is a closer point nearby" << std::endl;
524 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
529 template <typename FormulaPolicy>
530 struct tag<geographic_cross_track<FormulaPolicy> >
532 typedef strategy_tag_distance_point_segment type;
537 typename FormulaPolicy,
540 struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
542 typedef strategy_tag_distance_point_segment type;
547 typename FormulaPolicy,
549 typename CalculationType
551 struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
553 typedef strategy_tag_distance_point_segment type;
558 template <typename FormulaPolicy, typename P, typename PS>
559 struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
560 : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
565 typename FormulaPolicy,
570 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
571 : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
576 typename FormulaPolicy,
578 typename CalculationType,
582 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
583 : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
589 typename FormulaPolicy,
591 typename CalculationType
593 struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
595 typedef geographic_cross_track
597 FormulaPolicy, Spheroid, CalculationType
603 typename FormulaPolicy,
605 typename CalculationType
607 struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
609 typedef typename comparable_type
611 geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
612 >::type comparable_type;
614 static inline comparable_type
615 apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& )
617 return comparable_type();
624 typename FormulaPolicy,
628 struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
631 typedef typename geographic_cross_track
634 >::template return_type<P, PS>::type return_type;
636 template <typename T>
637 static inline return_type
638 apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
646 typename FormulaPolicy,
648 typename CalculationType,
652 struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
655 typedef typename geographic_cross_track
657 FormulaPolicy, Spheroid, CalculationType
658 >::template return_type<P, PS>::type return_type;
660 template <typename T>
661 static inline return_type
662 apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
669 template <typename Point, typename PointOfSegment>
670 struct default_strategy
672 point_tag, segment_tag, Point, PointOfSegment,
673 geographic_tag, geographic_tag
676 typedef geographic_cross_track<> type;
680 template <typename PointOfSegment, typename Point>
681 struct default_strategy
683 segment_tag, point_tag, PointOfSegment, Point,
684 geographic_tag, geographic_tag
687 typedef typename default_strategy
689 point_tag, segment_tag, Point, PointOfSegment,
690 geographic_tag, geographic_tag
694 } // namespace services
695 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
697 }} // namespace strategy::distance
699 }} // namespace boost::geometry
700 #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP