strategies/geographic/distance_cross_track.hpp

0001 // Boost.Geometry (aka GGL, Generic Geometry Library)
0002
0003 // Copyright (c) 2023-2025 Adam Wulkiewicz, Lodz, Poland.
0004
0005 // Copyright (c) 2016-2022, Oracle and/or its affiliates.
0006 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
0007 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0008
0009 // Use, modification and distribution is subject to the Boost Software License,
0010 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0011 // http://www.boost.org/LICENSE_1_0.txt)
0012
0013 #ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
0014 #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
0015
0016 #include <algorithm>
0017
0018 #include <boost/config.hpp>
0019 #include <boost/concept_check.hpp>
0020
0021 #include <boost/geometry/core/cs.hpp>
0022 #include <boost/geometry/core/access.hpp>
0023 #include <boost/geometry/core/coordinate_promotion.hpp>
0024 #include <boost/geometry/core/radian_access.hpp>
0025 #include <boost/geometry/core/tags.hpp>
0026
0027 #include <boost/geometry/strategies/distance.hpp>
0028 #include <boost/geometry/strategies/concepts/distance_concept.hpp>
0029 #include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
0030 #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
0031 #include <boost/geometry/strategies/spherical/point_in_point.hpp>
0032 #include <boost/geometry/strategies/geographic/azimuth.hpp>
0033 #include <boost/geometry/strategies/geographic/distance.hpp>
0034 #include <boost/geometry/strategies/geographic/parameters.hpp>
0035 #include <boost/geometry/strategies/geographic/intersection.hpp>
0036
0037 #include <boost/geometry/formulas/vincenty_direct.hpp>
0038
0039 #include <boost/geometry/util/math.hpp>
0040 #include <boost/geometry/util/select_calculation_type.hpp>
0041 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
0042
0043 #include <boost/geometry/formulas/result_direct.hpp>
0044 #include <boost/geometry/formulas/mean_radius.hpp>
0045 #include <boost/geometry/formulas/spherical.hpp>
0046
0047 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0048 #include <boost/geometry/io/dsv/write.hpp>
0049 #endif
0050
0051 #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
0052 #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
0053 #endif
0054
0055 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0056 #include <iostream>
0057 #endif
0058
0059 namespace boost { namespace geometry
0060 {
0061
0062 namespace strategy { namespace distance
0063 {
0064 namespace detail
0065 {
0066
0067 template <bool EnableClosestPoint>
0068 struct set_result
0069 {
0070     template <typename CT, typename ResultType>
0071     static void apply(CT const& distance,
0072                       CT const&,
0073                       CT const&,
0074                       ResultType& result)
0075     {
0076         result.distance = distance;
0077     }
0078 };
0079
0080 template<>
0081 struct set_result<true>
0082 {
0083     template <typename CT, typename ResultType>
0084     static void apply(CT const&,
0085                       CT const& lon,
0086                       CT const& lat,
0087                       ResultType& result)
0088     {
0089         result.lon = lon;
0090         result.lat = lat;
0091     }
0092 };
0093
0094
0095 /*!
0096 \brief Strategy functor for distance point to segment calculation on ellipsoid
0097        Algorithm uses direct and inverse geodesic problems as subroutines.
0098        The algorithm approximates the distance by an iterative Newton method.
0099 \ingroup strategies
0100 \details Class which calculates the distance of a point to a segment, for points
0101 on the ellipsoid
0102 \see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
0103       https://arxiv.org/abs/1102.1215
0104 \tparam FormulaPolicy underlying point-point distance strategy
0105 \tparam Spheroid is the spheroidal model used
0106 \tparam CalculationType \tparam_calculation
0107 \tparam EnableClosestPoint computes the closest point on segment if true
0108 */
0109 template
0110 <
0111     typename FormulaPolicy = strategy::andoyer,
0112     typename Spheroid = srs::spheroid<double>,
0113     typename CalculationType = void,
0114     bool Bisection = false,
0115     bool EnableClosestPoint = false
0116 >
0117 class geographic_cross_track
0118 {
0119
0120 public:
0121
0122     geographic_cross_track() = default;
0123
0124     explicit geographic_cross_track(Spheroid const& spheroid)
0125         : m_spheroid(spheroid)
0126     {}
0127
0128     Spheroid const& model() const
0129     {
0130         return m_spheroid;
0131     }
0132
0133     template <typename Point, typename PointOfSegment>
0134     struct return_type
0135         : promote_floating_point
0136           <
0137               typename select_calculation_type
0138                   <
0139                       Point,
0140                       PointOfSegment,
0141                       CalculationType
0142                   >::type
0143           >
0144     {};
0145
0146     template <typename Point, typename PointOfSegment>
0147     auto apply(Point const& p,
0148                PointOfSegment const& sp1,
0149                PointOfSegment const& sp2) const
0150     {
0151         return apply(get_as_radian<0>(sp1), get_as_radian<1>(sp1),
0152                      get_as_radian<0>(sp2), get_as_radian<1>(sp2),
0153                      get_as_radian<0>(p), get_as_radian<1>(p),
0154                      m_spheroid).distance;
0155     }
0156
0157     // points on a meridian not crossing poles
0158     template <typename CT>
0159     inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const
0160     {
0161         using meridian_inverse = typename formula::meridian_inverse
0162         <
0163             CT,
0164             strategy::default_order<FormulaPolicy>::value
0165         >;
0166
0167         return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2, m_spheroid);
0168     }
0169
0170 private:
0171
0172     template <typename CT>
0173     struct result_type
0174     {
0175         result_type()
0176             : distance(0)
0177             , lon(0)
0178             , lat(0)
0179         {}
0180
0181         CT distance;
0182         CT lon;
0183         CT lat;
0184     };
0185
0186     template <typename CT>
0187     static inline CT normalize(CT const& g4, CT& der)
0188     {
0189         CT const pi = math::pi<CT>();
0190         CT const c_1_5 = 1.5;
0191
0192         if (g4 < -1.25*pi)//close to -270
0193         {
0194 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0195             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to -270" << std::endl;
0196 #endif
0197             return g4 + c_1_5 * pi;
0198         }
0199         else if (g4 > 1.25*pi)//close to 270
0200         {
0201 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0202             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to 270" << std::endl;
0203 #endif
0204             der = -der;
0205             return - g4 + c_1_5 * pi;
0206         }
0207         else if (g4 < 0 && g4 > -0.75*pi)//close to -90
0208         {
0209 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0210             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to -90" << std::endl;
0211 #endif
0212             der = -der;
0213             return -g4 - pi/2;
0214         }
0215         return g4 - pi/2;
0216     }
0217
0218
0219     template <typename CT>
0220     static void bisection(CT const& lon1, CT const& lat1, //p1
0221                           CT const& lon2, CT const& lat2, //p2
0222                           CT const& lon3, CT const& lat3, //query point p3
0223                           Spheroid const& spheroid,
0224                           CT const& s14_start, CT const& a12,
0225                           result_type<CT>& result)
0226     {
0227         using direct_distance_type =
0228             typename FormulaPolicy::template direct<CT, true, false, false, false>;
0229         using inverse_distance_type =
0230             typename FormulaPolicy::template inverse<CT, true, false, false, false, false>;
0231
0232         geometry::formula::result_direct<CT> res14;
0233
0234         int counter = 0; // robustness
0235         bool dist_improve = true;
0236
0237         CT pl_lon = lon1;
0238         CT pl_lat = lat1;
0239         CT pr_lon = lon2;
0240         CT pr_lat = lat2;
0241
0242         CT s14 = s14_start;
0243
0244         do {
0245             // Solve the direct problem to find p4 (GEO)
0246             res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
0247
0248 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0249             std::cout << "dist(pl,p3)="
0250                       << inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance
0251                       << std::endl;
0252             std::cout << "dist(pr,p3)="
0253                       << inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance
0254                       << std::endl;
0255 #endif
0256             CT const dist_l =
0257                 inverse_distance_type::apply(lon3, lat3, pl_lon, pl_lat, spheroid).distance;
0258             CT const dist_r =
0259                 inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance;
0260
0261             if (dist_l < dist_r)
0262             {
0263                 s14 -= inverse_distance_type::apply(res14.lon2, res14.lat2, pl_lon,
0264                     pl_lat, spheroid).distance/2;
0265                 pr_lon = res14.lon2;
0266                 pr_lat = res14.lat2;
0267             }
0268             else
0269             {
0270                 s14 += inverse_distance_type::apply(res14.lon2, res14.lat2, pr_lon,
0271                     pr_lat, spheroid).distance/2;
0272                 pl_lon = res14.lon2;
0273                 pl_lat = res14.lat2;
0274             }
0275
0276             CT const new_distance = inverse_distance_type::apply(lon3, lat3, res14.lon2, res14.lat2,
0277                 spheroid).distance;
0278
0279             dist_improve = new_distance != result.distance;
0280
0281 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0282             std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
0283                          "," << res14.lat2 * math::r2d<CT>() << std::endl;
0284             std::cout << "pl=" << pl_lon * math::r2d<CT>() << ","
0285                       << pl_lat * math::r2d<CT>()<< std::endl;
0286             std::cout << "pr=" << pr_lon * math::r2d<CT>() << ","
0287                       << pr_lat * math::r2d<CT>() << std::endl;
0288             std::cout << "new_s14=" << s14 << std::endl;
0289             std::cout << std::setprecision(16) << "result.distance ="
0290                       << result.distance << std::endl;
0291             std::cout << std::setprecision(16) << "new_distance ="
0292                       << new_distance << std::endl;
0293             std::cout << "---------end of step " << counter
0294                       << std::endl<< std::endl;
0295             if (!dist_improve)
0296             {
0297                 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
0298             }
0299             if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
0300             {
0301                 std::cout << "Stop msg: counter" << std::endl;
0302             }
0303 #endif
0304             set_result<EnableClosestPoint>::apply(new_distance, res14.lon2, res14.lat2, result);
0305
0306         } while (dist_improve && counter++
0307                  < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
0308     }
0309
0310     template <typename CT>
0311     static void newton(CT const& lon1, CT const& lat1, //p1
0312                        CT const& lon2, CT const& lat2, //p2
0313                        CT const& lon3, CT const& lat3, //query point p3
0314                        Spheroid const& spheroid,
0315                        CT const& s14_start, CT const& a12,
0316                        result_type<CT>& result)
0317     {
0318         using inverse_distance_azimuth_quantities_type =
0319             typename FormulaPolicy::template inverse<CT, true, true, false, true, true>;
0320
0321         using inverse_dist_azimuth_type =
0322             typename FormulaPolicy::template inverse<CT, false, true, false, false, false>;
0323
0324         using direct_distance_type =
0325             typename FormulaPolicy::template direct<CT, true, false, false, false>;
0326
0327         CT const half_pi = math::pi<CT>() / CT(2);
0328         geometry::formula::result_direct<CT> res14;
0329         geometry::formula::result_inverse<CT> res34;
0330         res34.distance = -1;
0331
0332         int counter = 0; // robustness
0333         CT g4;
0334         CT delta_g4 = 0;
0335         bool dist_improve = true;
0336         CT s14 = s14_start;
0337
0338         do {
0339             auto prev_distance = res34.distance;
0340             auto prev_res = res14;
0341
0342             // Solve the direct problem to find p4 (GEO)
0343             res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
0344
0345             // Solve an inverse problem to find g4
0346             // g4 is the angle between segment (p1,p2) and segment (p3,p4)
0347             // that meet on p4 (GEO)
0348
0349             CT const a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2,
0350                                                            lon2, lat2, spheroid).azimuth;
0351             res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2,
0352                                                                     lon3, lat3, spheroid);
0353             g4 = res34.azimuth - a4;
0354
0355             // cos(s14/earth_radius) is the spherical limit
0356             CT M43 = res34.geodesic_scale;
0357             CT m34 = res34.reduced_length;
0358
0359             if (m34 != 0)
0360             {
0361                 CT der = (M43 / m34) * sin(g4);
0362                 delta_g4 = normalize(g4, der);
0363                 s14 -= der != 0 ? delta_g4 / der : 0;
0364             }
0365
0366             dist_improve = prev_distance > res34.distance || prev_distance == -1;
0367
0368             if (dist_improve)
0369             {
0370                 set_result<EnableClosestPoint>::apply(res34.distance, res14.lon2, res14.lat2,
0371                                                       result);
0372             }
0373             else
0374             {
0375                 set_result<EnableClosestPoint>::apply(prev_distance, prev_res.lon2, prev_res.lat2,
0376                                                       result);
0377             }
0378
0379 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0380             std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
0381                          "," << res14.lat2 * math::r2d<CT>() << std::endl;
0382             std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
0383             std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
0384             std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl;
0385             std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>()  << std::endl;
0386             std::cout << "M43=" << M43 << std::endl;
0387             std::cout << "m34=" << m34 << std::endl;
0388             std::cout << "new_s14=" << s14 << std::endl;
0389             std::cout << std::setprecision(16) << "dist     ="
0390                       << res34.distance << std::endl;
0391             std::cout << "---------end of step " << counter
0392                       << std::endl<< std::endl;
0393             if (g4 == half_pi)
0394             {
0395                 std::cout << "Stop msg: g4 == half_pi" << std::endl;
0396             }
0397             if (!dist_improve)
0398             {
0399                 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
0400             }
0401             if (delta_g4 == 0)
0402             {
0403                 std::cout << "Stop msg: delta_g4 == 0" << std::endl;
0404             }
0405             if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
0406             {
0407                 std::cout << "Stop msg: counter" << std::endl;
0408             }
0409 #endif
0410
0411         } while (g4 != half_pi
0412                  && dist_improve
0413                  && delta_g4 != 0
0414                  && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
0415 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0416         std::cout << "distance=" << res34.distance << std::endl;
0417
0418         std::cout << "s34(geo) ="
0419                   << inverse_distance_azimuth_quantities_type
0420                      ::apply(res14.lon2, res14.lat2,
0421                              lon3, lat3, spheroid).distance
0422                   << ", p4=(" << res14.lon2 * math::r2d<CT>() << ","
0423                               << res14.lat2 * math::r2d<CT>() << ")"
0424                   << std::endl;
0425
0426         CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid)
0427             .distance;
0428         CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid)
0429             .distance;
0430
0431         CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2, lon2, lat2, spheroid)
0432             .azimuth;
0433
0434         geometry::formula::result_direct<CT> res4 =
0435             direct_distance_type::apply(res14.lon2, res14.lat2, .04, a4, spheroid);
0436
0437         CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3,
0438             lat3, spheroid).distance;
0439
0440         geometry::formula::result_direct<CT> res1 =
0441             direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
0442
0443         CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3,
0444             lat3, spheroid).distance;
0445
0446         std::cout << "s31=" << s31 << "\ns32=" << s32
0447                   << "\np4_plus=" << p4_plus << ", p4=("
0448                   << res4.lon2 * math::r2d<CT>() << ","
0449                   << res4.lat2 * math::r2d<CT>() << ")"
0450                   << "\np4_minus=" << p4_minus << ", p4=("
0451                   << res1.lon2 * math::r2d<CT>() << ","
0452                   << res1.lat2 * math::r2d<CT>() << ")"
0453                   << std::endl;
0454
0455         if (res34.distance <= p4_plus && res34.distance <= p4_minus)
0456         {
0457             std::cout << "Closest point computed" << std::endl;
0458         }
0459         else
0460         {
0461             std::cout << "There is a closer point nearby" << std::endl;
0462         }
0463 #endif
0464     }
0465
0466
0467     template <typename CT>
0468     static inline auto non_iterative_case(CT const&     , CT const&     , //p1
0469                                           CT const& lon2, CT const& lat2, //p2
0470                                           CT const& distance)
0471     {
0472         result_type<CT> result;
0473
0474         set_result<EnableClosestPoint>::apply(distance, lon2, lat2, result);
0475
0476         return result;
0477     }
0478
0479     template <typename CT>
0480     static inline auto non_iterative_case(CT const& lon1, CT const& lat1, //p1
0481                                           CT const& lon2, CT const& lat2, //p2
0482                                           Spheroid const& spheroid)
0483     {
0484         CT distance = geometry::strategy::distance::geographic
0485             <
0486                 FormulaPolicy,
0487                 Spheroid,
0488                 CT
0489             >::apply(lon1, lat1, lon2, lat2, spheroid);
0490
0491         return non_iterative_case(lon1, lat1, lon2, lat2, distance);
0492     }
0493
0494 protected:
0495
0496     template <typename CT>
0497     static inline auto apply(CT const& lo1, CT const& la1, //p1
0498                              CT const& lo2, CT const& la2, //p2
0499                              CT const& lo3, CT const& la3, //query point p3
0500                              Spheroid const& spheroid)
0501     {
0502         using inverse_dist_azimuth_type =
0503             typename FormulaPolicy::template inverse<CT, true, true, false, false, false>;
0504
0505         using inverse_dist_azimuth_reverse_type =
0506             typename FormulaPolicy::template inverse<CT, true, true, true, false, false>;
0507
0508         CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
0509
0510         result_type<CT> result;
0511
0512         // if the query points coincide with one of segments' endpoints
0513         if ((lo1 == lo3 && la1 == la3) || (lo2 == lo3 && la2 == la3))
0514         {
0515             result.lon = lo3;
0516             result.lat = la3;
0517             return result;
0518         }
0519
0520         // Constants
0521         //CT const f = geometry::formula::flattening<CT>(spheroid);
0522         CT const pi = math::pi<CT>();
0523         CT const half_pi = pi / CT(2);
0524         CT const c0 = CT(0);
0525
0526         CT lon1 = lo1;
0527         CT lat1 = la1;
0528         CT lon2 = lo2;
0529         CT lat2 = la2;
0530         CT const lon3 = lo3;
0531         CT const lat3 = la3;
0532
0533         if (lon1 > lon2)
0534         {
0535             std::swap(lon1, lon2);
0536             std::swap(lat1, lat2);
0537         }
0538
0539 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0540         std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>();
0541         std::cout << "," << lat1 * math::r2d<CT>();
0542         std::cout << "),(" << lon2 * math::r2d<CT>();
0543         std::cout << "," << lat2 * math::r2d<CT>();
0544         std::cout << ")\np=(" << lon3 * math::r2d<CT>();
0545         std::cout << "," << lat3 * math::r2d<CT>();
0546         std::cout << ")" << std::endl;
0547 #endif
0548
0549         //segment on equator
0550         //Note: antipodal points on equator does not define segment on equator
0551         //but pass by the pole
0552         CT const diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
0553
0554         using meridian_inverse = typename formula::meridian_inverse<CT>;
0555
0556         bool const meridian_not_crossing_pole =
0557             meridian_inverse::meridian_not_crossing_pole(lat1, lat2, diff);
0558
0559         bool const meridian_crossing_pole =
0560             meridian_inverse::meridian_crossing_pole(diff);
0561
0562         if (math::equals(lat1, c0) && math::equals(lat2, c0)
0563             && !meridian_crossing_pole)
0564         {
0565 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0566             std::cout << "Equatorial segment" << std::endl;
0567             std::cout << "segment=(" << lon1 * math::r2d<CT>();
0568             std::cout << "," << lat1 * math::r2d<CT>();
0569             std::cout << "),(" << lon2 * math::r2d<CT>();
0570             std::cout << "," << lat2 * math::r2d<CT>();
0571             std::cout << ")\np=(" << lon3 * math::r2d<CT>();
0572             std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
0573 #endif
0574             if (lon3 <= lon1)
0575             {
0576                 return non_iterative_case(lon3, lat3, lon1, lat1, spheroid);
0577             }
0578             if (lon3 >= lon2)
0579             {
0580                 return non_iterative_case(lon3, lat3, lon2, lat2, spheroid);
0581             }
0582             return non_iterative_case(lon3, lat3, lon3, lat1, spheroid);
0583         }
0584
0585         if ( (meridian_not_crossing_pole || meridian_crossing_pole )
0586             && std::abs(lat1) > std::abs(lat2))
0587         {
0588 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0589             std::cout << "Meridian segment not crossing pole" << std::endl;
0590 #endif
0591             std::swap(lat1,lat2);
0592         }
0593
0594         if (meridian_crossing_pole)
0595         {
0596 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0597             std::cout << "Meridian segment crossing pole" << std::endl;
0598 #endif
0599             CT const sign_non_zero = lat3 >= c0 ? 1 : -1;
0600
0601             auto const res13 = apply(lon1, lat1, lon1, half_pi * sign_non_zero, lon3, lat3, spheroid);
0602
0603             auto const res23 = apply(lon2, lat2, lon2, half_pi * sign_non_zero, lon3, lat3, spheroid);
0604
0605             return (res13.distance) < (res23.distance) ? res13 : res23;
0606         }
0607
0608         auto const res12 = inverse_dist_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
0609
0610         auto const res13 = inverse_dist_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid);
0611
0612         if (geometry::math::equals(res12.distance, c0))
0613         {
0614 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0615             std::cout << "Degenerate segment" << std::endl;
0616             std::cout << "distance between points="
0617                       << res13.distance << std::endl;
0618 #endif
0619             auto const res = meridian_inverse::apply(lon1, lat1, lon3, lat3, spheroid);
0620
0621             return non_iterative_case(lon3, lat3, lon1, lat2,
0622                 res.meridian ? res.distance : res13.distance);
0623         }
0624
0625         // Compute a12 (GEO)
0626         CT const a312 = res13.azimuth - res12.azimuth;
0627
0628         // TODO: meridian case optimization
0629         if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole)
0630         {
0631             auto const minmax_elem = std::minmax(lat1, lat2);
0632
0633             if (lat3 >= std::get<0>(minmax_elem) &&
0634                 lat3 <= std::get<1>(minmax_elem))
0635             {
0636 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0637                 std::cout << "Point on meridian segment" << std::endl;
0638 #endif
0639                 return non_iterative_case(lon3, lat3, lon3, lat3, c0);
0640             }
0641         }
0642
0643         CT const projection1 = cos( a312 ) * res13.distance / res12.distance;
0644
0645 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0646         std::cout << "a1=" << res12.azimuth * math::r2d<CT>() << std::endl;
0647         std::cout << "a13=" << res13.azimuth * math::r2d<CT>() << std::endl;
0648         std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
0649         std::cout << "cos(a312)=" << cos(a312) << std::endl;
0650         std::cout << "projection 1=" << projection1 << std::endl;
0651 #endif
0652
0653         if (projection1 < c0)
0654         {
0655 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0656             std::cout << "projection closer to p1" << std::endl;
0657 #endif
0658             // projection of p3 on geodesic spanned by segment (p1,p2) fall
0659             // outside of segment on the side of p1
0660
0661             return non_iterative_case(lon3, lat3, lon1, lat1, spheroid);
0662         }
0663
0664         auto const res23 = inverse_dist_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid);
0665
0666         CT const a321 = res23.azimuth - res12.reverse_azimuth + pi;
0667         CT const projection2 = cos( a321 ) * res23.distance / res12.distance;
0668
0669 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0670         std::cout << "a21=" << res12.reverse_azimuth * math::r2d<CT>()
0671                   << std::endl;
0672         std::cout << "a23=" << res23.azimuth * math::r2d<CT>() << std::endl;
0673         std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
0674         std::cout << "cos(a321)=" << cos(a321) << std::endl;
0675         std::cout << "projection 2=" << projection2 << std::endl;
0676 #endif
0677
0678         if (projection2 < c0)
0679         {
0680 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0681             std::cout << "projection closer to p2" << std::endl;
0682 #endif
0683             // projection of p3 on geodesic spanned by segment (p1,p2) fall
0684             // outside of segment on the side of p2
0685             return non_iterative_case(lon3, lat3, lon2, lat2, spheroid);
0686         }
0687
0688         // Guess s14 (SPHERICAL) aka along-track distance
0689         using point = geometry::model::point
0690             <
0691                 CT,
0692                 2,
0693                 geometry::cs::spherical_equatorial<geometry::radian>
0694             >;
0695
0696         point const p1(lon1, lat1);
0697         point const p2(lon2, lat2);
0698         point const p3(lon3, lat3);
0699
0700         using haversine_t = geometry::strategy::distance::haversine<CT>;
0701         using cross_track_t = geometry::strategy::distance::cross_track<void, haversine_t>;
0702
0703         cross_track_t const cross_track(earth_radius);
0704         CT const s34_sph = cross_track.apply(p3, p1, p2);
0705
0706         haversine_t const str(earth_radius);
0707         CT const s13_sph = str.apply(p1, p3);
0708
0709         //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
0710         CT const cos_frac = cos(s13_sph / earth_radius) / cos(s34_sph / earth_radius);
0711         CT const s14_sph = cos_frac >= 1 ? CT(0)
0712             : cos_frac <= -1 ? pi * earth_radius
0713             : acos(cos_frac) * earth_radius;
0714
0715         CT const a12_sph = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2);
0716
0717         auto const res = geometry::formula::spherical_direct<true, false>(lon1, lat1,
0718             s14_sph, a12_sph, srs::sphere<CT>(earth_radius));
0719
0720         // this is what postgis (version 2.5) returns
0721         // geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
0722         //                     ::apply(lon3, lat3, res.lon2, res.lat2, spheroid);
0723
0724 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0725         std::cout << "s34=" << s34_sph << std::endl;
0726         std::cout << "s13=" << res13.distance << std::endl;
0727         std::cout << "s14=" << s14_sph << std::endl;
0728         std::cout << "===============" << std::endl;
0729 #endif
0730
0731         // Update s14 (using Newton method)
0732
0733         if (Bisection)
0734         {
0735             bisection(lon1, lat1, lon2, lat2, lon3, lat3, spheroid,
0736                 res12.distance/2, res12.azimuth, result);
0737         }
0738         else
0739         {
0740             CT const s14_start = geometry::strategy::distance::geographic
0741                 <
0742                     FormulaPolicy,
0743                     Spheroid,
0744                     CT
0745                 >::apply(lon1, lat1, res.lon2, res.lat2, spheroid);
0746
0747             newton(lon1, lat1, lon2, lat2, lon3, lat3, spheroid, s14_start, res12.azimuth, result);
0748         }
0749
0750         return result;
0751     }
0752
0753     Spheroid m_spheroid;
0754 };
0755
0756 } // namespace detail
0757
0758 template
0759 <
0760     typename FormulaPolicy = strategy::andoyer,
0761     typename Spheroid = srs::spheroid<double>,
0762     typename CalculationType = void
0763 >
0764 class geographic_cross_track
0765     : public detail::geographic_cross_track
0766         <
0767             FormulaPolicy,
0768             Spheroid,
0769             CalculationType,
0770             false,
0771             false
0772         >
0773 {
0774     using base_t = detail::geographic_cross_track
0775         <
0776             FormulaPolicy,
0777             Spheroid,
0778             CalculationType,
0779             false,
0780             false
0781         >;
0782
0783 public :
0784     explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
0785         : base_t(spheroid)
0786         {}
0787 };
0788
0789 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
0790 namespace services
0791 {
0792
0793 //tags
0794 template <typename FormulaPolicy>
0795 struct tag<geographic_cross_track<FormulaPolicy> >
0796 {
0797     typedef strategy_tag_distance_point_segment type;
0798 };
0799
0800 template
0801 <
0802         typename FormulaPolicy,
0803         typename Spheroid
0804 >
0805 struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
0806 {
0807     typedef strategy_tag_distance_point_segment type;
0808 };
0809
0810 template
0811 <
0812     typename FormulaPolicy,
0813     typename Spheroid,
0814     typename CalculationType
0815 >
0816 struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0817 {
0818     typedef strategy_tag_distance_point_segment type;
0819 };
0820
0821 template
0822 <
0823     typename FormulaPolicy,
0824     typename Spheroid,
0825     typename CalculationType,
0826     bool Bisection,
0827     bool EnableClosestPoint
0828 >
0829 struct tag<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0830 {
0831     typedef strategy_tag_distance_point_segment type;
0832 };
0833
0834 //return types
0835 template <typename FormulaPolicy, typename P, typename PS>
0836 struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
0837     : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
0838 {};
0839
0840 template
0841 <
0842     typename FormulaPolicy,
0843     typename Spheroid,
0844     typename P,
0845     typename PS
0846 >
0847 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
0848     : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
0849 {};
0850
0851 template
0852 <
0853     typename FormulaPolicy,
0854     typename Spheroid,
0855     typename CalculationType,
0856     typename P,
0857     typename PS
0858 >
0859 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
0860     : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
0861 {};
0862
0863 template
0864 <
0865     typename FormulaPolicy,
0866     typename Spheroid,
0867     typename CalculationType,
0868     bool Bisection,
0869     bool EnableClosestPoint,
0870     typename P,
0871     typename PS
0872 >
0873 struct return_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
0874     : detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>::template return_type<P, PS>
0875 {};
0876
0877 //comparable types
0878 template
0879 <
0880     typename FormulaPolicy,
0881     typename Spheroid,
0882     typename CalculationType
0883 >
0884 struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0885 {
0886     typedef geographic_cross_track
0887         <
0888             FormulaPolicy, Spheroid, CalculationType
0889         >  type;
0890 };
0891
0892
0893 template
0894 <
0895     typename FormulaPolicy,
0896     typename Spheroid,
0897     typename CalculationType,
0898     bool Bisection,
0899     bool EnableClosestPoint
0900 >
0901 struct comparable_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0902 {
0903     typedef detail::geographic_cross_track
0904         <
0905             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
0906         >  type;
0907 };
0908
0909 template
0910 <
0911     typename FormulaPolicy,
0912     typename Spheroid,
0913     typename CalculationType
0914 >
0915 struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0916 {
0917 public :
0918     static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
0919     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
0920     {
0921         return strategy;
0922     }
0923 };
0924
0925 template
0926 <
0927     typename FormulaPolicy,
0928     typename Spheroid,
0929     typename CalculationType,
0930     bool Bisection,
0931     bool EnableClosestPoint
0932 >
0933 struct get_comparable<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0934 {
0935 public :
0936     static inline detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>
0937     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& strategy)
0938     {
0939         return strategy;
0940     }
0941 };
0942
0943
0944 template
0945 <
0946     typename FormulaPolicy,
0947     typename Spheroid,
0948     typename CalculationType,
0949     typename P,
0950     typename PS
0951 >
0952 struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
0953 {
0954 private :
0955     typedef typename geographic_cross_track
0956         <
0957             FormulaPolicy, Spheroid, CalculationType
0958         >::template return_type<P, PS>::type return_type;
0959 public :
0960     template <typename T>
0961     static inline return_type
0962     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
0963     {
0964         return distance;
0965     }
0966 };
0967
0968 template
0969 <
0970     typename FormulaPolicy,
0971     typename Spheroid,
0972     typename CalculationType,
0973     bool Bisection,
0974     bool EnableClosestPoint,
0975     typename P,
0976     typename PS
0977 >
0978 struct result_from_distance<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
0979 {
0980 private :
0981     typedef typename detail::geographic_cross_track
0982         <
0983             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
0984         >::template return_type<P, PS>::type return_type;
0985 public :
0986     template <typename T>
0987     static inline return_type
0988     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& , T const& distance)
0989     {
0990         return distance;
0991     }
0992 };
0993
0994
0995 template <typename Point, typename PointOfSegment>
0996 struct default_strategy
0997     <
0998         point_tag, segment_tag, Point, PointOfSegment,
0999         geographic_tag, geographic_tag
1000     >
1001 {
1002     typedef geographic_cross_track<> type;
1003 };
1004
1005
1006 template <typename PointOfSegment, typename Point>
1007 struct default_strategy
1008     <
1009         segment_tag, point_tag, PointOfSegment, Point,
1010         geographic_tag, geographic_tag
1011     >
1012 {
1013     typedef typename default_strategy
1014         <
1015             point_tag, segment_tag, Point, PointOfSegment,
1016             geographic_tag, geographic_tag
1017         >::type type;
1018 };
1019
1020 } // namespace services
1021 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
1022
1023 }} // namespace strategy::distance
1024
1025 }} // namespace boost::geometry
1026 #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP