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)
170 CT const pi = math::pi<CT>();
171 if (g4 < 0 && g4 < -pi)//close to -270
173 return g4 + 1.5 * pi;
175 else if (g4 > 0 && g4 > pi)//close to 270
177 return - g4 + 1.5 * pi;
179 else if (g4 < 0 && g4 > -pi)//close to -90
186 template <typename Units, typename CT>
187 result_distance_point_segment<CT>
188 static inline apply(CT lon1, CT lat1, //p1
189 CT lon2, CT lat2, //p2
190 CT lon3, CT lat3, //query point p3
191 Spheroid const& spheroid)
193 typedef typename FormulaPolicy::template inverse<CT, true, false, false, true, true>
194 inverse_distance_quantities_type;
195 typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
196 inverse_azimuth_type;
197 typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
198 inverse_azimuth_reverse_type;
199 typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
200 direct_distance_type;
202 CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
204 result_distance_point_segment<CT> result;
207 CT const f = geometry::formula::flattening<CT>(spheroid);
208 CT const pi = math::pi<CT>();
209 CT const half_pi = pi / CT(2);
212 // Convert to radians
213 lon1 = math::as_radian<Units>(lon1);
214 lat1 = math::as_radian<Units>(lat1);
215 lon2 = math::as_radian<Units>(lon2);
216 lat2 = math::as_radian<Units>(lat2);
217 lon3 = math::as_radian<Units>(lon3);
218 lat3 = math::as_radian<Units>(lat3);
222 std::swap(lon1, lon2);
223 std::swap(lat1, lat2);
227 //Note: antipodal points on equator does not define segment on equator
228 //but pass by the pole
229 CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
230 if (math::equals(lat1, c0) && math::equals(lat2, c0)
231 && !math::equals(math::abs(diff), pi))
233 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
234 std::cout << "Equatorial segment" << std::endl;
238 return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
242 return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
244 return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
247 if (math::equals(math::abs(diff), pi))
249 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
250 std::cout << "Meridian segment" << std::endl;
252 result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid);
253 result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid);
254 if (d1.distance < d2.distance)
264 CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
265 ::apply(lon1, lat1, lon3, lat3, spheroid);
267 CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
268 ::apply(lon1, lat1, lon2, lat2, spheroid);
270 if (geometry::math::equals(d3, c0))
272 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
273 std::cout << "Degenerate segment" << std::endl;
274 std::cout << "distance between points=" << d1 << std::endl;
276 return non_iterative_case(lon1, lat2, d1);
279 CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
280 ::apply(lon2, lat2, lon3, lat3, spheroid);
283 geometry::formula::result_inverse<CT> res12 =
284 inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
285 CT a12 = res12.azimuth;
286 CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;
290 if (geometry::math::equals(a312, c0))
292 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
293 std::cout << "point on segment" << std::endl;
295 return non_iterative_case(lon3, lat3, c0);
298 CT projection1 = cos( a312 ) * d1 / d3;
300 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
301 std::cout << "segment=(" << lon1 * math::r2d<CT>();
302 std::cout << "," << lat1 * math::r2d<CT>();
303 std::cout << "),(" << lon2 * math::r2d<CT>();
304 std::cout << "," << lat2 * math::r2d<CT>();
305 std::cout << ")\np=(" << lon3 * math::r2d<CT>();
306 std::cout << "," << lat3 * math::r2d<CT>();
307 std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl;
308 std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
309 std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
310 std::cout << "cos(a312)=" << cos(a312) << std::endl;
312 if (projection1 < 0.0)
314 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
315 std::cout << "projection closer to p1" << std::endl;
317 // projection of p3 on geodesic spanned by segment (p1,p2) fall
318 // outside of segment on the side of p1
319 return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
322 CT a21 = res12.reverse_azimuth - pi;
323 CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;
327 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
328 std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
329 std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
330 std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
331 std::cout << "cos(a321)=" << cos(a321) << std::endl;
333 CT projection2 = cos( a321 ) * d2 / d3;
335 if (projection2 < 0.0)
337 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
338 std::cout << "projection closer to p2" << std::endl;
340 // projection of p3 on geodesic spanned by segment (p1,p2) fall
341 // outside of segment on the side of p2
342 return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
345 // Guess s14 (SPHERICAL)
346 typedef geometry::model::point
349 geometry::cs::spherical_equatorial<geometry::radian>
352 CT bet1 = atan((1 - f) * tan(lon1));
353 CT bet2 = atan((1 - f) * tan(lon2));
354 CT bet3 = atan((1 - f) * tan(lon3));
355 point p1 = point(bet1, lat1);
356 point p2 = point(bet2, lat2);
357 point p3 = point(bet3, lat3);
359 geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
360 CT s34 = cross_track.apply(p3, p1, p2);
362 geometry::strategy::distance::haversine<CT> str(earth_radius);
363 CT s13 = str.apply(p1, p3);
364 CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
366 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
367 std::cout << "s34=" << s34 << std::endl;
368 std::cout << "s13=" << s13 << std::endl;
369 std::cout << "s14=" << s14 << std::endl;
370 std::cout << "===============" << std::endl;
373 // Update s14 (using Newton method)
374 CT prev_distance = 0;
375 geometry::formula::result_direct<CT> res14;
376 geometry::formula::result_inverse<CT> res34;
378 int counter = 0; // robustness
383 prev_distance = res34.distance;
385 // Solve the direct problem to find p4 (GEO)
386 res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
388 // Solve an inverse problem to find g4
389 // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)
391 CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
392 lon2, lat2, spheroid).azimuth;
393 res34 = inverse_distance_quantities_type::apply(res14.lon2, res14.lat2,
394 lon3, lat3, spheroid);
395 g4 = res34.azimuth - a4;
397 delta_g4 = normalize(g4);
399 CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
400 CT m34 = res34.reduced_length;
401 CT der = (M43 / m34) * sin(g4);
402 s14 = s14 - delta_g4 / der;
404 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
405 std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
406 "," << res14.lat2 * math::r2d<CT>() << std::endl;
407 std::cout << "delta_g4=" << delta_g4 << std::endl;
408 std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl;
409 std::cout << "der=" << der << std::endl;
410 std::cout << "M43=" << M43 << std::endl;
411 std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
412 std::cout << "m34=" << m34 << std::endl;
413 std::cout << "new_s14=" << s14 << std::endl;
414 std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl;
415 std::cout << "---------end of step " << counter << std::endl<< std::endl;
417 result.distance = prev_distance;
419 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
422 std::cout << "Stop msg: g4 == half_pi" << std::endl;
424 if (res34.distance >= prev_distance && prev_distance != 0)
426 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
430 std::cout << "Stop msg: delta_g4 == 0" << std::endl;
434 std::cout << "Stop msg: counter" << std::endl;
438 } while (g4 != half_pi
439 && (prev_distance > res34.distance || prev_distance == 0)
441 && ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ;
443 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
444 std::cout << "distance=" << res34.distance << std::endl;
446 point p4(res14.lon2, res14.lat2);
447 CT s34_sph = str.apply(p4, p3);
449 std::cout << "s34(sph) =" << s34_sph << std::endl;
450 std::cout << "s34(geo) ="
451 << inverse_distance_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
452 << ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
453 << get<1>(p4) * math::r2d<double>() << ")"
456 CT s31 = inverse_distance_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
457 CT s32 = inverse_distance_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;
459 CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
460 geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
461 CT p4_plus = inverse_distance_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;
463 geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
464 CT p4_minus = inverse_distance_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;
466 std::cout << "s31=" << s31 << "\ns32=" << s32
467 << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
468 << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
471 if (res34.distance <= p4_plus && res34.distance <= p4_minus)
473 std::cout << "Closest point computed" << std::endl;
477 std::cout << "There is a closer point nearby" << std::endl;
489 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
494 template <typename FormulaPolicy>
495 struct tag<geographic_cross_track<FormulaPolicy> >
497 typedef strategy_tag_distance_point_segment type;
502 typename FormulaPolicy,
505 struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
507 typedef strategy_tag_distance_point_segment type;
512 typename FormulaPolicy,
514 typename CalculationType
516 struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
518 typedef strategy_tag_distance_point_segment type;
523 template <typename FormulaPolicy, typename P, typename PS>
524 struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
525 : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
530 typename FormulaPolicy,
535 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
536 : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
541 typename FormulaPolicy,
543 typename CalculationType,
547 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
548 : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
554 typename FormulaPolicy,
556 typename CalculationType
558 struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
560 typedef geographic_cross_track
562 FormulaPolicy, Spheroid, CalculationType
568 typename FormulaPolicy,
570 typename CalculationType
572 struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
574 typedef typename comparable_type
576 geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
577 >::type comparable_type;
579 static inline comparable_type
580 apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& )
582 return comparable_type();
589 typename FormulaPolicy,
593 struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
596 typedef typename geographic_cross_track
599 >::template return_type<P, PS>::type return_type;
601 template <typename T>
602 static inline return_type
603 apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
610 template <typename Point, typename PointOfSegment>
611 struct default_strategy
613 point_tag, segment_tag, Point, PointOfSegment,
614 geographic_tag, geographic_tag
617 typedef geographic_cross_track<> type;
621 template <typename PointOfSegment, typename Point>
622 struct default_strategy
624 segment_tag, point_tag, PointOfSegment, Point,
625 geographic_tag, geographic_tag
628 typedef typename default_strategy
630 point_tag, segment_tag, Point, PointOfSegment,
631 geographic_tag, geographic_tag
635 } // namespace services
636 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
638 }} // namespace strategy::distance
640 }} // namespace boost::geometry
641 #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP