strategies/geographic/distance_cross_track.hpp

0001 // Boost.Geometry (aka GGL, Generic Geometry Library)
0002
0003 // Copyright (c) 2023 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 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 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 lon3 = lo3;
0531         CT 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 diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
0553
0554         using meridian_inverse = typename formula::meridian_inverse<CT>;
0555
0556         bool meridian_not_crossing_pole =
0557             meridian_inverse::meridian_not_crossing_pole(lat1, lat2, diff);
0558
0559         bool 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 sign_non_zero = lat3 >= c0 ? 1 : -1;
0600
0601             auto res13 = apply(lon1, lat1, lon1, half_pi * sign_non_zero, lon3, lat3, spheroid);
0602
0603             auto 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 res12 = inverse_dist_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
0609
0610         auto 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 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 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 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 res23 = inverse_dist_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid);
0665
0666         CT a321 = res23.azimuth - res12.reverse_azimuth + pi;
0667         CT 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 p1 = point(lon1, lat1);
0697         point p2 = point(lon2, lat2);
0698         point p3 = point(lon3, lat3);
0699
0700         geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
0701         CT s34_sph = cross_track.apply(p3, p1, p2);
0702
0703         geometry::strategy::distance::haversine<CT> str(earth_radius);
0704         CT s13_sph = str.apply(p1, p3);
0705
0706         //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
0707         CT cos_frac = cos(s13_sph / earth_radius) / cos(s34_sph / earth_radius);
0708         CT s14_sph = cos_frac >= 1 ? CT(0)
0709             : cos_frac <= -1 ? pi * earth_radius
0710             : acos(cos_frac) * earth_radius;
0711
0712         CT const a12_sph = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2);
0713
0714         auto res = geometry::formula::spherical_direct<true, false>(lon1, lat1,
0715             s14_sph, a12_sph, srs::sphere<CT>(earth_radius));
0716
0717         // this is what postgis (version 2.5) returns
0718         // geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
0719         //                     ::apply(lon3, lat3, res.lon2, res.lat2, spheroid);
0720
0721 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
0722         std::cout << "s34=" << s34_sph << std::endl;
0723         std::cout << "s13=" << res13.distance << std::endl;
0724         std::cout << "s14=" << s14_sph << std::endl;
0725         std::cout << "===============" << std::endl;
0726 #endif
0727
0728         // Update s14 (using Newton method)
0729
0730         if (Bisection)
0731         {
0732             bisection(lon1, lat1, lon2, lat2, lon3, lat3, spheroid,
0733                 res12.distance/2, res12.azimuth, result);
0734         }
0735         else
0736         {
0737             CT s14_start = geometry::strategy::distance::geographic
0738                 <
0739                     FormulaPolicy,
0740                     Spheroid,
0741                     CT
0742                 >::apply(lon1, lat1, res.lon2, res.lat2, spheroid);
0743
0744             newton(lon1, lat1, lon2, lat2, lon3, lat3, spheroid, s14_start, res12.azimuth, result);
0745         }
0746
0747         return result;
0748     }
0749
0750     Spheroid m_spheroid;
0751 };
0752
0753 } // namespace detail
0754
0755 template
0756 <
0757     typename FormulaPolicy = strategy::andoyer,
0758     typename Spheroid = srs::spheroid<double>,
0759     typename CalculationType = void
0760 >
0761 class geographic_cross_track
0762     : public detail::geographic_cross_track
0763         <
0764             FormulaPolicy,
0765             Spheroid,
0766             CalculationType,
0767             false,
0768             false
0769         >
0770 {
0771     using base_t = detail::geographic_cross_track
0772         <
0773             FormulaPolicy,
0774             Spheroid,
0775             CalculationType,
0776             false,
0777             false
0778         >;
0779
0780 public :
0781     explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
0782         : base_t(spheroid)
0783         {}
0784 };
0785
0786 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
0787 namespace services
0788 {
0789
0790 //tags
0791 template <typename FormulaPolicy>
0792 struct tag<geographic_cross_track<FormulaPolicy> >
0793 {
0794     typedef strategy_tag_distance_point_segment type;
0795 };
0796
0797 template
0798 <
0799         typename FormulaPolicy,
0800         typename Spheroid
0801 >
0802 struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
0803 {
0804     typedef strategy_tag_distance_point_segment type;
0805 };
0806
0807 template
0808 <
0809     typename FormulaPolicy,
0810     typename Spheroid,
0811     typename CalculationType
0812 >
0813 struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0814 {
0815     typedef strategy_tag_distance_point_segment type;
0816 };
0817
0818 template
0819 <
0820     typename FormulaPolicy,
0821     typename Spheroid,
0822     typename CalculationType,
0823     bool Bisection,
0824     bool EnableClosestPoint
0825 >
0826 struct tag<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0827 {
0828     typedef strategy_tag_distance_point_segment type;
0829 };
0830
0831 //return types
0832 template <typename FormulaPolicy, typename P, typename PS>
0833 struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
0834     : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
0835 {};
0836
0837 template
0838 <
0839     typename FormulaPolicy,
0840     typename Spheroid,
0841     typename P,
0842     typename PS
0843 >
0844 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
0845     : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
0846 {};
0847
0848 template
0849 <
0850     typename FormulaPolicy,
0851     typename Spheroid,
0852     typename CalculationType,
0853     typename P,
0854     typename PS
0855 >
0856 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
0857     : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
0858 {};
0859
0860 template
0861 <
0862     typename FormulaPolicy,
0863     typename Spheroid,
0864     typename CalculationType,
0865     bool Bisection,
0866     bool EnableClosestPoint,
0867     typename P,
0868     typename PS
0869 >
0870 struct return_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
0871     : detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>::template return_type<P, PS>
0872 {};
0873
0874 //comparable types
0875 template
0876 <
0877     typename FormulaPolicy,
0878     typename Spheroid,
0879     typename CalculationType
0880 >
0881 struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0882 {
0883     typedef geographic_cross_track
0884         <
0885             FormulaPolicy, Spheroid, CalculationType
0886         >  type;
0887 };
0888
0889
0890 template
0891 <
0892     typename FormulaPolicy,
0893     typename Spheroid,
0894     typename CalculationType,
0895     bool Bisection,
0896     bool EnableClosestPoint
0897 >
0898 struct comparable_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0899 {
0900     typedef detail::geographic_cross_track
0901         <
0902             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
0903         >  type;
0904 };
0905
0906 template
0907 <
0908     typename FormulaPolicy,
0909     typename Spheroid,
0910     typename CalculationType
0911 >
0912 struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
0913 {
0914 public :
0915     static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
0916     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
0917     {
0918         return strategy;
0919     }
0920 };
0921
0922 template
0923 <
0924     typename FormulaPolicy,
0925     typename Spheroid,
0926     typename CalculationType,
0927     bool Bisection,
0928     bool EnableClosestPoint
0929 >
0930 struct get_comparable<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
0931 {
0932 public :
0933     static inline detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>
0934     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& strategy)
0935     {
0936         return strategy;
0937     }
0938 };
0939
0940
0941 template
0942 <
0943     typename FormulaPolicy,
0944     typename Spheroid,
0945     typename CalculationType,
0946     typename P,
0947     typename PS
0948 >
0949 struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
0950 {
0951 private :
0952     typedef typename geographic_cross_track
0953         <
0954             FormulaPolicy, Spheroid, CalculationType
0955         >::template return_type<P, PS>::type return_type;
0956 public :
0957     template <typename T>
0958     static inline return_type
0959     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
0960     {
0961         return distance;
0962     }
0963 };
0964
0965 template
0966 <
0967     typename FormulaPolicy,
0968     typename Spheroid,
0969     typename CalculationType,
0970     bool Bisection,
0971     bool EnableClosestPoint,
0972     typename P,
0973     typename PS
0974 >
0975 struct result_from_distance<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
0976 {
0977 private :
0978     typedef typename detail::geographic_cross_track
0979         <
0980             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
0981         >::template return_type<P, PS>::type return_type;
0982 public :
0983     template <typename T>
0984     static inline return_type
0985     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& , T const& distance)
0986     {
0987         return distance;
0988     }
0989 };
0990
0991
0992 template <typename Point, typename PointOfSegment>
0993 struct default_strategy
0994     <
0995         point_tag, segment_tag, Point, PointOfSegment,
0996         geographic_tag, geographic_tag
0997     >
0998 {
0999     typedef geographic_cross_track<> type;
1000 };
1001
1002
1003 template <typename PointOfSegment, typename Point>
1004 struct default_strategy
1005     <
1006         segment_tag, point_tag, PointOfSegment, Point,
1007         geographic_tag, geographic_tag
1008     >
1009 {
1010     typedef typename default_strategy
1011         <
1012             point_tag, segment_tag, Point, PointOfSegment,
1013             geographic_tag, geographic_tag
1014         >::type type;
1015 };
1016
1017 } // namespace services
1018 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
1019
1020 }} // namespace strategy::distance
1021
1022 }} // namespace boost::geometry
1023 #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP