geometry/formulas/karney_inverse.hpp

0001 // Boost.Geometry
0002
0003 // Copyright (c) 2018 Adeel Ahmad, Islamabad, Pakistan.
0004 // Copyright (c) 2023 Adam Wulkiewicz, Lodz, Poland.
0005
0006 // Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program.
0007
0008 // This file was modified by Oracle on 2019-2021.
0009 // Modifications copyright (c) 2019-2021 Oracle and/or its affiliates.
0010
0011 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
0012 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0013
0014 // Use, modification and distribution is subject to the Boost Software License,
0015 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0016 // http://www.boost.org/LICENSE_1_0.txt)
0017
0018 // This file is converted from GeographicLib, https://geographiclib.sourceforge.io
0019 // GeographicLib is originally written by Charles Karney.
0020
0021 // Author: Charles Karney (2008-2017)
0022
0023 // Last updated version of GeographicLib: 1.49
0024
0025 // Original copyright notice:
0026
0027 // Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
0028 // under the MIT/X11 License. For more information, see
0029 // https://geographiclib.sourceforge.io
0030
0031 #ifndef BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
0032 #define BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
0033
0034
0035 #include <boost/core/invoke_swap.hpp>
0036 #include <boost/math/constants/constants.hpp>
0037 #include <boost/math/special_functions/hypot.hpp>
0038
0039 #include <boost/geometry/util/constexpr.hpp>
0040 #include <boost/geometry/util/math.hpp>
0041 #include <boost/geometry/util/precise_math.hpp>
0042 #include <boost/geometry/util/series_expansion.hpp>
0043 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
0044
0045 #include <boost/geometry/formulas/flattening.hpp>
0046 #include <boost/geometry/formulas/result_inverse.hpp>
0047
0048
0049 namespace boost { namespace geometry { namespace math {
0050
0051 /*!
0052 \brief The exact difference of two angles reduced to (-180deg, 180deg].
0053 */
0054 template<typename T>
0055 inline T difference_angle(T const& x, T const& y, T& e)
0056 {
0057     auto res1 = boost::geometry::detail::precise_math::two_sum(
0058         std::remainder(-x, T(360)), std::remainder(y, T(360)));
0059
0060     normalize_azimuth<degree, T>(res1[0]);
0061
0062     // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
0063     // abs(t) <= eps (eps = 2^-45 for doubles).  The only case where the
0064     // addition of t takes the result outside the range (-180,180] is d = 180
0065     // and t > 0.  The case, d = -180 + eps, t = -eps, can't happen, since
0066     // sum_error would have returned the exact result in such a case (i.e., given t = 0).
0067     auto res2 = boost::geometry::detail::precise_math::two_sum(
0068         res1[0] == 180 && res1[1] > 0 ? -180 : res1[0], res1[1]);
0069     e = res2[1];
0070     return res2[0];
0071 }
0072
0073 }}} // namespace boost::geometry::math
0074
0075
0076 namespace boost { namespace geometry { namespace formula
0077 {
0078
0079 namespace se = series_expansion;
0080
0081 namespace detail
0082 {
0083
0084 template <
0085     typename CT,
0086     bool EnableDistance,
0087     bool EnableAzimuth,
0088     bool EnableReverseAzimuth = false,
0089     bool EnableReducedLength = false,
0090     bool EnableGeodesicScale = false,
0091     size_t SeriesOrder = 8
0092 >
0093 class karney_inverse
0094 {
0095     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
0096     static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
0097     static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
0098     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
0099
0100 public:
0101     typedef result_inverse<CT> result_type;
0102
0103     template <typename T1, typename T2, typename Spheroid>
0104     static inline result_type apply(T1 const& lo1,
0105                                     T1 const& la1,
0106                                     T2 const& lo2,
0107                                     T2 const& la2,
0108                                     Spheroid const& spheroid)
0109     {
0110         static CT const c0 = 0;
0111         static CT const c0_001 = 0.001;
0112         static CT const c0_1 = 0.1;
0113         static CT const c1 = 1;
0114         static CT const c2 = 2;
0115         static CT const c3 = 3;
0116         static CT const c8 = 8;
0117         static CT const c16 = 16;
0118         static CT const c90 = 90;
0119         static CT const c180 = 180;
0120         static CT const c200 = 200;
0121         static CT const pi = math::pi<CT>();
0122         static CT const d2r = math::d2r<CT>();
0123         static CT const r2d = math::r2d<CT>();
0124
0125         result_type result;
0126
0127         CT lat1 = la1 * r2d;
0128         CT lat2 = la2 * r2d;
0129
0130         CT lon1 = lo1 * r2d;
0131         CT lon2 = lo2 * r2d;
0132
0133         CT const a = CT(get_radius<0>(spheroid));
0134         CT const b = CT(get_radius<2>(spheroid));
0135         CT const f = formula::flattening<CT>(spheroid);
0136         CT const one_minus_f = c1 - f;
0137         CT const two_minus_f = c2 - f;
0138
0139         CT const tol0 = std::numeric_limits<CT>::epsilon();
0140         CT const tol1 = c200 * tol0;
0141         CT const tol2 = sqrt(tol0);
0142
0143         // Check on bisection interval.
0144         CT const tol_bisection = tol0 * tol2;
0145
0146         CT const etol2 = c0_1 * tol2 /
0147             sqrt((std::max)(c0_001, std::abs(f)) * (std::min)(c1, c1 - f / c2) / c2);
0148
0149         CT tiny = std::sqrt((std::numeric_limits<CT>::min)());
0150
0151         CT const n = f / two_minus_f;
0152         CT const e2 = f * two_minus_f;
0153         CT const ep2 = e2 / math::sqr(one_minus_f);
0154
0155         // Compute the longitudinal difference.
0156         CT lon12_error;
0157         CT lon12 = math::difference_angle(lon1, lon2, lon12_error);
0158
0159         int lon12_sign = lon12 >= 0 ? 1 : -1;
0160
0161         // Make points close to the meridian to lie on it.
0162         lon12 = lon12_sign * lon12;
0163         lon12_error = (c180 - lon12) - lon12_sign * lon12_error;
0164
0165         // Convert to radians.
0166         CT lam12 = lon12 * d2r;
0167         CT sin_lam12;
0168         CT cos_lam12;
0169
0170         if (lon12 > c90)
0171         {
0172             math::sin_cos_degrees(lon12_error, sin_lam12, cos_lam12);
0173             cos_lam12 *= -c1;
0174         }
0175         else
0176         {
0177             math::sin_cos_degrees(lon12, sin_lam12, cos_lam12);
0178         }
0179
0180         // Make points close to the equator to lie on it.
0181         lat1 = math::round_angle(std::abs(lat1) > c90 ? c90 : lat1);
0182         lat2 = math::round_angle(std::abs(lat2) > c90 ? c90 : lat2);
0183
0184         // Arrange points in a canonical form, as explained in
0185         // paper, Algorithms for geodesics, Eq. (44):
0186         //
0187         //     0 <= lon12 <= 180
0188         //     -90 <= lat1 <= 0
0189         //     lat1 <= lat2 <= -lat1
0190         int swap_point = std::abs(lat1) < std::abs(lat2) ? -1 : 1;
0191
0192         if (swap_point < 0)
0193         {
0194             lon12_sign *= -1;
0195             boost::core::invoke_swap(lat1, lat2);
0196         }
0197
0198         // Enforce lat1 to be <= 0.
0199         int lat_sign = lat1 < 0 ? 1 : -1;
0200         lat1 *= lat_sign;
0201         lat2 *= lat_sign;
0202
0203         CT sin_beta1, cos_beta1;
0204         math::sin_cos_degrees(lat1, sin_beta1, cos_beta1);
0205         sin_beta1 *= one_minus_f;
0206
0207         math::normalize_unit_vector<CT>(sin_beta1, cos_beta1);
0208         cos_beta1 = (std::max)(tiny, cos_beta1);
0209
0210         CT sin_beta2, cos_beta2;
0211         math::sin_cos_degrees(lat2, sin_beta2, cos_beta2);
0212         sin_beta2 *= one_minus_f;
0213
0214         math::normalize_unit_vector<CT>(sin_beta2, cos_beta2);
0215         cos_beta2 = (std::max)(tiny, cos_beta2);
0216
0217         // If cos_beta1 < -sin_beta1, then cos_beta2 - cos_beta1 is a
0218         // sensitive measure of the |beta1| - |beta2|. Alternatively,
0219         // (cos_beta1 >= -sin_beta1), abs(sin_beta2) + sin_beta1 is
0220         // a better measure.
0221         // Sometimes these quantities vanish and in that case we
0222         // force beta2 = +/- bet1a exactly.
0223         if (cos_beta1 < -sin_beta1)
0224         {
0225             if (cos_beta1 == cos_beta2)
0226             {
0227                 sin_beta2 = sin_beta2 < 0 ? sin_beta1 : -sin_beta1;
0228             }
0229         }
0230         else
0231         {
0232             if (std::abs(sin_beta2) == -sin_beta1)
0233             {
0234                 cos_beta2 = cos_beta1;
0235             }
0236         }
0237
0238         CT const dn1 = sqrt(c1 + ep2 * math::sqr(sin_beta1));
0239         CT const dn2 = sqrt(c1 + ep2 * math::sqr(sin_beta2));
0240
0241         CT sigma12;
0242         CT m12x = c0;
0243         CT s12x;
0244         CT M21;
0245
0246         // Index zero element of coeffs_C1 is unused.
0247         se::coeffs_C1<SeriesOrder, CT> const coeffs_C1(n);
0248
0249         bool meridian = lat1 == -90 || sin_lam12 == 0;
0250
0251         CT cos_alpha1, sin_alpha1;
0252         CT cos_alpha2, sin_alpha2;
0253
0254         if (meridian)
0255         {
0256             // Endpoints lie on a single full meridian.
0257
0258             // Point to the target latitude.
0259             cos_alpha1 = cos_lam12;
0260             sin_alpha1 = sin_lam12;
0261
0262             // Heading north at the target.
0263             cos_alpha2 = c1;
0264             sin_alpha2 = c0;
0265
0266             CT sin_sigma1 = sin_beta1;
0267             CT cos_sigma1 = cos_alpha1 * cos_beta1;
0268
0269             CT sin_sigma2 = sin_beta2;
0270             CT cos_sigma2 = cos_alpha2 * cos_beta2;
0271
0272             sigma12 = std::atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
0273                                                 cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);
0274
0275             CT dummy;
0276             meridian_length(n, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
0277                                              sin_sigma2, cos_sigma2, dn2,
0278                                              cos_beta1, cos_beta2, s12x,
0279                                              m12x, dummy, result.geodesic_scale,
0280                                              M21, coeffs_C1);
0281
0282             if (sigma12 < c1 || m12x >= c0)
0283             {
0284                 if (sigma12 < c3 * tiny)
0285                 {
0286                     sigma12 = m12x = s12x = c0;
0287                 }
0288
0289                 m12x *= b;
0290                 s12x *= b;
0291             }
0292             else
0293             {
0294                 // m12 < 0, i.e., prolate and too close to anti-podal.
0295                 meridian = false;
0296             }
0297         }
0298
0299         CT omega12;
0300
0301         if (!meridian && sin_beta1 == c0 &&
0302             (f <= c0 || lon12_error >= f * c180))
0303         {
0304             // Points lie on the equator.
0305             cos_alpha1 = cos_alpha2 = c0;
0306             sin_alpha1 = sin_alpha2 = c1;
0307
0308             s12x = a * lam12;
0309             sigma12 = omega12 = lam12 / one_minus_f;
0310             m12x = b * sin(sigma12);
0311
0312             if BOOST_GEOMETRY_CONSTEXPR (EnableGeodesicScale)
0313             {
0314                 result.geodesic_scale = cos(sigma12);
0315             }
0316         }
0317         else if (!meridian)
0318         {
0319             // If point1 and point2 belong within a hemisphere bounded by a
0320             // meridian and geodesic is neither meridional nor equatorial.
0321
0322             // Find the starting point for Newton's method.
0323             CT dnm = c1;
0324             sigma12 = newton_start(sin_beta1, cos_beta1, dn1,
0325                                    sin_beta2, cos_beta2, dn2,
0326                                    lam12, sin_lam12, cos_lam12,
0327                                    sin_alpha1, cos_alpha1,
0328                                    sin_alpha2, cos_alpha2,
0329                                    dnm, coeffs_C1, ep2,
0330                                    tol1, tol2, etol2,
0331                                    n, f);
0332
0333             if (sigma12 >= c0)
0334             {
0335                 // Short lines case (newton_start sets sin_alpha2, cos_alpha2, dnm).
0336                 s12x = sigma12 * b * dnm;
0337                 m12x = math::sqr(dnm) * b * sin(sigma12 / dnm);
0338                 if BOOST_GEOMETRY_CONSTEXPR (EnableGeodesicScale)
0339                 {
0340                     result.geodesic_scale = cos(sigma12 / dnm);
0341                 }
0342
0343                 // Convert to radians.
0344                 omega12 = lam12 / (one_minus_f * dnm);
0345             }
0346             else
0347             {
0348                 // Apply the Newton's method.
0349                 CT sin_sigma1 = c0, cos_sigma1 = c0;
0350                 CT sin_sigma2 = c0, cos_sigma2 = c0;
0351                 CT eps = c0, diff_omega12 = c0;
0352
0353                 // Bracketing range.
0354                 CT sin_alpha1a = tiny, cos_alpha1a = c1;
0355                 CT sin_alpha1b = tiny, cos_alpha1b = -c1;
0356
0357                 size_t iteration = 0;
0358                 size_t max_iterations = 20 + std::numeric_limits<size_t>::digits + 10;
0359
0360                 for (bool tripn = false, tripb = false;
0361                      iteration < max_iterations;
0362                      ++iteration)
0363                 {
0364                     CT dv = c0;
0365                     CT v = lambda12(sin_beta1, cos_beta1, dn1,
0366                                     sin_beta2, cos_beta2, dn2,
0367                                     sin_alpha1, cos_alpha1,
0368                                     sin_lam12, cos_lam12,
0369                                     sin_alpha2, cos_alpha2,
0370                                     sigma12,
0371                                     sin_sigma1, cos_sigma1,
0372                                     sin_sigma2, cos_sigma2,
0373                                     eps, diff_omega12,
0374                                     dv, f, n, ep2, tiny, coeffs_C1);
0375
0376                     // Reversed test to allow escape with NaNs.
0377                     if (tripb || !(std::abs(v) >= (tripn ? c8 : c1) * tol0))
0378                         break;
0379
0380                     // Update bracketing values.
0381                     if (v > c0 && (iteration > max_iterations ||
0382                         cos_alpha1 / sin_alpha1 > cos_alpha1b / sin_alpha1b))
0383                     {
0384                         sin_alpha1b = sin_alpha1;
0385                         cos_alpha1b = cos_alpha1;
0386                     }
0387                     else if (v < c0 && (iteration > max_iterations ||
0388                              cos_alpha1 / sin_alpha1 < cos_alpha1a / sin_alpha1a))
0389                     {
0390                         sin_alpha1a = sin_alpha1;
0391                         cos_alpha1a = cos_alpha1;
0392                     }
0393
0394                     if (dv > c0)
0395                     {
0396                         CT diff_alpha1 = -v / dv;
0397
0398                         CT sin_diff_alpha1 = sin(diff_alpha1);
0399                         CT cos_diff_alpha1 = cos(diff_alpha1);
0400
0401                         CT nsin_alpha1 = sin_alpha1 * cos_diff_alpha1 +
0402                             cos_alpha1 * sin_diff_alpha1;
0403
0404                         if (nsin_alpha1 > c0 && std::abs(diff_alpha1) < pi)
0405                         {
0406                             cos_alpha1 = cos_alpha1 * cos_diff_alpha1 - sin_alpha1 * sin_diff_alpha1;
0407                             sin_alpha1 = nsin_alpha1;
0408                             math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0409
0410                             // In some regimes we don't get quadratic convergence because
0411                             // slope -> 0. So use convergence conditions based on epsilon
0412                             // instead of sqrt(epsilon).
0413                             tripn = std::abs(v) <= c16 * tol0;
0414                             continue;
0415                         }
0416                     }
0417
0418                     // Either dv was not positive or updated value was outside legal
0419                     // range. Use the midpoint of the bracket as the next estimate.
0420                     // This mechanism is not needed for the WGS84 ellipsoid, but it does
0421                     // catch problems with more eeccentric ellipsoids. Its efficacy is
0422                     // such for the WGS84 test set with the starting guess set to alp1 =
0423                     // 90deg:
0424                     // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
0425                     // WGS84 and random input: mean = 4.74, sd = 0.99
0426                     sin_alpha1 = (sin_alpha1a + sin_alpha1b) / c2;
0427                     cos_alpha1 = (cos_alpha1a + cos_alpha1b) / c2;
0428                     math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0429                     tripn = false;
0430                     tripb = (std::abs(sin_alpha1a - sin_alpha1) + (cos_alpha1a - cos_alpha1) < tol_bisection ||
0431                              std::abs(sin_alpha1 - sin_alpha1b) + (cos_alpha1 - cos_alpha1b) < tol_bisection);
0432                 }
0433
0434                 CT dummy;
0435                 se::coeffs_C1<SeriesOrder, CT> const coeffs_C1_eps(eps);
0436                 // Ensure that the reduced length and geodesic scale are computed in
0437                 // a "canonical" way, with the I2 integral.
0438                 meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
0439                                                    sin_sigma2, cos_sigma2, dn2,
0440                                                    cos_beta1, cos_beta2, s12x,
0441                                                    m12x, dummy, result.geodesic_scale,
0442                                                    M21, coeffs_C1_eps);
0443
0444                 m12x *= b;
0445                 s12x *= b;
0446             }
0447         }
0448
0449         if (swap_point < 0)
0450         {
0451             boost::core::invoke_swap(sin_alpha1, sin_alpha2);
0452             boost::core::invoke_swap(cos_alpha1, cos_alpha2);
0453             boost::core::invoke_swap(result.geodesic_scale, M21);
0454         }
0455
0456         sin_alpha1 *= swap_point * lon12_sign;
0457         cos_alpha1 *= swap_point * lat_sign;
0458
0459         sin_alpha2 *= swap_point * lon12_sign;
0460         cos_alpha2 *= swap_point * lat_sign;
0461
0462         if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength)
0463         {
0464             result.reduced_length = m12x;
0465         }
0466
0467         if BOOST_GEOMETRY_CONSTEXPR (CalcAzimuths)
0468         {
0469             if BOOST_GEOMETRY_CONSTEXPR (CalcFwdAzimuth)
0470             {
0471                 result.azimuth = atan2(sin_alpha1, cos_alpha1);
0472             }
0473
0474             if BOOST_GEOMETRY_CONSTEXPR (CalcRevAzimuth)
0475             {
0476                 result.reverse_azimuth = atan2(sin_alpha2, cos_alpha2);
0477             }
0478         }
0479
0480         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance)
0481         {
0482             result.distance = s12x;
0483         }
0484
0485         return result;
0486     }
0487
0488     template <typename CoeffsC1>
0489     static inline void meridian_length(CT const& epsilon, CT const& ep2, CT const& sigma12,
0490                                        CT const& sin_sigma1, CT const& cos_sigma1, CT const& dn1,
0491                                        CT const& sin_sigma2, CT const& cos_sigma2, CT const& dn2,
0492                                        CT const& cos_beta1, CT const& cos_beta2,
0493                                        CT& s12x, CT& m12x, CT& m0,
0494                                        CT& M12, CT& M21,
0495                                        CoeffsC1 const& coeffs_C1)
0496     {
0497         static CT const c1 = 1;
0498
0499         CT A12x = 0, J12 = 0;
0500         CT expansion_A1, expansion_A2;
0501
0502         // Evaluate the coefficients for C2.
0503         se::coeffs_C2<SeriesOrder, CT> coeffs_C2(epsilon);
0504
0505         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance || EnableReducedLength || EnableGeodesicScale)
0506         {
0507             // Find the coefficients for A1 by computing the
0508             // series expansion using Horner scehme.
0509             expansion_A1 = se::evaluate_A1<SeriesOrder>(epsilon);
0510
0511             if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0512             {
0513                 // Find the coefficients for A2 by computing the
0514                 // series expansion using Horner scehme.
0515                 expansion_A2 = se::evaluate_A2<SeriesOrder>(epsilon);
0516
0517                 A12x = expansion_A1 - expansion_A2;
0518                 expansion_A2 += c1;
0519             }
0520             expansion_A1 += c1;
0521         }
0522
0523         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance)
0524         {
0525             CT B1 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C1)
0526                   - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C1);
0527
0528             s12x = expansion_A1 * (sigma12 + B1);
0529
0530             if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0531             {
0532                 CT B2 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
0533                       - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2);
0534
0535                 J12 = A12x * sigma12 + (expansion_A1 * B1 - expansion_A2 * B2);
0536             }
0537         }
0538         else if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0539         {
0540             for (size_t i = 1; i <= SeriesOrder; ++i)
0541             {
0542                 coeffs_C2[i] = expansion_A1 * coeffs_C1[i] -
0543                                expansion_A2 * coeffs_C2[i];
0544             }
0545
0546             J12 = A12x * sigma12 +
0547                    (se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
0548                   - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2));
0549         }
0550
0551         if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength)
0552         {
0553             m0 = A12x;
0554
0555             m12x = dn2 * (cos_sigma1 * sin_sigma2) -
0556                    dn1 * (sin_sigma1 * cos_sigma2) -
0557                    cos_sigma1 * cos_sigma2 * J12;
0558         }
0559
0560         if BOOST_GEOMETRY_CONSTEXPR (EnableGeodesicScale)
0561         {
0562             CT cos_sigma12 = cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2;
0563             CT t = ep2 * (cos_beta1 - cos_beta2) *
0564                          (cos_beta1 + cos_beta2) / (dn1 + dn2);
0565
0566             M12 = cos_sigma12 + (t * sin_sigma2 - cos_sigma2 * J12) * sin_sigma1 / dn1;
0567             M21 = cos_sigma12 - (t * sin_sigma1 - cos_sigma1 * J12) * sin_sigma2 / dn2;
0568         }
0569     }
0570
0571     /*
0572      Return a starting point for Newton's method in sin_alpha1 and
0573      cos_alpha1 (function value is -1). If Newton's method
0574      doesn't need to be used, return also sin_alpha2 and
0575      cos_alpha2 and function value is sig12.
0576     */
0577     template <typename CoeffsC1>
0578     static inline CT newton_start(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
0579                                   CT const& sin_beta2, CT const& cos_beta2, CT dn2,
0580                                   CT const& lam12, CT const& sin_lam12, CT const& cos_lam12,
0581                                   CT& sin_alpha1, CT& cos_alpha1,
0582                                   CT& sin_alpha2, CT& cos_alpha2,
0583                                   CT& dnm, CoeffsC1 const& coeffs_C1, CT const& ep2,
0584                                   CT const& tol1, CT const& tol2, CT const& etol2, CT const& n,
0585                                   CT const& f)
0586     {
0587         static CT const c0 = 0;
0588         static CT const c0_01 = 0.01;
0589         static CT const c0_1 = 0.1;
0590         static CT const c0_5 = 0.5;
0591         static CT const c1 = 1;
0592         static CT const c2 = 2;
0593         static CT const c6 = 6;
0594         static CT const c1000 = 1000;
0595         static CT const pi = math::pi<CT>();
0596
0597         CT const one_minus_f = c1 - f;
0598         CT const x_thresh = c1000 * tol2;
0599
0600         // Return a starting point for Newton's method in sin_alpha1
0601         // and cos_alpha1 (function value is -1). If Newton's method
0602         // doesn't need to be used, return also sin_alpha2 and
0603         // cos_alpha2 and function value is sig12.
0604         CT sig12 = -c1;
0605
0606         // bet12 = bet2 - bet1 in [0, pi); beta12a = bet2 + bet1 in (-pi, 0]
0607         CT sin_beta12 = sin_beta2 * cos_beta1 - cos_beta2 * sin_beta1;
0608         CT cos_beta12 = cos_beta2 * cos_beta1 + sin_beta2 * sin_beta1;
0609
0610         CT sin_beta12a = sin_beta2 * cos_beta1 + cos_beta2 * sin_beta1;
0611
0612         bool shortline = cos_beta12 >= c0 && sin_beta12 < c0_5 &&
0613             cos_beta2 * lam12 < c0_5;
0614
0615         CT sin_omega12, cos_omega12;
0616
0617         if (shortline)
0618         {
0619             CT sin_beta_m2 = math::sqr(sin_beta1 + sin_beta2);
0620
0621             sin_beta_m2 /= sin_beta_m2 + math::sqr(cos_beta1 + cos_beta2);
0622             dnm = math::sqrt(c1 + ep2 * sin_beta_m2);
0623
0624             CT omega12 = lam12 / (one_minus_f * dnm);
0625
0626             sin_omega12 = sin(omega12);
0627             cos_omega12 = cos(omega12);
0628         }
0629         else
0630         {
0631             sin_omega12 = sin_lam12;
0632             cos_omega12 = cos_lam12;
0633         }
0634
0635         sin_alpha1 = cos_beta2 * sin_omega12;
0636         cos_alpha1 = cos_omega12 >= c0 ?
0637             sin_beta12 + cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 + cos_omega12) :
0638             sin_beta12a - cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 - cos_omega12);
0639
0640         CT sin_sigma12 = boost::math::hypot(sin_alpha1, cos_alpha1);
0641         CT cos_sigma12 = sin_beta1 * sin_beta2 + cos_beta1 * cos_beta2 * cos_omega12;
0642
0643         if (shortline && sin_sigma12 < etol2)
0644         {
0645             sin_alpha2 = cos_beta1 * sin_omega12;
0646             cos_alpha2 = sin_beta12 - cos_beta1 * sin_beta2 *
0647                 (cos_omega12 >= c0 ? math::sqr(sin_omega12) /
0648                 (c1 + cos_omega12) : c1 - cos_omega12);
0649
0650             math::normalize_unit_vector<CT>(sin_alpha2, cos_alpha2);
0651             // Set return value.
0652             sig12 = atan2(sin_sigma12, cos_sigma12);
0653         }
0654         // Skip astroid calculation if too eccentric.
0655         else if (std::abs(n) > c0_1 ||
0656                  cos_sigma12 >= c0 ||
0657                  sin_sigma12 >= c6 * std::abs(n) * pi *
0658                  math::sqr(cos_beta1))
0659         {
0660             // Nothing to do, zeroth order spherical approximation will do.
0661         }
0662         else
0663         {
0664             // Scale lam12 and bet2 to x, y coordinate system where antipodal
0665             // point is at origin and singular point is at y = 0, x = -1.
0666             CT lambda_scale, beta_scale;
0667
0668             CT y;
0669             volatile CT x;
0670
0671             CT lam12x = atan2(-sin_lam12, -cos_lam12);
0672             if (f >= c0)
0673             {
0674                 CT k2 = math::sqr(sin_beta1) * ep2;
0675                 CT eps = k2 / (c2 * (c1 + sqrt(c1 + k2)) + k2);
0676
0677                 se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
0678
0679                 CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
0680
0681                 lambda_scale = f * cos_beta1 * A3 * pi;
0682                 beta_scale = lambda_scale * cos_beta1;
0683
0684                 x = lam12x / lambda_scale;
0685                 y = sin_beta12a / beta_scale;
0686             }
0687             else
0688             {
0689                 CT cos_beta12a = cos_beta2 * cos_beta1 - sin_beta2 * sin_beta1;
0690                 CT beta12a = atan2(sin_beta12a, cos_beta12a);
0691
0692                 CT m12b = c0;
0693                 CT m0 = c1;
0694                 CT dummy;
0695                 meridian_length(n, ep2, pi + beta12a,
0696                                 sin_beta1, -cos_beta1, dn1,
0697                                 sin_beta2, cos_beta2, dn2,
0698                                 cos_beta1, cos_beta2, dummy,
0699                                 m12b, m0, dummy, dummy, coeffs_C1);
0700
0701                 x = -c1 + m12b / (cos_beta1 * cos_beta2 * m0 * pi);
0702                 beta_scale = x < -c0_01
0703                            ? sin_beta12a / x
0704                            : -f * math::sqr(cos_beta1) * pi;
0705                 lambda_scale = beta_scale / cos_beta1;
0706
0707                 y = lam12x / lambda_scale;
0708             }
0709
0710             if (y > -tol1 && x > -c1 - x_thresh)
0711             {
0712                 // Strip near cut.
0713                 if (f >= c0)
0714                 {
0715                     sin_alpha1 = (std::min)(c1, -CT(x));
0716                     cos_alpha1 = - math::sqrt(c1 - math::sqr(sin_alpha1));
0717                 }
0718                 else
0719                 {
0720                     cos_alpha1 = (std::max)(CT(x > -tol1 ? c0 : -c1), CT(x));
0721                     sin_alpha1 = math::sqrt(c1 - math::sqr(cos_alpha1));
0722                 }
0723             }
0724             else
0725             {
0726                 // Solve the astroid problem.
0727                 CT k = astroid(CT(x), y);
0728
0729                 CT omega12a = lambda_scale * (f >= c0 ? -x * k /
0730                     (c1 + k) : -y * (c1 + k) / k);
0731
0732                 sin_omega12 = sin(omega12a);
0733                 cos_omega12 = -cos(omega12a);
0734
0735                 // Update spherical estimate of alpha1 using omgega12 instead of lam12.
0736                 sin_alpha1 = cos_beta2 * sin_omega12;
0737                 cos_alpha1 = sin_beta12a - cos_beta2 * sin_beta1 *
0738                     math::sqr(sin_omega12) / (c1 - cos_omega12);
0739             }
0740         }
0741
0742         // Sanity check on starting guess. Backwards check allows NaN through.
0743         if (!(sin_alpha1 <= c0))
0744         {
0745             math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0746         }
0747         else
0748         {
0749             sin_alpha1 = c1;
0750             cos_alpha1 = c0;
0751         }
0752
0753         return sig12;
0754     }
0755
0756     /*
0757      Solve the astroid problem using the equation:
0758      κ4 + 2κ3 + (1 − x2 − y 2 )κ2 − 2y 2 κ − y 2 = 0.
0759
0760      For details, please refer to Eq. (65) in,
0761      Geodesics on an ellipsoid of revolution, Charles F.F Karney,
0762      https://arxiv.org/abs/1102.1215
0763     */
0764     static inline CT astroid(CT const& x, CT const& y)
0765     {
0766         static CT const c0 = 0;
0767         static CT const c1 = 1;
0768         static CT const c2 = 2;
0769         static CT const c3 = 3;
0770         static CT const c4 = 4;
0771         static CT const c6 = 6;
0772
0773         CT k;
0774
0775         CT p = math::sqr(x);
0776         CT q = math::sqr(y);
0777         CT r = (p + q - c1) / c6;
0778
0779         if (!(q == c0 && r <= c0))
0780         {
0781             // Avoid possible division by zero when r = 0 by multiplying
0782             // equations for s and t by r^3 and r, respectively.
0783             CT S = p * q / c4;
0784             CT r2 = math::sqr(r);
0785             CT r3 = r * r2;
0786
0787             // The discriminant of the quadratic equation for T3. This is
0788             // zero on the evolute curve p^(1/3)+q^(1/3) = 1.
0789             CT discriminant = S * (S + c2 * r3);
0790
0791             CT u = r;
0792
0793             if (discriminant >= c0)
0794             {
0795                 CT T3 = S + r3;
0796
0797                 // Pick the sign on the sqrt to maximize abs(T3). This minimizes
0798                 // loss of precision due to cancellation. The result is unchanged
0799                 // because of the way the T is used in definition of u.
0800                 T3 += T3 < c0 ? -std::sqrt(discriminant) : std::sqrt(discriminant);
0801
0802                 CT T = std::cbrt(T3);
0803
0804                 // T can be zero; but then r2 / T -> 0.
0805                 u += T + (T != c0 ? r2 / T : c0);
0806             }
0807             else
0808             {
0809                 CT ang = std::atan2(std::sqrt(-discriminant), -(S + r3));
0810
0811                 // There are three possible cube roots. We choose the root which avoids
0812                 // cancellation. Note that discriminant < 0 implies that r < 0.
0813                 u += c2 * r * cos(ang / c3);
0814             }
0815
0816             CT v = std::sqrt(math::sqr(u) + q);
0817
0818             // Avoid loss of accuracy when u < 0.
0819             CT uv = u < c0 ? q / (v - u) : u + v;
0820             CT w = (uv - q) / (c2 * v);
0821
0822             // Rearrange expression for k to avoid loss of accuracy due to
0823             // subtraction. Division by 0 not possible because uv > 0, w >= 0.
0824             k = uv / (std::sqrt(uv + math::sqr(w)) + w);
0825         }
0826         else // q == 0 && r <= 0
0827         {
0828             // y = 0 with |x| <= 1. Handle this case directly.
0829             // For y small, positive root is k = abs(y)/sqrt(1-x^2).
0830             k = c0;
0831         }
0832         return k;
0833     }
0834
0835     template <typename CoeffsC1>
0836     static inline CT lambda12(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
0837                               CT const& sin_beta2, CT const& cos_beta2, CT const& dn2,
0838                               CT const& sin_alpha1, CT cos_alpha1,
0839                               CT const& sin_lam120, CT const& cos_lam120,
0840                               CT& sin_alpha2, CT& cos_alpha2,
0841                               CT& sigma12,
0842                               CT& sin_sigma1, CT& cos_sigma1,
0843                               CT& sin_sigma2, CT& cos_sigma2,
0844                               CT& eps, CT& diff_omega12,
0845                               CT& diff_lam12,
0846                               CT const& f, CT const& n, CT const& ep2, CT const& tiny,
0847                               CoeffsC1 const& coeffs_C1)
0848     {
0849         static CT const c0 = 0;
0850         static CT const c1 = 1;
0851         static CT const c2 = 2;
0852
0853         CT const one_minus_f = c1 - f;
0854
0855         if (sin_beta1 == c0 && cos_alpha1 == c0)
0856         {
0857             // Break degeneracy of equatorial line.
0858             cos_alpha1 = -tiny;
0859         }
0860
0861
0862         CT sin_alpha0 = sin_alpha1 * cos_beta1;
0863         CT cos_alpha0 = boost::math::hypot(cos_alpha1, sin_alpha1 * sin_beta1);
0864
0865         CT sin_omega1, cos_omega1;
0866         CT sin_omega2, cos_omega2;
0867         CT sin_omega12, cos_omega12;
0868
0869         CT lam12;
0870
0871         sin_sigma1 = sin_beta1;
0872         sin_omega1 = sin_alpha0 * sin_beta1;
0873
0874         cos_sigma1 = cos_omega1 = cos_alpha1 * cos_beta1;
0875
0876         math::normalize_unit_vector<CT>(sin_sigma1, cos_sigma1);
0877
0878         // Enforce symmetries in the case abs(beta2) = -beta1.
0879         // Otherwise, this can yield singularities in the Newton iteration.
0880
0881         // sin(alpha2) * cos(beta2) = sin(alpha0).
0882         sin_alpha2 = cos_beta2 != cos_beta1 ?
0883             sin_alpha0 / cos_beta2 : sin_alpha1;
0884
0885         cos_alpha2 = cos_beta2 != cos_beta1 || std::abs(sin_beta2) != -sin_beta1 ?
0886             sqrt(math::sqr(cos_alpha1 * cos_beta1) +
0887                 (cos_beta1 < -sin_beta1 ?
0888                     (cos_beta2 - cos_beta1) * (cos_beta1 + cos_beta2) :
0889                     (sin_beta1 - sin_beta2) * (sin_beta1 + sin_beta2))) / cos_beta2 :
0890             std::abs(cos_alpha1);
0891
0892         sin_sigma2 = sin_beta2;
0893         sin_omega2 = sin_alpha0 * sin_beta2;
0894
0895         cos_sigma2 = cos_omega2 =
0896             (cos_alpha2 * cos_beta2);
0897
0898         // Break degeneracy of equatorial line.
0899         math::normalize_unit_vector<CT>(sin_sigma2, cos_sigma2);
0900
0901
0902         // sig12 = sig2 - sig1, limit to [0, pi].
0903         sigma12 = atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
0904                                           cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);
0905
0906         // omg12 = omg2 - omg1, limit to [0, pi].
0907         sin_omega12 = (std::max)(c0, cos_omega1 * sin_omega2 - sin_omega1 * cos_omega2);
0908         cos_omega12 = cos_omega1 * cos_omega2 + sin_omega1 * sin_omega2;
0909
0910         // eta = omg12 - lam120.
0911         CT eta = atan2(sin_omega12 * cos_lam120 - cos_omega12 * sin_lam120,
0912                        cos_omega12 * cos_lam120 + sin_omega12 * sin_lam120);
0913
0914         CT B312;
0915         CT k2 = math::sqr(cos_alpha0) * ep2;
0916
0917         eps = k2 / (c2 * (c1 + std::sqrt(c1 + k2)) + k2);
0918
0919         se::coeffs_C3<SeriesOrder, CT> const coeffs_C3(n, eps);
0920
0921         B312 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C3)
0922              - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C3);
0923
0924         se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
0925
0926         CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
0927
0928         diff_omega12 = -f * A3 * sin_alpha0 * (sigma12 + B312);
0929         lam12 = eta + diff_omega12;
0930
0931         if (cos_alpha2 == c0)
0932         {
0933             diff_lam12 = - c2 * one_minus_f * dn1 / sin_beta1;
0934         }
0935         else
0936         {
0937             CT dummy;
0938             meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
0939                                                 sin_sigma2, cos_sigma2, dn2,
0940                                                 cos_beta1, cos_beta2, dummy,
0941                                                 diff_lam12, dummy, dummy,
0942                                                 dummy, coeffs_C1);
0943
0944             diff_lam12 *= one_minus_f / (cos_alpha2 * cos_beta2);
0945         }
0946
0947         return lam12;
0948     }
0949
0950 };
0951
0952 } // namespace detail
0953
0954 /*!
0955 \brief The solution of the inverse problem of geodesics on latlong coordinates,
0956        after Karney (2011).
0957 \author See
0958 - Charles F.F Karney, Algorithms for geodesics, 2011
0959 https://arxiv.org/pdf/1109.4448.pdf
0960 */
0961
0962 template <
0963     typename CT,
0964     bool EnableDistance,
0965     bool EnableAzimuth,
0966     bool EnableReverseAzimuth = false,
0967     bool EnableReducedLength = false,
0968     bool EnableGeodesicScale = false
0969 >
0970 struct karney_inverse
0971     : detail::karney_inverse
0972         <
0973             CT,
0974             EnableDistance,
0975             EnableAzimuth,
0976             EnableReverseAzimuth,
0977             EnableReducedLength,
0978             EnableGeodesicScale
0979         >
0980 {};
0981
0982 }}} // namespace boost::geometry::formula
0983
0984
0985 #endif // BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP