123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976 |
- // Boost.Geometry (aka GGL, Generic Geometry Library)
- // Copyright (c) 2016-2019, Oracle and/or its affiliates.
- // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
- // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
- // Use, modification and distribution is subject to the Boost Software License,
- // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
- // http://www.boost.org/LICENSE_1_0.txt)
- #ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
- #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
- #include <algorithm>
- #include <boost/tuple/tuple.hpp>
- #include <boost/algorithm/minmax.hpp>
- #include <boost/config.hpp>
- #include <boost/concept_check.hpp>
- #include <boost/mpl/if.hpp>
- #include <boost/type_traits/is_void.hpp>
- #include <boost/geometry/core/cs.hpp>
- #include <boost/geometry/core/access.hpp>
- #include <boost/geometry/core/radian_access.hpp>
- #include <boost/geometry/core/tags.hpp>
- #include <boost/geometry/strategies/distance.hpp>
- #include <boost/geometry/strategies/concepts/distance_concept.hpp>
- #include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
- #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
- #include <boost/geometry/strategies/spherical/point_in_point.hpp>
- #include <boost/geometry/strategies/geographic/azimuth.hpp>
- #include <boost/geometry/strategies/geographic/distance.hpp>
- #include <boost/geometry/strategies/geographic/parameters.hpp>
- #include <boost/geometry/strategies/geographic/intersection.hpp>
- #include <boost/geometry/formulas/vincenty_direct.hpp>
- #include <boost/geometry/util/math.hpp>
- #include <boost/geometry/util/promote_floating_point.hpp>
- #include <boost/geometry/util/select_calculation_type.hpp>
- #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
- #include <boost/geometry/formulas/result_direct.hpp>
- #include <boost/geometry/formulas/mean_radius.hpp>
- #include <boost/geometry/formulas/spherical.hpp>
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- #include <boost/geometry/io/dsv/write.hpp>
- #endif
- #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
- #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
- #endif
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- #include <iostream>
- #endif
- namespace boost { namespace geometry
- {
- namespace strategy { namespace distance
- {
- namespace detail
- {
- /*!
- \brief Strategy functor for distance point to segment calculation on ellipsoid
- Algorithm uses direct and inverse geodesic problems as subroutines.
- The algorithm approximates the distance by an iterative Newton method.
- \ingroup strategies
- \details Class which calculates the distance of a point to a segment, for points
- on the ellipsoid
- \see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
- https://arxiv.org/abs/1102.1215
- \tparam FormulaPolicy underlying point-point distance strategy
- \tparam Spheroid is the spheroidal model used
- \tparam CalculationType \tparam_calculation
- \tparam EnableClosestPoint computes the closest point on segment if true
- */
- template
- <
- typename FormulaPolicy = strategy::andoyer,
- typename Spheroid = srs::spheroid<double>,
- typename CalculationType = void,
- bool Bisection = false,
- bool EnableClosestPoint = false
- >
- class geographic_cross_track
- {
- public :
- typedef within::spherical_point_point equals_point_point_strategy_type;
- typedef intersection::geographic_segments
- <
- FormulaPolicy,
- strategy::default_order<FormulaPolicy>::value,
- Spheroid,
- CalculationType
- > relate_segment_segment_strategy_type;
- inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy() const
- {
- return relate_segment_segment_strategy_type(m_spheroid);
- }
- typedef within::geographic_winding
- <
- void, void, FormulaPolicy, Spheroid, CalculationType
- > point_in_geometry_strategy_type;
- inline point_in_geometry_strategy_type get_point_in_geometry_strategy() const
- {
- return point_in_geometry_strategy_type(m_spheroid);
- }
- template <typename Point, typename PointOfSegment>
- struct return_type
- : promote_floating_point
- <
- typename select_calculation_type
- <
- Point,
- PointOfSegment,
- CalculationType
- >::type
- >
- {};
- explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
- : m_spheroid(spheroid)
- {}
- template <typename Point, typename PointOfSegment>
- inline typename return_type<Point, PointOfSegment>::type
- apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
- {
- typedef typename geometry::detail::cs_angular_units<Point>::type units_type;
- return (apply<units_type>(get_as_radian<0>(sp1), get_as_radian<1>(sp1),
- get_as_radian<0>(sp2), get_as_radian<1>(sp2),
- get_as_radian<0>(p), get_as_radian<1>(p),
- m_spheroid)).distance;
- }
- // points on a meridian not crossing poles
- template <typename CT>
- inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const
- {
- typedef typename formula::meridian_inverse
- <
- CT,
- strategy::default_order<FormulaPolicy>::value
- > meridian_inverse;
- return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2,
- m_spheroid);
- }
- Spheroid const& model() const
- {
- return m_spheroid;
- }
- private :
- template <typename CT>
- struct result_distance_point_segment
- {
- result_distance_point_segment()
- : distance(0)
- , closest_point_lon(0)
- , closest_point_lat(0)
- {}
- CT distance;
- CT closest_point_lon;
- CT closest_point_lat;
- };
- template <typename CT>
- result_distance_point_segment<CT>
- static inline non_iterative_case(CT const& lon, CT const& lat, CT const& distance)
- {
- result_distance_point_segment<CT> result;
- result.distance = distance;
- if (EnableClosestPoint)
- {
- result.closest_point_lon = lon;
- result.closest_point_lat = lat;
- }
- return result;
- }
- template <typename CT>
- result_distance_point_segment<CT>
- static inline non_iterative_case(CT const& lon1, CT const& lat1, //p1
- CT const& lon2, CT const& lat2, //p2
- Spheroid const& spheroid)
- {
- CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
- ::apply(lon1, lat1, lon2, lat2, spheroid);
- return non_iterative_case(lon1, lat1, distance);
- }
- template <typename CT>
- CT static inline normalize(CT const& g4, CT& der)
- {
- CT const pi = math::pi<CT>();
- if (g4 < -1.25*pi)//close to -270
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -270" << std::endl;
- #endif
- return g4 + 1.5 * pi;
- }
- else if (g4 > 1.25*pi)//close to 270
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to 270" << std::endl;
- #endif
- der = -der;
- return - g4 + 1.5 * pi;
- }
- else if (g4 < 0 && g4 > -0.75*pi)//close to -90
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -90" << std::endl;
- #endif
- der = -der;
- return -g4 - pi/2;
- }
- return g4 - pi/2;
- }
- template <typename CT>
- static void bisection(CT const& lon1, CT const& lat1, //p1
- CT const& lon2, CT const& lat2, //p2
- CT const& lon3, CT const& lat3, //query point p3
- Spheroid const& spheroid,
- CT const& s14_start, CT const& a12,
- result_distance_point_segment<CT>& result)
- {
- typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
- direct_distance_type;
- typedef typename FormulaPolicy::template inverse<CT, true, false, false, false, false>
- inverse_distance_type;
- geometry::formula::result_direct<CT> res14;
- int counter = 0; // robustness
- bool dist_improve = true;
- CT pl_lon = lon1;
- CT pl_lat = lat1;
- CT pr_lon = lon2;
- CT pr_lat = lat2;
- CT s14 = s14_start;
- do{
- // Solve the direct problem to find p4 (GEO)
- res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "dist(pl,p3)="
- << inverse_distance_type::apply(lon3, lat3,
- pr_lon, pr_lat,
- spheroid).distance
- << std::endl;
- std::cout << "dist(pr,p3)="
- << inverse_distance_type::apply(lon3, lat3,
- pr_lon, pr_lat,
- spheroid).distance
- << std::endl;
- #endif
- if (inverse_distance_type::apply(lon3, lat3, pl_lon, pl_lat, spheroid).distance
- < inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance)
- {
- s14 -= inverse_distance_type::apply(res14.lon2, res14.lat2, pl_lon, pl_lat,
- spheroid).distance/2;
- pr_lon = res14.lon2;
- pr_lat = res14.lat2;
- }
- else
- {
- s14 += inverse_distance_type::apply(res14.lon2, res14.lat2, pr_lon, pr_lat,
- spheroid).distance/2;
- pl_lon = res14.lon2;
- pl_lat = res14.lat2;
- }
- CT new_distance = inverse_distance_type::apply(lon3, lat3,
- res14.lon2, res14.lat2,
- spheroid).distance;
- dist_improve = new_distance != result.distance;
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
- "," << res14.lat2 * math::r2d<CT>() << std::endl;
- std::cout << "pl=" << pl_lon * math::r2d<CT>() << "," << pl_lat * math::r2d<CT>()<< std::endl;
- std::cout << "pr=" << pr_lon * math::r2d<CT>() << "," << pr_lat * math::r2d<CT>() << std::endl;
- std::cout << "new_s14=" << s14 << std::endl;
- std::cout << std::setprecision(16) << "result.distance =" << result.distance << std::endl;
- std::cout << std::setprecision(16) << "new_distance =" << new_distance << std::endl;
- std::cout << "---------end of step " << counter << std::endl<< std::endl;
- if (!dist_improve)
- {
- std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
- }
- if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
- {
- std::cout << "Stop msg: counter" << std::endl;
- }
- #endif
- result.distance = new_distance;
- } while (dist_improve
- && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
- }
- template <typename CT>
- static void newton(CT const& lon1, CT const& lat1, //p1
- CT const& lon2, CT const& lat2, //p2
- CT const& lon3, CT const& lat3, //query point p3
- Spheroid const& spheroid,
- CT const& s14_start, CT const& a12,
- result_distance_point_segment<CT>& result)
- {
- typedef typename FormulaPolicy::template inverse<CT, true, true, false, true, true>
- inverse_distance_azimuth_quantities_type;
- typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
- inverse_dist_azimuth_type;
- typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
- direct_distance_type;
- CT const half_pi = math::pi<CT>() / CT(2);
- CT prev_distance;
- geometry::formula::result_direct<CT> res14;
- geometry::formula::result_inverse<CT> res34;
- res34.distance = -1;
- int counter = 0; // robustness
- CT g4;
- CT delta_g4 = 0;
- bool dist_improve = true;
- CT s14 = s14_start;
- do{
- prev_distance = res34.distance;
- // Solve the direct problem to find p4 (GEO)
- res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
- // Solve an inverse problem to find g4
- // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)
- CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2,
- lon2, lat2, spheroid).azimuth;
- res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2,
- lon3, lat3, spheroid);
- g4 = res34.azimuth - a4;
- CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
- CT m34 = res34.reduced_length;
- if (m34 != 0)
- {
- CT der = (M43 / m34) * sin(g4);
- delta_g4 = normalize(g4, der);
- s14 -= der != 0 ? delta_g4 / der : 0;
- }
- result.distance = res34.distance;
- dist_improve = prev_distance > res34.distance || prev_distance == -1;
- if (!dist_improve)
- {
- result.distance = prev_distance;
- }
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
- "," << res14.lat2 * math::r2d<CT>() << std::endl;
- std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
- std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
- std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl;
- std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>() << std::endl;
- std::cout << "der=" << der << std::endl;
- std::cout << "M43=" << M43 << std::endl;
- std::cout << "m34=" << m34 << std::endl;
- std::cout << "new_s14=" << s14 << std::endl;
- std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl;
- std::cout << "---------end of step " << counter << std::endl<< std::endl;
- if (g4 == half_pi)
- {
- std::cout << "Stop msg: g4 == half_pi" << std::endl;
- }
- if (!dist_improve)
- {
- std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
- }
- if (delta_g4 == 0)
- {
- std::cout << "Stop msg: delta_g4 == 0" << std::endl;
- }
- if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
- {
- std::cout << "Stop msg: counter" << std::endl;
- }
- #endif
- } while (g4 != half_pi
- && dist_improve
- && delta_g4 != 0
- && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "distance=" << res34.distance << std::endl;
- std::cout << "s34(geo) ="
- << inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2, lon3, lat3, spheroid).distance
- << ", p4=(" << res14.lon2 * math::r2d<double>() << ","
- << res14.lat2 * math::r2d<double>() << ")"
- << std::endl;
- CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
- CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;
- CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2, lon2, lat2, spheroid).azimuth;
- geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(res14.lon2, res14.lat2, .04, a4, spheroid);
- CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;
- geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
- CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;
- std::cout << "s31=" << s31 << "\ns32=" << s32
- << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
- << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
- << std::endl;
- if (res34.distance <= p4_plus && res34.distance <= p4_minus)
- {
- std::cout << "Closest point computed" << std::endl;
- }
- else
- {
- std::cout << "There is a closer point nearby" << std::endl;
- }
- #endif
- }
- template <typename Units, typename CT>
- result_distance_point_segment<CT>
- static inline apply(CT const& lo1, CT const& la1, //p1
- CT const& lo2, CT const& la2, //p2
- CT const& lo3, CT const& la3, //query point p3
- Spheroid const& spheroid)
- {
- typedef typename FormulaPolicy::template inverse<CT, true, true, false, false, false>
- inverse_dist_azimuth_type;
- typedef typename FormulaPolicy::template inverse<CT, true, true, true, false, false>
- inverse_dist_azimuth_reverse_type;
- CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
- result_distance_point_segment<CT> result;
- // Constants
- //CT const f = geometry::formula::flattening<CT>(spheroid);
- CT const pi = math::pi<CT>();
- CT const half_pi = pi / CT(2);
- CT const c0 = CT(0);
- CT lon1 = lo1;
- CT lat1 = la1;
- CT lon2 = lo2;
- CT lat2 = la2;
- CT lon3 = lo3;
- CT lat3 = la3;
- if (lon1 > lon2)
- {
- std::swap(lon1, lon2);
- std::swap(lat1, lat2);
- }
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>();
- std::cout << "," << lat1 * math::r2d<CT>();
- std::cout << "),(" << lon2 * math::r2d<CT>();
- std::cout << "," << lat2 * math::r2d<CT>();
- std::cout << ")\np=(" << lon3 * math::r2d<CT>();
- std::cout << "," << lat3 * math::r2d<CT>();
- std::cout << ")" << std::endl;
- #endif
- //segment on equator
- //Note: antipodal points on equator does not define segment on equator
- //but pass by the pole
- CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
- typedef typename formula::meridian_inverse<CT>
- meridian_inverse;
- bool meridian_not_crossing_pole =
- meridian_inverse::meridian_not_crossing_pole
- (lat1, lat2, diff);
- bool meridian_crossing_pole =
- meridian_inverse::meridian_crossing_pole(diff);
- if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole)
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "Equatorial segment" << std::endl;
- std::cout << "segment=(" << lon1 * math::r2d<CT>();
- std::cout << "," << lat1 * math::r2d<CT>();
- std::cout << "),(" << lon2 * math::r2d<CT>();
- std::cout << "," << lat2 * math::r2d<CT>();
- std::cout << ")\np=(" << lon3 * math::r2d<CT>();
- std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
- #endif
- if (lon3 <= lon1)
- {
- return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
- }
- if (lon3 >= lon2)
- {
- return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
- }
- return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
- }
- if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && std::abs(lat1) > std::abs(lat2))
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "Meridian segment not crossing pole" << std::endl;
- #endif
- std::swap(lat1,lat2);
- }
- if (meridian_crossing_pole)
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "Meridian segment crossing pole" << std::endl;
- #endif
- CT sign_non_zero = lat3 >= c0 ? 1 : -1;
- result_distance_point_segment<CT> res13 =
- apply<geometry::radian>(lon1, lat1,
- lon1, half_pi * sign_non_zero,
- lon3, lat3, spheroid);
- result_distance_point_segment<CT> res23 =
- apply<geometry::radian>(lon2, lat2,
- lon2, half_pi * sign_non_zero,
- lon3, lat3, spheroid);
- if (res13.distance < res23.distance)
- {
- return res13;
- }
- else
- {
- return res23;
- }
- }
- geometry::formula::result_inverse<CT> res12 =
- inverse_dist_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
- geometry::formula::result_inverse<CT> res13 =
- inverse_dist_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid);
- if (geometry::math::equals(res12.distance, c0))
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "Degenerate segment" << std::endl;
- std::cout << "distance between points=" << res13.distance << std::endl;
- #endif
- typename meridian_inverse::result res =
- meridian_inverse::apply(lon1, lat1, lon3, lat3, spheroid);
- return non_iterative_case(lon1, lat2,
- res.meridian ? res.distance : res13.distance);
- }
- // Compute a12 (GEO)
- CT a312 = res13.azimuth - res12.azimuth;
- // TODO: meridian case optimization
- if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole)
- {
- boost::tuple<CT,CT> minmax_elem = boost::minmax(lat1, lat2);
- if (lat3 >= minmax_elem.template get<0>() &&
- lat3 <= minmax_elem.template get<1>())
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "Point on meridian segment" << std::endl;
- #endif
- return non_iterative_case(lon3, lat3, c0);
- }
- }
- CT projection1 = cos( a312 ) * res13.distance / res12.distance;
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "a1=" << res12.azimuth * math::r2d<CT>() << std::endl;
- std::cout << "a13=" << res13.azimuth * math::r2d<CT>() << std::endl;
- std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
- std::cout << "cos(a312)=" << cos(a312) << std::endl;
- std::cout << "projection 1=" << projection1 << std::endl;
- #endif
- if (projection1 < c0)
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "projection closer to p1" << std::endl;
- #endif
- // projection of p3 on geodesic spanned by segment (p1,p2) fall
- // outside of segment on the side of p1
- return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
- }
- geometry::formula::result_inverse<CT> res23 =
- inverse_dist_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid);
- CT a321 = res23.azimuth - res12.reverse_azimuth + pi;
- CT projection2 = cos( a321 ) * res23.distance / res12.distance;
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "a21=" << res12.reverse_azimuth * math::r2d<CT>() << std::endl;
- std::cout << "a23=" << res23.azimuth * math::r2d<CT>() << std::endl;
- std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
- std::cout << "cos(a321)=" << cos(a321) << std::endl;
- std::cout << "projection 2=" << projection2 << std::endl;
- #endif
- if (projection2 < c0)
- {
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "projection closer to p2" << std::endl;
- #endif
- // projection of p3 on geodesic spanned by segment (p1,p2) fall
- // outside of segment on the side of p2
- return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
- }
- // Guess s14 (SPHERICAL) aka along-track distance
- typedef geometry::model::point
- <
- CT, 2,
- geometry::cs::spherical_equatorial<geometry::radian>
- > point;
- point p1 = point(lon1, lat1);
- point p2 = point(lon2, lat2);
- point p3 = point(lon3, lat3);
- geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
- CT s34_sph = cross_track.apply(p3, p1, p2);
- geometry::strategy::distance::haversine<CT> str(earth_radius);
- CT s13_sph = str.apply(p1, p3);
- //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
- CT cos_frac = cos(s13_sph / earth_radius) / cos(s34_sph / earth_radius);
- CT s14_sph = cos_frac >= 1 ? CT(0)
- : cos_frac <= -1 ? pi * earth_radius
- : acos(cos_frac) * earth_radius;
- CT a12_sph = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2);
- geometry::formula::result_direct<CT> res
- = geometry::formula::spherical_direct<true, false>
- (lon1, lat1, s14_sph, a12_sph, srs::sphere<CT>(earth_radius));
- // this is what postgis (version 2.5) returns
- // geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
- // ::apply(lon3, lat3, res.lon2, res.lat2, spheroid);
- #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
- std::cout << "s34=" << s34_sph << std::endl;
- std::cout << "s13=" << res13.distance << std::endl;
- std::cout << "s14=" << s14_sph << std::endl;
- std::cout << "===============" << std::endl;
- #endif
- // Update s14 (using Newton method)
- if (Bisection)
- {
- bisection<CT>(lon1, lat1, lon2, lat2, lon3, lat3,
- spheroid, res12.distance/2, res12.azimuth, result);
- }
- else
- {
- CT s14_start = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
- ::apply(lon1, lat1, res.lon2, res.lat2, spheroid);
- newton<CT>(lon1, lat1, lon2, lat2, lon3, lat3,
- spheroid, s14_start, res12.azimuth, result);
- }
- return result;
- }
- Spheroid m_spheroid;
- };
- } // namespace detail
- template
- <
- typename FormulaPolicy = strategy::andoyer,
- typename Spheroid = srs::spheroid<double>,
- typename CalculationType = void
- >
- class geographic_cross_track
- : public detail::geographic_cross_track
- <
- FormulaPolicy,
- Spheroid,
- CalculationType,
- false,
- false
- >
- {
- public :
- explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
- :
- detail::geographic_cross_track<
- FormulaPolicy,
- Spheroid,
- CalculationType,
- false,
- false
- >(spheroid)
- {}
- };
- #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
- namespace services
- {
- //tags
- template <typename FormulaPolicy>
- struct tag<geographic_cross_track<FormulaPolicy> >
- {
- typedef strategy_tag_distance_point_segment type;
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid
- >
- struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
- {
- typedef strategy_tag_distance_point_segment type;
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType
- >
- struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
- {
- typedef strategy_tag_distance_point_segment type;
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- bool Bisection
- >
- struct tag<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> >
- {
- typedef strategy_tag_distance_point_segment type;
- };
- //return types
- template <typename FormulaPolicy, typename P, typename PS>
- struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
- : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
- {};
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename P,
- typename PS
- >
- struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
- : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
- {};
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- typename P,
- typename PS
- >
- struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
- : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
- {};
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- bool Bisection,
- typename P,
- typename PS
- >
- struct return_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection>, P, PS>
- : detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection>::template return_type<P, PS>
- {};
- //comparable types
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType
- >
- struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
- {
- typedef geographic_cross_track
- <
- FormulaPolicy, Spheroid, CalculationType
- > type;
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- bool Bisection
- >
- struct comparable_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> >
- {
- typedef detail::geographic_cross_track
- <
- FormulaPolicy, Spheroid, CalculationType, Bisection
- > type;
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType
- >
- struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
- {
- public :
- static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
- apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
- {
- return strategy;
- }
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- bool Bisection
- >
- struct get_comparable<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> >
- {
- public :
- static inline detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection>
- apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection> const& strategy)
- {
- return strategy;
- }
- };
- template
- <
- typename FormulaPolicy,
- typename P,
- typename PS
- >
- struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
- {
- private :
- typedef typename geographic_cross_track
- <
- FormulaPolicy
- >::template return_type<P, PS>::type return_type;
- public :
- template <typename T>
- static inline return_type
- apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
- {
- return distance;
- }
- };
- template
- <
- typename FormulaPolicy,
- typename Spheroid,
- typename CalculationType,
- typename P,
- typename PS
- >
- struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
- {
- private :
- typedef typename geographic_cross_track
- <
- FormulaPolicy, Spheroid, CalculationType
- >::template return_type<P, PS>::type return_type;
- public :
- template <typename T>
- static inline return_type
- apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
- {
- return distance;
- }
- };
- template <typename Point, typename PointOfSegment>
- struct default_strategy
- <
- point_tag, segment_tag, Point, PointOfSegment,
- geographic_tag, geographic_tag
- >
- {
- typedef geographic_cross_track<> type;
- };
- template <typename PointOfSegment, typename Point>
- struct default_strategy
- <
- segment_tag, point_tag, PointOfSegment, Point,
- geographic_tag, geographic_tag
- >
- {
- typedef typename default_strategy
- <
- point_tag, segment_tag, Point, PointOfSegment,
- geographic_tag, geographic_tag
- >::type type;
- };
- } // namespace services
- #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
- }} // namespace strategy::distance
- }} // namespace boost::geometry
- #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
|