ceph/src/boost/boost/geometry/strategies/geographic/distance_cross_track.hpp

   1 // Boost.Geometry (aka GGL, Generic Geometry Library)
   2
   3 // Copyright (c) 2016-2021, Oracle and/or its affiliates.
   4
   5 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
   6 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
   7
   8 // Use, modification and distribution is subject to the Boost Software License,
   9 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  10 // http://www.boost.org/LICENSE_1_0.txt)
  11
  12 #ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
  13 #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
  14
  15 #include <algorithm>
  16
  17 #include <boost/tuple/tuple.hpp>
  18 #include <boost/algorithm/minmax.hpp>
  19 #include <boost/config.hpp>
  20 #include <boost/concept_check.hpp>
  21
  22 #include <boost/geometry/core/cs.hpp>
  23 #include <boost/geometry/core/access.hpp>
  24 #include <boost/geometry/core/coordinate_promotion.hpp>
  25 #include <boost/geometry/core/radian_access.hpp>
  26 #include <boost/geometry/core/tags.hpp>
  27
  28 #include <boost/geometry/strategies/distance.hpp>
  29 #include <boost/geometry/strategies/concepts/distance_concept.hpp>
  30 #include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
  31 #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
  32 #include <boost/geometry/strategies/spherical/point_in_point.hpp>
  33 #include <boost/geometry/strategies/geographic/azimuth.hpp>
  34 #include <boost/geometry/strategies/geographic/distance.hpp>
  35 #include <boost/geometry/strategies/geographic/parameters.hpp>
  36 #include <boost/geometry/strategies/geographic/intersection.hpp>
  37
  38 #include <boost/geometry/formulas/vincenty_direct.hpp>
  39
  40 #include <boost/geometry/util/math.hpp>
  41 #include <boost/geometry/util/select_calculation_type.hpp>
  42 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
  43
  44 #include <boost/geometry/formulas/result_direct.hpp>
  45 #include <boost/geometry/formulas/mean_radius.hpp>
  46 #include <boost/geometry/formulas/spherical.hpp>
  47
  48 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  49 #include <boost/geometry/io/dsv/write.hpp>
  50 #endif
  51
  52 #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
  53 #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
  54 #endif
  55
  56 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  57 #include <iostream>
  58 #endif
  59
  60 namespace boost { namespace geometry
  61 {
  62
  63 namespace strategy { namespace distance
  64 {
  65 namespace detail
  66 {
  67
  68 template <bool EnableClosestPoint>
  69 struct set_result
  70 {
  71     template <typename CT, typename ResultType>
  72     static void apply(CT const& distance,
  73                       CT const&,
  74                       CT const&,
  75                       ResultType& result)
  76     {
  77         result.distance = distance;
  78     }
  79 };
  80
  81 template<>
  82 struct set_result<true>
  83 {
  84     template <typename CT, typename ResultType>
  85     static void apply(CT const&,
  86                       CT const& lon,
  87                       CT const& lat,
  88                       ResultType& result)
  89     {
  90         result.lon = lon;
  91         result.lat = lat;
  92     }
  93 };
  94
  95
  96 /*!
  97 \brief Strategy functor for distance point to segment calculation on ellipsoid
  98        Algorithm uses direct and inverse geodesic problems as subroutines.
  99        The algorithm approximates the distance by an iterative Newton method.
 100 \ingroup strategies
 101 \details Class which calculates the distance of a point to a segment, for points
 102 on the ellipsoid
 103 \see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
 104       https://arxiv.org/abs/1102.1215
 105 \tparam FormulaPolicy underlying point-point distance strategy
 106 \tparam Spheroid is the spheroidal model used
 107 \tparam CalculationType \tparam_calculation
 108 \tparam EnableClosestPoint computes the closest point on segment if true
 109 */
 110 template
 111 <
 112     typename FormulaPolicy = strategy::andoyer,
 113     typename Spheroid = srs::spheroid<double>,
 114     typename CalculationType = void,
 115     bool Bisection = false,
 116     bool EnableClosestPoint = false
 117 >
 118 class geographic_cross_track
 119 {
 120
 121 public:
 122
 123     geographic_cross_track() = default;
 124
 125     explicit geographic_cross_track(Spheroid const& spheroid)
 126         : m_spheroid(spheroid)
 127     {}
 128
 129     Spheroid const& model() const
 130     {
 131         return m_spheroid;
 132     }
 133
 134     template <typename Point, typename PointOfSegment>
 135     struct return_type
 136         : promote_floating_point
 137           <
 138               typename select_calculation_type
 139                   <
 140                       Point,
 141                       PointOfSegment,
 142                       CalculationType
 143                   >::type
 144           >
 145     {};
 146
 147     template <typename Point, typename PointOfSegment>
 148     auto apply(Point const& p,
 149                PointOfSegment const& sp1,
 150                PointOfSegment const& sp2) const
 151     {
 152         return apply(get_as_radian<0>(sp1), get_as_radian<1>(sp1),
 153                      get_as_radian<0>(sp2), get_as_radian<1>(sp2),
 154                      get_as_radian<0>(p), get_as_radian<1>(p),
 155                      m_spheroid).distance;
 156     }
 157
 158     // points on a meridian not crossing poles
 159     template <typename CT>
 160     inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const
 161     {
 162         using meridian_inverse = typename formula::meridian_inverse
 163         <
 164             CT,
 165             strategy::default_order<FormulaPolicy>::value
 166         >;
 167
 168         return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2, m_spheroid);
 169     }
 170
 171 private:
 172
 173     template <typename CT>
 174     struct result_type
 175     {
 176         result_type()
 177             : distance(0)
 178             , lon(0)
 179             , lat(0)
 180         {}
 181
 182         CT distance;
 183         CT lon;
 184         CT lat;
 185     };
 186
 187     template <typename CT>
 188     auto static inline normalize(CT const& g4, CT& der)
 189     {
 190         CT const pi = math::pi<CT>();
 191         if (g4 < -1.25*pi)//close to -270
 192         {
 193 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 194             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to -270" << std::endl;
 195 #endif
 196             return g4 + 1.5 * pi;
 197         }
 198         else if (g4 > 1.25*pi)//close to 270
 199         {
 200 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 201             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to 270" << std::endl;
 202 #endif
 203             der = -der;
 204             return - g4 + 1.5 * pi;
 205         }
 206         else if (g4 < 0 && g4 > -0.75*pi)//close to -90
 207         {
 208 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 209             std::cout << "g4=" << g4 * math::r2d<CT>() <<  ", close to -90" << std::endl;
 210 #endif
 211             der = -der;
 212             return -g4 - pi/2;
 213         }
 214         return g4 - pi/2;
 215     }
 216
 217
 218     template <typename CT>
 219     static void bisection(CT const& lon1, CT const& lat1, //p1
 220                           CT const& lon2, CT const& lat2, //p2
 221                           CT const& lon3, CT const& lat3, //query point p3
 222                           Spheroid const& spheroid,
 223                           CT const& s14_start, CT const& a12,
 224                           result_type<CT>& result)
 225     {
 226         using direct_distance_type =
 227             typename FormulaPolicy::template direct<CT, true, false, false, false>;
 228         using inverse_distance_type =
 229             typename FormulaPolicy::template inverse<CT, true, false, false, false, false>;
 230
 231         geometry::formula::result_direct<CT> res14;
 232
 233         int counter = 0; // robustness
 234         bool dist_improve = true;
 235
 236         CT pl_lon = lon1;
 237         CT pl_lat = lat1;
 238         CT pr_lon = lon2;
 239         CT pr_lat = lat2;
 240
 241         CT s14 = s14_start;
 242
 243         do {
 244             // Solve the direct problem to find p4 (GEO)
 245             res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
 246
 247 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 248             std::cout << "dist(pl,p3)="
 249                       << inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance
 250                       << std::endl;
 251             std::cout << "dist(pr,p3)="
 252                       << inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance
 253                       << std::endl;
 254 #endif
 255             CT const dist_l =
 256                 inverse_distance_type::apply(lon3, lat3, pl_lon, pl_lat, spheroid).distance;
 257             CT const dist_r =
 258                 inverse_distance_type::apply(lon3, lat3, pr_lon, pr_lat, spheroid).distance;
 259
 260             if (dist_l < dist_r)
 261             {
 262                 s14 -= inverse_distance_type::apply(res14.lon2, res14.lat2, pl_lon,
 263                     pl_lat, spheroid).distance/2;
 264                 pr_lon = res14.lon2;
 265                 pr_lat = res14.lat2;
 266             }
 267             else
 268             {
 269                 s14 += inverse_distance_type::apply(res14.lon2, res14.lat2, pr_lon,
 270                     pr_lat, spheroid).distance/2;
 271                 pl_lon = res14.lon2;
 272                 pl_lat = res14.lat2;
 273             }
 274
 275             CT new_distance = inverse_distance_type::apply(lon3, lat3, res14.lon2, res14.lat2,
 276                 spheroid).distance;
 277
 278             dist_improve = new_distance != result.distance;
 279
 280 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 281             std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
 282                          "," << res14.lat2 * math::r2d<CT>() << std::endl;
 283             std::cout << "pl=" << pl_lon * math::r2d<CT>() << ","
 284                       << pl_lat * math::r2d<CT>()<< std::endl;
 285             std::cout << "pr=" << pr_lon * math::r2d<CT>() << ","
 286                       << pr_lat * math::r2d<CT>() << std::endl;
 287             std::cout << "new_s14=" << s14 << std::endl;
 288             std::cout << std::setprecision(16) << "result.distance ="
 289                       << result.distance << std::endl;
 290             std::cout << std::setprecision(16) << "new_distance ="
 291                       << new_distance << std::endl;
 292             std::cout << "---------end of step " << counter
 293                       << std::endl<< std::endl;
 294             if (!dist_improve)
 295             {
 296                 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
 297             }
 298             if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
 299             {
 300                 std::cout << "Stop msg: counter" << std::endl;
 301             }
 302 #endif
 303             set_result<EnableClosestPoint>::apply(new_distance, res14.lon2, res14.lat2, result);
 304
 305         } while (dist_improve && counter++
 306                  < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
 307     }
 308
 309     template <typename CT>
 310     static void newton(CT const& lon1, CT const& lat1, //p1
 311                        CT const& lon2, CT const& lat2, //p2
 312                        CT const& lon3, CT const& lat3, //query point p3
 313                        Spheroid const& spheroid,
 314                        CT const& s14_start, CT const& a12,
 315                        result_type<CT>& result)
 316     {
 317         using inverse_distance_azimuth_quantities_type =
 318             typename FormulaPolicy::template inverse<CT, true, true, false, true, true>;
 319
 320         using inverse_dist_azimuth_type =
 321             typename FormulaPolicy::template inverse<CT, false, true, false, false, false>;
 322
 323         using direct_distance_type =
 324             typename FormulaPolicy::template direct<CT, true, false, false, false>;
 325
 326         CT const half_pi = math::pi<CT>() / CT(2);
 327         geometry::formula::result_direct<CT> res14;
 328         geometry::formula::result_inverse<CT> res34;
 329         res34.distance = -1;
 330
 331         int counter = 0; // robustness
 332         CT g4;
 333         CT delta_g4 = 0;
 334         bool dist_improve = true;
 335         CT s14 = s14_start;
 336
 337         do {
 338             auto prev_distance = res34.distance;
 339             auto prev_res = res14;
 340
 341             // Solve the direct problem to find p4 (GEO)
 342             res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
 343
 344             // Solve an inverse problem to find g4
 345             // g4 is the angle between segment (p1,p2) and segment (p3,p4)
 346             // that meet on p4 (GEO)
 347
 348             CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2,
 349                                                      lon2, lat2, spheroid).azimuth;
 350             res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2,
 351                                                                     lon3, lat3, spheroid);
 352             g4 = res34.azimuth - a4;
 353
 354             // cos(s14/earth_radius) is the spherical limit
 355             CT M43 = res34.geodesic_scale;
 356             CT m34 = res34.reduced_length;
 357
 358             if (m34 != 0)
 359             {
 360                 CT der = (M43 / m34) * sin(g4);
 361                 delta_g4 = normalize(g4, der);
 362                 s14 -= der != 0 ? delta_g4 / der : 0;
 363             }
 364
 365             dist_improve = prev_distance > res34.distance || prev_distance == -1;
 366
 367             if (dist_improve)
 368             {
 369                 set_result<EnableClosestPoint>::apply(res34.distance, res14.lon2, res14.lat2,
 370                                                       result);
 371             }
 372             else
 373             {
 374                 set_result<EnableClosestPoint>::apply(prev_distance, prev_res.lon2, prev_res.lat2,
 375                                                       result);
 376             }
 377
 378 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 379             std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
 380                          "," << res14.lat2 * math::r2d<CT>() << std::endl;
 381             std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
 382             std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
 383             std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl;
 384             std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>()  << std::endl;
 385             std::cout << "M43=" << M43 << std::endl;
 386             std::cout << "m34=" << m34 << std::endl;
 387             std::cout << "new_s14=" << s14 << std::endl;
 388             std::cout << std::setprecision(16) << "dist     ="
 389                       << res34.distance << std::endl;
 390             std::cout << "---------end of step " << counter
 391                       << std::endl<< std::endl;
 392             if (g4 == half_pi)
 393             {
 394                 std::cout << "Stop msg: g4 == half_pi" << std::endl;
 395             }
 396             if (!dist_improve)
 397             {
 398                 std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
 399             }
 400             if (delta_g4 == 0)
 401             {
 402                 std::cout << "Stop msg: delta_g4 == 0" << std::endl;
 403             }
 404             if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
 405             {
 406                 std::cout << "Stop msg: counter" << std::endl;
 407             }
 408 #endif
 409
 410         } while (g4 != half_pi
 411                  && dist_improve
 412                  && delta_g4 != 0
 413                  && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
 414 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 415         std::cout << "distance=" << res34.distance << std::endl;
 416
 417         std::cout << "s34(geo) ="
 418                   << inverse_distance_azimuth_quantities_type
 419                      ::apply(res14.lon2, res14.lat2,
 420                              lon3, lat3, spheroid).distance
 421                   << ", p4=(" << res14.lon2 * math::r2d<CT>() << ","
 422                               << res14.lat2 * math::r2d<CT>() << ")"
 423                   << std::endl;
 424
 425         CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid)
 426             .distance;
 427         CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid)
 428             .distance;
 429
 430         CT a4 = inverse_dist_azimuth_type::apply(res14.lon2, res14.lat2, lon2, lat2, spheroid)
 431             .azimuth;
 432
 433         geometry::formula::result_direct<CT> res4 =
 434             direct_distance_type::apply(res14.lon2, res14.lat2, .04, a4, spheroid);
 435
 436         CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3,
 437             lat3, spheroid).distance;
 438
 439         geometry::formula::result_direct<CT> res1 =
 440             direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
 441
 442         CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3,
 443             lat3, spheroid).distance;
 444
 445         std::cout << "s31=" << s31 << "\ns32=" << s32
 446                   << "\np4_plus=" << p4_plus << ", p4=("
 447                   << res4.lon2 * math::r2d<CT>() << ","
 448                   << res4.lat2 * math::r2d<CT>() << ")"
 449                   << "\np4_minus=" << p4_minus << ", p4=("
 450                   << res1.lon2 * math::r2d<CT>() << ","
 451                   << res1.lat2 * math::r2d<CT>() << ")"
 452                   << std::endl;
 453
 454         if (res34.distance <= p4_plus && res34.distance <= p4_minus)
 455         {
 456             std::cout << "Closest point computed" << std::endl;
 457         }
 458         else
 459         {
 460             std::cout << "There is a closer point nearby" << std::endl;
 461         }
 462 #endif
 463     }
 464
 465
 466     template <typename CT>
 467     static inline auto non_iterative_case(CT const& lon1, CT const& lat1, //p1
 468                                           CT const& lon2, CT const& lat2, //p2
 469                                           CT const& distance)
 470     {
 471         result_type<CT> result;
 472
 473         set_result<EnableClosestPoint>::apply(distance, lon2, lat2, result);
 474
 475         return result;
 476     }
 477
 478     template <typename CT>
 479     static inline auto non_iterative_case(CT const& lon1, CT const& lat1, //p1
 480                                           CT const& lon2, CT const& lat2, //p2
 481                                           Spheroid const& spheroid)
 482     {
 483         CT distance = geometry::strategy::distance::geographic
 484             <
 485                 FormulaPolicy,
 486                 Spheroid,
 487                 CT
 488             >::apply(lon1, lat1, lon2, lat2, spheroid);
 489
 490         return non_iterative_case(lon1, lat1, lon2, lat2, distance);
 491     }
 492
 493 protected:
 494
 495     template <typename CT>
 496     static inline auto apply(CT const& lo1, CT const& la1, //p1
 497                              CT const& lo2, CT const& la2, //p2
 498                              CT const& lo3, CT const& la3, //query point p3
 499                              Spheroid const& spheroid)
 500     {
 501         using inverse_dist_azimuth_type =
 502             typename FormulaPolicy::template inverse<CT, true, true, false, false, false>;
 503
 504         using inverse_dist_azimuth_reverse_type =
 505             typename FormulaPolicy::template inverse<CT, true, true, true, false, false>;
 506
 507         CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
 508
 509         result_type<CT> result;
 510
 511         // if the query points coincide with one of segments' endpoints
 512         if ((lo1 == lo3 && la1 == la3) || (lo2 == lo3 && la2 == la3))
 513         {
 514             result.lon = lo3;
 515             result.lat = la3;
 516             return result;
 517         }
 518
 519         // Constants
 520         //CT const f = geometry::formula::flattening<CT>(spheroid);
 521         CT const pi = math::pi<CT>();
 522         CT const half_pi = pi / CT(2);
 523         CT const c0 = CT(0);
 524
 525         CT lon1 = lo1;
 526         CT lat1 = la1;
 527         CT lon2 = lo2;
 528         CT lat2 = la2;
 529         CT lon3 = lo3;
 530         CT lat3 = la3;
 531
 532         if (lon1 > lon2)
 533         {
 534             std::swap(lon1, lon2);
 535             std::swap(lat1, lat2);
 536         }
 537
 538 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 539         std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>();
 540         std::cout << "," << lat1 * math::r2d<CT>();
 541         std::cout << "),(" << lon2 * math::r2d<CT>();
 542         std::cout << "," << lat2 * math::r2d<CT>();
 543         std::cout << ")\np=(" << lon3 * math::r2d<CT>();
 544         std::cout << "," << lat3 * math::r2d<CT>();
 545         std::cout << ")" << std::endl;
 546 #endif
 547
 548         //segment on equator
 549         //Note: antipodal points on equator does not define segment on equator
 550         //but pass by the pole
 551         CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
 552
 553         using meridian_inverse = typename formula::meridian_inverse<CT>;
 554
 555         bool meridian_not_crossing_pole =
 556             meridian_inverse::meridian_not_crossing_pole(lat1, lat2, diff);
 557
 558         bool meridian_crossing_pole =
 559             meridian_inverse::meridian_crossing_pole(diff);
 560
 561         if (math::equals(lat1, c0) && math::equals(lat2, c0)
 562             && !meridian_crossing_pole)
 563         {
 564 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 565             std::cout << "Equatorial segment" << std::endl;
 566             std::cout << "segment=(" << lon1 * math::r2d<CT>();
 567             std::cout << "," << lat1 * math::r2d<CT>();
 568             std::cout << "),(" << lon2 * math::r2d<CT>();
 569             std::cout << "," << lat2 * math::r2d<CT>();
 570             std::cout << ")\np=(" << lon3 * math::r2d<CT>();
 571             std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
 572 #endif
 573             if (lon3 <= lon1)
 574             {
 575                 return non_iterative_case(lon3, lat3, lon1, lat1, spheroid);
 576             }
 577             if (lon3 >= lon2)
 578             {
 579                 return non_iterative_case(lon3, lat3, lon2, lat2, spheroid);
 580             }
 581             return non_iterative_case(lon3, lat3, lon3, lat1, spheroid);
 582         }
 583
 584         if ( (meridian_not_crossing_pole || meridian_crossing_pole )
 585             && std::abs(lat1) > std::abs(lat2))
 586         {
 587 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 588             std::cout << "Meridian segment not crossing pole" << std::endl;
 589 #endif
 590             std::swap(lat1,lat2);
 591         }
 592
 593         if (meridian_crossing_pole)
 594         {
 595 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 596             std::cout << "Meridian segment crossing pole" << std::endl;
 597 #endif
 598             CT sign_non_zero = lat3 >= c0 ? 1 : -1;
 599
 600             auto res13 = apply(lon1, lat1, lon1, half_pi * sign_non_zero, lon3, lat3, spheroid);
 601
 602             auto res23 = apply(lon2, lat2, lon2, half_pi * sign_non_zero, lon3, lat3, spheroid);
 603
 604             return (res13.distance) < (res23.distance) ? res13 : res23;
 605         }
 606
 607         auto res12 = inverse_dist_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
 608
 609         auto res13 = inverse_dist_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid);
 610
 611         if (geometry::math::equals(res12.distance, c0))
 612         {
 613 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 614             std::cout << "Degenerate segment" << std::endl;
 615             std::cout << "distance between points="
 616                       << res13.distance << std::endl;
 617 #endif
 618             auto res = meridian_inverse::apply(lon1, lat1, lon3, lat3, spheroid);
 619
 620             return non_iterative_case(lon3, lat3, lon1, lat2,
 621                 res.meridian ? res.distance : res13.distance);
 622         }
 623
 624         // Compute a12 (GEO)
 625         CT a312 = res13.azimuth - res12.azimuth;
 626
 627         // TODO: meridian case optimization
 628         if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole)
 629         {
 630             auto const minmax_elem = std::minmax(lat1, lat2);
 631
 632             if (lat3 >= std::get<0>(minmax_elem) &&
 633                 lat3 <= std::get<1>(minmax_elem))
 634             {
 635 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 636                 std::cout << "Point on meridian segment" << std::endl;
 637 #endif
 638                 return non_iterative_case(lon3, lat3, lon3, lat3, c0);
 639             }
 640         }
 641
 642         CT projection1 = cos( a312 ) * res13.distance / res12.distance;
 643
 644 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 645         std::cout << "a1=" << res12.azimuth * math::r2d<CT>() << std::endl;
 646         std::cout << "a13=" << res13.azimuth * math::r2d<CT>() << std::endl;
 647         std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
 648         std::cout << "cos(a312)=" << cos(a312) << std::endl;
 649         std::cout << "projection 1=" << projection1 << std::endl;
 650 #endif
 651
 652         if (projection1 < c0)
 653         {
 654 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 655             std::cout << "projection closer to p1" << std::endl;
 656 #endif
 657             // projection of p3 on geodesic spanned by segment (p1,p2) fall
 658             // outside of segment on the side of p1
 659
 660             return non_iterative_case(lon3, lat3, lon1, lat1, spheroid);
 661         }
 662
 663         auto res23 = inverse_dist_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid);
 664
 665         CT a321 = res23.azimuth - res12.reverse_azimuth + pi;
 666         CT projection2 = cos( a321 ) * res23.distance / res12.distance;
 667
 668 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 669         std::cout << "a21=" << res12.reverse_azimuth * math::r2d<CT>()
 670                   << std::endl;
 671         std::cout << "a23=" << res23.azimuth * math::r2d<CT>() << std::endl;
 672         std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
 673         std::cout << "cos(a321)=" << cos(a321) << std::endl;
 674         std::cout << "projection 2=" << projection2 << std::endl;
 675 #endif
 676
 677         if (projection2 < c0)
 678         {
 679 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 680             std::cout << "projection closer to p2" << std::endl;
 681 #endif
 682             // projection of p3 on geodesic spanned by segment (p1,p2) fall
 683             // outside of segment on the side of p2
 684             return non_iterative_case(lon3, lat3, lon2, lat2, spheroid);
 685         }
 686
 687         // Guess s14 (SPHERICAL) aka along-track distance
 688         using point = geometry::model::point
 689             <
 690                 CT,
 691                 2,
 692                 geometry::cs::spherical_equatorial<geometry::radian>
 693             >;
 694
 695         point p1 = point(lon1, lat1);
 696         point p2 = point(lon2, lat2);
 697         point p3 = point(lon3, lat3);
 698
 699         geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
 700         CT s34_sph = cross_track.apply(p3, p1, p2);
 701
 702         geometry::strategy::distance::haversine<CT> str(earth_radius);
 703         CT s13_sph = str.apply(p1, p3);
 704
 705         //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
 706         CT cos_frac = cos(s13_sph / earth_radius) / cos(s34_sph / earth_radius);
 707         CT s14_sph = cos_frac >= 1 ? CT(0)
 708             : cos_frac <= -1 ? pi * earth_radius
 709             : acos(cos_frac) * earth_radius;
 710
 711         CT const a12_sph = geometry::formula::spherical_azimuth<>(lon1, lat1, lon2, lat2);
 712
 713         auto res = geometry::formula::spherical_direct<true, false>(lon1, lat1,
 714             s14_sph, a12_sph, srs::sphere<CT>(earth_radius));
 715
 716         // this is what postgis (version 2.5) returns
 717         // geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
 718         //                     ::apply(lon3, lat3, res.lon2, res.lat2, spheroid);
 719
 720 #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
 721         std::cout << "s34=" << s34_sph << std::endl;
 722         std::cout << "s13=" << res13.distance << std::endl;
 723         std::cout << "s14=" << s14_sph << std::endl;
 724         std::cout << "===============" << std::endl;
 725 #endif
 726
 727         // Update s14 (using Newton method)
 728
 729         if (Bisection)
 730         {
 731             bisection(lon1, lat1, lon2, lat2, lon3, lat3, spheroid,
 732                 res12.distance/2, res12.azimuth, result);
 733         }
 734         else
 735         {
 736             CT s14_start = geometry::strategy::distance::geographic
 737                 <
 738                     FormulaPolicy,
 739                     Spheroid,
 740                     CT
 741                 >::apply(lon1, lat1, res.lon2, res.lat2, spheroid);
 742
 743             newton(lon1, lat1, lon2, lat2, lon3, lat3, spheroid, s14_start, res12.azimuth, result);
 744         }
 745
 746         return result;
 747     }
 748
 749     Spheroid m_spheroid;
 750 };
 751
 752 } // namespace detail
 753
 754 template
 755 <
 756     typename FormulaPolicy = strategy::andoyer,
 757     typename Spheroid = srs::spheroid<double>,
 758     typename CalculationType = void
 759 >
 760 class geographic_cross_track
 761     : public detail::geographic_cross_track
 762         <
 763             FormulaPolicy,
 764             Spheroid,
 765             CalculationType,
 766             false,
 767             false
 768         >
 769 {
 770     using base_t = detail::geographic_cross_track
 771         <
 772             FormulaPolicy,
 773             Spheroid,
 774             CalculationType,
 775             false,
 776             false
 777         >;
 778
 779 public :
 780     explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
 781         : base_t(spheroid)
 782         {}
 783 };
 784
 785 #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
 786 namespace services
 787 {
 788
 789 //tags
 790 template <typename FormulaPolicy>
 791 struct tag<geographic_cross_track<FormulaPolicy> >
 792 {
 793     typedef strategy_tag_distance_point_segment type;
 794 };
 795
 796 template
 797 <
 798         typename FormulaPolicy,
 799         typename Spheroid
 800 >
 801 struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
 802 {
 803     typedef strategy_tag_distance_point_segment type;
 804 };
 805
 806 template
 807 <
 808     typename FormulaPolicy,
 809     typename Spheroid,
 810     typename CalculationType
 811 >
 812 struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
 813 {
 814     typedef strategy_tag_distance_point_segment type;
 815 };
 816
 817 template
 818 <
 819     typename FormulaPolicy,
 820     typename Spheroid,
 821     typename CalculationType,
 822     bool Bisection,
 823     bool EnableClosestPoint
 824 >
 825 struct tag<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
 826 {
 827     typedef strategy_tag_distance_point_segment type;
 828 };
 829
 830 //return types
 831 template <typename FormulaPolicy, typename P, typename PS>
 832 struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
 833     : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
 834 {};
 835
 836 template
 837 <
 838     typename FormulaPolicy,
 839     typename Spheroid,
 840     typename P,
 841     typename PS
 842 >
 843 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
 844     : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
 845 {};
 846
 847 template
 848 <
 849     typename FormulaPolicy,
 850     typename Spheroid,
 851     typename CalculationType,
 852     typename P,
 853     typename PS
 854 >
 855 struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
 856     : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
 857 {};
 858
 859 template
 860 <
 861     typename FormulaPolicy,
 862     typename Spheroid,
 863     typename CalculationType,
 864     bool Bisection,
 865     bool EnableClosestPoint,
 866     typename P,
 867     typename PS
 868 >
 869 struct return_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
 870     : detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>::template return_type<P, PS>
 871 {};
 872
 873 //comparable types
 874 template
 875 <
 876     typename FormulaPolicy,
 877     typename Spheroid,
 878     typename CalculationType
 879 >
 880 struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
 881 {
 882     typedef geographic_cross_track
 883         <
 884             FormulaPolicy, Spheroid, CalculationType
 885         >  type;
 886 };
 887
 888
 889 template
 890 <
 891     typename FormulaPolicy,
 892     typename Spheroid,
 893     typename CalculationType,
 894     bool Bisection,
 895     bool EnableClosestPoint
 896 >
 897 struct comparable_type<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
 898 {
 899     typedef detail::geographic_cross_track
 900         <
 901             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
 902         >  type;
 903 };
 904
 905 template
 906 <
 907     typename FormulaPolicy,
 908     typename Spheroid,
 909     typename CalculationType
 910 >
 911 struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
 912 {
 913 public :
 914     static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
 915     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
 916     {
 917         return strategy;
 918     }
 919 };
 920
 921 template
 922 <
 923     typename FormulaPolicy,
 924     typename Spheroid,
 925     typename CalculationType,
 926     bool Bisection,
 927     bool EnableClosestPoint
 928 >
 929 struct get_comparable<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> >
 930 {
 931 public :
 932     static inline detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>
 933     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& strategy)
 934     {
 935         return strategy;
 936     }
 937 };
 938
 939
 940 template
 941 <
 942     typename FormulaPolicy,
 943     typename Spheroid,
 944     typename CalculationType,
 945     typename P,
 946     typename PS
 947 >
 948 struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
 949 {
 950 private :
 951     typedef typename geographic_cross_track
 952         <
 953             FormulaPolicy, Spheroid, CalculationType
 954         >::template return_type<P, PS>::type return_type;
 955 public :
 956     template <typename T>
 957     static inline return_type
 958     apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
 959     {
 960         return distance;
 961     }
 962 };
 963
 964 template
 965 <
 966     typename FormulaPolicy,
 967     typename Spheroid,
 968     typename CalculationType,
 969     bool Bisection,
 970     bool EnableClosestPoint,
 971     typename P,
 972     typename PS
 973 >
 974 struct result_from_distance<detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>, P, PS>
 975 {
 976 private :
 977     typedef typename detail::geographic_cross_track
 978         <
 979             FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
 980         >::template return_type<P, PS>::type return_type;
 981 public :
 982     template <typename T>
 983     static inline return_type
 984     apply(detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& , T const& distance)
 985     {
 986         return distance;
 987     }
 988 };
 989
 990
 991 template <typename Point, typename PointOfSegment>
 992 struct default_strategy
 993     <
 994         point_tag, segment_tag, Point, PointOfSegment,
 995         geographic_tag, geographic_tag
 996     >
 997 {
 998     typedef geographic_cross_track<> type;
 999 };
1000
1001
1002 template <typename PointOfSegment, typename Point>
1003 struct default_strategy
1004     <
1005         segment_tag, point_tag, PointOfSegment, Point,
1006         geographic_tag, geographic_tag
1007     >
1008 {
1009     typedef typename default_strategy
1010         <
1011             point_tag, segment_tag, Point, PointOfSegment,
1012             geographic_tag, geographic_tag
1013         >::type type;
1014 };
1015
1016 } // namespace services
1017 #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
1018
1019 }} // namespace strategy::distance
1020
1021 }} // namespace boost::geometry
1022 #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP