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                                     iteration < max_iterations,
0375                                     dv, f, n, ep2, tiny, coeffs_C1);
0376
0377                     // Reversed test to allow escape with NaNs.
0378                     if (tripb || !(std::abs(v) >= (tripn ? c8 : c1) * tol0))
0379                         break;
0380
0381                     // Update bracketing values.
0382                     if (v > c0 && (iteration > max_iterations ||
0383                         cos_alpha1 / sin_alpha1 > cos_alpha1b / sin_alpha1b))
0384                     {
0385                         sin_alpha1b = sin_alpha1;
0386                         cos_alpha1b = cos_alpha1;
0387                     }
0388                     else if (v < c0 && (iteration > max_iterations ||
0389                              cos_alpha1 / sin_alpha1 < cos_alpha1a / sin_alpha1a))
0390                     {
0391                         sin_alpha1a = sin_alpha1;
0392                         cos_alpha1a = cos_alpha1;
0393                     }
0394
0395                     if (iteration < max_iterations && dv > c0)
0396                     {
0397                         CT diff_alpha1 = -v / dv;
0398
0399                         CT sin_diff_alpha1 = sin(diff_alpha1);
0400                         CT cos_diff_alpha1 = cos(diff_alpha1);
0401
0402                         CT nsin_alpha1 = sin_alpha1 * cos_diff_alpha1 +
0403                             cos_alpha1 * sin_diff_alpha1;
0404
0405                         if (nsin_alpha1 > c0 && std::abs(diff_alpha1) < pi)
0406                         {
0407                             cos_alpha1 = cos_alpha1 * cos_diff_alpha1 - sin_alpha1 * sin_diff_alpha1;
0408                             sin_alpha1 = nsin_alpha1;
0409                             math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0410
0411                             // In some regimes we don't get quadratic convergence because
0412                             // slope -> 0. So use convergence conditions based on epsilon
0413                             // instead of sqrt(epsilon).
0414                             tripn = std::abs(v) <= c16 * tol0;
0415                             continue;
0416                         }
0417                     }
0418
0419                     // Either dv was not positive or updated value was outside legal
0420                     // range. Use the midpoint of the bracket as the next estimate.
0421                     // This mechanism is not needed for the WGS84 ellipsoid, but it does
0422                     // catch problems with more eeccentric ellipsoids. Its efficacy is
0423                     // such for the WGS84 test set with the starting guess set to alp1 =
0424                     // 90deg:
0425                     // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
0426                     // WGS84 and random input: mean = 4.74, sd = 0.99
0427                     sin_alpha1 = (sin_alpha1a + sin_alpha1b) / c2;
0428                     cos_alpha1 = (cos_alpha1a + cos_alpha1b) / c2;
0429                     math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0430                     tripn = false;
0431                     tripb = (std::abs(sin_alpha1a - sin_alpha1) + (cos_alpha1a - cos_alpha1) < tol_bisection ||
0432                              std::abs(sin_alpha1 - sin_alpha1b) + (cos_alpha1 - cos_alpha1b) < tol_bisection);
0433                 }
0434
0435                 CT dummy;
0436                 se::coeffs_C1<SeriesOrder, CT> const coeffs_C1_eps(eps);
0437                 // Ensure that the reduced length and geodesic scale are computed in
0438                 // a "canonical" way, with the I2 integral.
0439                 meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
0440                                                    sin_sigma2, cos_sigma2, dn2,
0441                                                    cos_beta1, cos_beta2, s12x,
0442                                                    m12x, dummy, result.geodesic_scale,
0443                                                    M21, coeffs_C1_eps);
0444
0445                 m12x *= b;
0446                 s12x *= b;
0447             }
0448         }
0449
0450         if (swap_point < 0)
0451         {
0452             boost::core::invoke_swap(sin_alpha1, sin_alpha2);
0453             boost::core::invoke_swap(cos_alpha1, cos_alpha2);
0454             boost::core::invoke_swap(result.geodesic_scale, M21);
0455         }
0456
0457         sin_alpha1 *= swap_point * lon12_sign;
0458         cos_alpha1 *= swap_point * lat_sign;
0459
0460         sin_alpha2 *= swap_point * lon12_sign;
0461         cos_alpha2 *= swap_point * lat_sign;
0462
0463         if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength)
0464         {
0465             result.reduced_length = m12x;
0466         }
0467
0468         if BOOST_GEOMETRY_CONSTEXPR (CalcAzimuths)
0469         {
0470             if BOOST_GEOMETRY_CONSTEXPR (CalcFwdAzimuth)
0471             {
0472                 result.azimuth = atan2(sin_alpha1, cos_alpha1);
0473             }
0474
0475             if BOOST_GEOMETRY_CONSTEXPR (CalcRevAzimuth)
0476             {
0477                 result.reverse_azimuth = atan2(sin_alpha2, cos_alpha2);
0478             }
0479         }
0480
0481         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance)
0482         {
0483             result.distance = s12x;
0484         }
0485
0486         return result;
0487     }
0488
0489     template <typename CoeffsC1>
0490     static inline void meridian_length(CT const& epsilon, CT const& ep2, CT const& sigma12,
0491                                        CT const& sin_sigma1, CT const& cos_sigma1, CT const& dn1,
0492                                        CT const& sin_sigma2, CT const& cos_sigma2, CT const& dn2,
0493                                        CT const& cos_beta1, CT const& cos_beta2,
0494                                        CT& s12x, CT& m12x, CT& m0,
0495                                        CT& M12, CT& M21,
0496                                        CoeffsC1 const& coeffs_C1)
0497     {
0498         static CT const c1 = 1;
0499
0500         CT A12x = 0, J12 = 0;
0501         CT expansion_A1, expansion_A2;
0502
0503         // Evaluate the coefficients for C2.
0504         se::coeffs_C2<SeriesOrder, CT> coeffs_C2(epsilon);
0505
0506         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance || EnableReducedLength || EnableGeodesicScale)
0507         {
0508             // Find the coefficients for A1 by computing the
0509             // series expansion using Horner scehme.
0510             expansion_A1 = se::evaluate_A1<SeriesOrder>(epsilon);
0511
0512             if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0513             {
0514                 // Find the coefficients for A2 by computing the
0515                 // series expansion using Horner scehme.
0516                 expansion_A2 = se::evaluate_A2<SeriesOrder>(epsilon);
0517
0518                 A12x = expansion_A1 - expansion_A2;
0519                 expansion_A2 += c1;
0520             }
0521             expansion_A1 += c1;
0522         }
0523
0524         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance)
0525         {
0526             CT B1 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C1)
0527                   - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C1);
0528
0529             s12x = expansion_A1 * (sigma12 + B1);
0530
0531             if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0532             {
0533                 CT B2 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
0534                       - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2);
0535
0536                 J12 = A12x * sigma12 + (expansion_A1 * B1 - expansion_A2 * B2);
0537             }
0538         }
0539         else if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength || EnableGeodesicScale)
0540         {
0541             for (size_t i = 1; i <= SeriesOrder; ++i)
0542             {
0543                 coeffs_C2[i] = expansion_A1 * coeffs_C1[i] -
0544                                expansion_A2 * coeffs_C2[i];
0545             }
0546
0547             J12 = A12x * sigma12 +
0548                    (se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
0549                   - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2));
0550         }
0551
0552         if BOOST_GEOMETRY_CONSTEXPR (EnableReducedLength)
0553         {
0554             m0 = A12x;
0555
0556             m12x = dn2 * (cos_sigma1 * sin_sigma2) -
0557                    dn1 * (sin_sigma1 * cos_sigma2) -
0558                    cos_sigma1 * cos_sigma2 * J12;
0559         }
0560
0561         if BOOST_GEOMETRY_CONSTEXPR (EnableGeodesicScale)
0562         {
0563             CT cos_sigma12 = cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2;
0564             CT t = ep2 * (cos_beta1 - cos_beta2) *
0565                          (cos_beta1 + cos_beta2) / (dn1 + dn2);
0566
0567             M12 = cos_sigma12 + (t * sin_sigma2 - cos_sigma2 * J12) * sin_sigma1 / dn1;
0568             M21 = cos_sigma12 - (t * sin_sigma1 - cos_sigma1 * J12) * sin_sigma2 / dn2;
0569         }
0570     }
0571
0572     /*
0573      Return a starting point for Newton's method in sin_alpha1 and
0574      cos_alpha1 (function value is -1). If Newton's method
0575      doesn't need to be used, return also sin_alpha2 and
0576      cos_alpha2 and function value is sig12.
0577     */
0578     template <typename CoeffsC1>
0579     static inline CT newton_start(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
0580                                   CT const& sin_beta2, CT const& cos_beta2, CT dn2,
0581                                   CT const& lam12, CT const& sin_lam12, CT const& cos_lam12,
0582                                   CT& sin_alpha1, CT& cos_alpha1,
0583                                   CT& sin_alpha2, CT& cos_alpha2,
0584                                   CT& dnm, CoeffsC1 const& coeffs_C1, CT const& ep2,
0585                                   CT const& tol1, CT const& tol2, CT const& etol2, CT const& n,
0586                                   CT const& f)
0587     {
0588         static CT const c0 = 0;
0589         static CT const c0_01 = 0.01;
0590         static CT const c0_1 = 0.1;
0591         static CT const c0_5 = 0.5;
0592         static CT const c1 = 1;
0593         static CT const c2 = 2;
0594         static CT const c6 = 6;
0595         static CT const c1000 = 1000;
0596         static CT const pi = math::pi<CT>();
0597
0598         CT const one_minus_f = c1 - f;
0599         CT const x_thresh = c1000 * tol2;
0600
0601         // Return a starting point for Newton's method in sin_alpha1
0602         // and cos_alpha1 (function value is -1). If Newton's method
0603         // doesn't need to be used, return also sin_alpha2 and
0604         // cos_alpha2 and function value is sig12.
0605         CT sig12 = -c1;
0606
0607         // bet12 = bet2 - bet1 in [0, pi); beta12a = bet2 + bet1 in (-pi, 0]
0608         CT sin_beta12 = sin_beta2 * cos_beta1 - cos_beta2 * sin_beta1;
0609         CT cos_beta12 = cos_beta2 * cos_beta1 + sin_beta2 * sin_beta1;
0610
0611         CT sin_beta12a = sin_beta2 * cos_beta1 + cos_beta2 * sin_beta1;
0612
0613         bool shortline = cos_beta12 >= c0 && sin_beta12 < c0_5 &&
0614             cos_beta2 * lam12 < c0_5;
0615
0616         CT sin_omega12, cos_omega12;
0617
0618         if (shortline)
0619         {
0620             CT sin_beta_m2 = math::sqr(sin_beta1 + sin_beta2);
0621
0622             sin_beta_m2 /= sin_beta_m2 + math::sqr(cos_beta1 + cos_beta2);
0623             dnm = math::sqrt(c1 + ep2 * sin_beta_m2);
0624
0625             CT omega12 = lam12 / (one_minus_f * dnm);
0626
0627             sin_omega12 = sin(omega12);
0628             cos_omega12 = cos(omega12);
0629         }
0630         else
0631         {
0632             sin_omega12 = sin_lam12;
0633             cos_omega12 = cos_lam12;
0634         }
0635
0636         sin_alpha1 = cos_beta2 * sin_omega12;
0637         cos_alpha1 = cos_omega12 >= c0 ?
0638             sin_beta12 + cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 + cos_omega12) :
0639             sin_beta12a - cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 - cos_omega12);
0640
0641         CT sin_sigma12 = boost::math::hypot(sin_alpha1, cos_alpha1);
0642         CT cos_sigma12 = sin_beta1 * sin_beta2 + cos_beta1 * cos_beta2 * cos_omega12;
0643
0644         if (shortline && sin_sigma12 < etol2)
0645         {
0646             sin_alpha2 = cos_beta1 * sin_omega12;
0647             cos_alpha2 = sin_beta12 - cos_beta1 * sin_beta2 *
0648                 (cos_omega12 >= c0 ? math::sqr(sin_omega12) /
0649                 (c1 + cos_omega12) : c1 - cos_omega12);
0650
0651             math::normalize_unit_vector<CT>(sin_alpha2, cos_alpha2);
0652             // Set return value.
0653             sig12 = atan2(sin_sigma12, cos_sigma12);
0654         }
0655         // Skip astroid calculation if too eccentric.
0656         else if (std::abs(n) > c0_1 ||
0657                  cos_sigma12 >= c0 ||
0658                  sin_sigma12 >= c6 * std::abs(n) * pi *
0659                  math::sqr(cos_beta1))
0660         {
0661             // Nothing to do, zeroth order spherical approximation will do.
0662         }
0663         else
0664         {
0665             // Scale lam12 and bet2 to x, y coordinate system where antipodal
0666             // point is at origin and singular point is at y = 0, x = -1.
0667             CT lambda_scale, beta_scale;
0668
0669             CT y;
0670             volatile CT x;
0671
0672             CT lam12x = atan2(-sin_lam12, -cos_lam12);
0673             if (f >= c0)
0674             {
0675                 CT k2 = math::sqr(sin_beta1) * ep2;
0676                 CT eps = k2 / (c2 * (c1 + sqrt(c1 + k2)) + k2);
0677
0678                 se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
0679
0680                 CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
0681
0682                 lambda_scale = f * cos_beta1 * A3 * pi;
0683                 beta_scale = lambda_scale * cos_beta1;
0684
0685                 x = lam12x / lambda_scale;
0686                 y = sin_beta12a / beta_scale;
0687             }
0688             else
0689             {
0690                 CT cos_beta12a = cos_beta2 * cos_beta1 - sin_beta2 * sin_beta1;
0691                 CT beta12a = atan2(sin_beta12a, cos_beta12a);
0692
0693                 CT m12b = c0;
0694                 CT m0 = c1;
0695                 CT dummy;
0696                 meridian_length(n, ep2, pi + beta12a,
0697                                 sin_beta1, -cos_beta1, dn1,
0698                                 sin_beta2, cos_beta2, dn2,
0699                                 cos_beta1, cos_beta2, dummy,
0700                                 m12b, m0, dummy, dummy, coeffs_C1);
0701
0702                 x = -c1 + m12b / (cos_beta1 * cos_beta2 * m0 * pi);
0703                 beta_scale = x < -c0_01
0704                            ? sin_beta12a / x
0705                            : -f * math::sqr(cos_beta1) * pi;
0706                 lambda_scale = beta_scale / cos_beta1;
0707
0708                 y = lam12x / lambda_scale;
0709             }
0710
0711             if (y > -tol1 && x > -c1 - x_thresh)
0712             {
0713                 // Strip near cut.
0714                 if (f >= c0)
0715                 {
0716                     sin_alpha1 = (std::min)(c1, -CT(x));
0717                     cos_alpha1 = - math::sqrt(c1 - math::sqr(sin_alpha1));
0718                 }
0719                 else
0720                 {
0721                     cos_alpha1 = (std::max)(CT(x > -tol1 ? c0 : -c1), CT(x));
0722                     sin_alpha1 = math::sqrt(c1 - math::sqr(cos_alpha1));
0723                 }
0724             }
0725             else
0726             {
0727                 // Solve the astroid problem.
0728                 CT k = astroid(CT(x), y);
0729
0730                 CT omega12a = lambda_scale * (f >= c0 ? -x * k /
0731                     (c1 + k) : -y * (c1 + k) / k);
0732
0733                 sin_omega12 = sin(omega12a);
0734                 cos_omega12 = -cos(omega12a);
0735
0736                 // Update spherical estimate of alpha1 using omgega12 instead of lam12.
0737                 sin_alpha1 = cos_beta2 * sin_omega12;
0738                 cos_alpha1 = sin_beta12a - cos_beta2 * sin_beta1 *
0739                     math::sqr(sin_omega12) / (c1 - cos_omega12);
0740             }
0741         }
0742
0743         // Sanity check on starting guess. Backwards check allows NaN through.
0744         if (!(sin_alpha1 <= c0))
0745         {
0746             math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
0747         }
0748         else
0749         {
0750             sin_alpha1 = c1;
0751             cos_alpha1 = c0;
0752         }
0753
0754         return sig12;
0755     }
0756
0757     /*
0758      Solve the astroid problem using the equation:
0759      κ4 + 2κ3 + (1 − x2 − y 2 )κ2 − 2y 2 κ − y 2 = 0.
0760
0761      For details, please refer to Eq. (65) in,
0762      Geodesics on an ellipsoid of revolution, Charles F.F Karney,
0763      https://arxiv.org/abs/1102.1215
0764     */
0765     static inline CT astroid(CT const& x, CT const& y)
0766     {
0767         static CT const c0 = 0;
0768         static CT const c1 = 1;
0769         static CT const c2 = 2;
0770         static CT const c3 = 3;
0771         static CT const c4 = 4;
0772         static CT const c6 = 6;
0773
0774         CT k;
0775
0776         CT p = math::sqr(x);
0777         CT q = math::sqr(y);
0778         CT r = (p + q - c1) / c6;
0779
0780         if (!(q == c0 && r <= c0))
0781         {
0782             // Avoid possible division by zero when r = 0 by multiplying
0783             // equations for s and t by r^3 and r, respectively.
0784             CT S = p * q / c4;
0785             CT r2 = math::sqr(r);
0786             CT r3 = r * r2;
0787
0788             // The discriminant of the quadratic equation for T3. This is
0789             // zero on the evolute curve p^(1/3)+q^(1/3) = 1.
0790             CT discriminant = S * (S + c2 * r3);
0791
0792             CT u = r;
0793
0794             if (discriminant >= c0)
0795             {
0796                 CT T3 = S + r3;
0797
0798                 // Pick the sign on the sqrt to maximize abs(T3). This minimizes
0799                 // loss of precision due to cancellation. The result is unchanged
0800                 // because of the way the T is used in definition of u.
0801                 T3 += T3 < c0 ? -std::sqrt(discriminant) : std::sqrt(discriminant);
0802
0803                 CT T = std::cbrt(T3);
0804
0805                 // T can be zero; but then r2 / T -> 0.
0806                 u += T + (T != c0 ? r2 / T : c0);
0807             }
0808             else
0809             {
0810                 CT ang = std::atan2(std::sqrt(-discriminant), -(S + r3));
0811
0812                 // There are three possible cube roots. We choose the root which avoids
0813                 // cancellation. Note that discriminant < 0 implies that r < 0.
0814                 u += c2 * r * cos(ang / c3);
0815             }
0816
0817             CT v = std::sqrt(math::sqr(u) + q);
0818
0819             // Avoid loss of accuracy when u < 0.
0820             CT uv = u < c0 ? q / (v - u) : u + v;
0821             CT w = (uv - q) / (c2 * v);
0822
0823             // Rearrange expression for k to avoid loss of accuracy due to
0824             // subtraction. Division by 0 not possible because uv > 0, w >= 0.
0825             k = uv / (std::sqrt(uv + math::sqr(w)) + w);
0826         }
0827         else // q == 0 && r <= 0
0828         {
0829             // y = 0 with |x| <= 1. Handle this case directly.
0830             // For y small, positive root is k = abs(y)/sqrt(1-x^2).
0831             k = c0;
0832         }
0833         return k;
0834     }
0835
0836     template <typename CoeffsC1>
0837     static inline CT lambda12(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
0838                               CT const& sin_beta2, CT const& cos_beta2, CT const& dn2,
0839                               CT const& sin_alpha1, CT cos_alpha1,
0840                               CT const& sin_lam120, CT const& cos_lam120,
0841                               CT& sin_alpha2, CT& cos_alpha2,
0842                               CT& sigma12,
0843                               CT& sin_sigma1, CT& cos_sigma1,
0844                               CT& sin_sigma2, CT& cos_sigma2,
0845                               CT& eps, CT& diff_omega12,
0846                               bool diffp, CT& diff_lam12,
0847                               CT const& f, CT const& n, CT const& ep2, CT const& tiny,
0848                               CoeffsC1 const& coeffs_C1)
0849     {
0850         static CT const c0 = 0;
0851         static CT const c1 = 1;
0852         static CT const c2 = 2;
0853
0854         CT const one_minus_f = c1 - f;
0855
0856         if (sin_beta1 == c0 && cos_alpha1 == c0)
0857         {
0858             // Break degeneracy of equatorial line.
0859             cos_alpha1 = -tiny;
0860         }
0861
0862
0863         CT sin_alpha0 = sin_alpha1 * cos_beta1;
0864         CT cos_alpha0 = boost::math::hypot(cos_alpha1, sin_alpha1 * sin_beta1);
0865
0866         CT sin_omega1, cos_omega1;
0867         CT sin_omega2, cos_omega2;
0868         CT sin_omega12, cos_omega12;
0869
0870         CT lam12;
0871
0872         sin_sigma1 = sin_beta1;
0873         sin_omega1 = sin_alpha0 * sin_beta1;
0874
0875         cos_sigma1 = cos_omega1 = cos_alpha1 * cos_beta1;
0876
0877         math::normalize_unit_vector<CT>(sin_sigma1, cos_sigma1);
0878
0879         // Enforce symmetries in the case abs(beta2) = -beta1.
0880         // Otherwise, this can yield singularities in the Newton iteration.
0881
0882         // sin(alpha2) * cos(beta2) = sin(alpha0).
0883         sin_alpha2 = cos_beta2 != cos_beta1 ?
0884             sin_alpha0 / cos_beta2 : sin_alpha1;
0885
0886         cos_alpha2 = cos_beta2 != cos_beta1 || std::abs(sin_beta2) != -sin_beta1 ?
0887             sqrt(math::sqr(cos_alpha1 * cos_beta1) +
0888                 (cos_beta1 < -sin_beta1 ?
0889                     (cos_beta2 - cos_beta1) * (cos_beta1 + cos_beta2) :
0890                     (sin_beta1 - sin_beta2) * (sin_beta1 + sin_beta2))) / cos_beta2 :
0891             std::abs(cos_alpha1);
0892
0893         sin_sigma2 = sin_beta2;
0894         sin_omega2 = sin_alpha0 * sin_beta2;
0895
0896         cos_sigma2 = cos_omega2 =
0897             (cos_alpha2 * cos_beta2);
0898
0899         // Break degeneracy of equatorial line.
0900         math::normalize_unit_vector<CT>(sin_sigma2, cos_sigma2);
0901
0902
0903         // sig12 = sig2 - sig1, limit to [0, pi].
0904         sigma12 = atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
0905                                           cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);
0906
0907         // omg12 = omg2 - omg1, limit to [0, pi].
0908         sin_omega12 = (std::max)(c0, cos_omega1 * sin_omega2 - sin_omega1 * cos_omega2);
0909         cos_omega12 = cos_omega1 * cos_omega2 + sin_omega1 * sin_omega2;
0910
0911         // eta = omg12 - lam120.
0912         CT eta = atan2(sin_omega12 * cos_lam120 - cos_omega12 * sin_lam120,
0913                        cos_omega12 * cos_lam120 + sin_omega12 * sin_lam120);
0914
0915         CT B312;
0916         CT k2 = math::sqr(cos_alpha0) * ep2;
0917
0918         eps = k2 / (c2 * (c1 + std::sqrt(c1 + k2)) + k2);
0919
0920         se::coeffs_C3<SeriesOrder, CT> const coeffs_C3(n, eps);
0921
0922         B312 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C3)
0923              - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C3);
0924
0925         se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
0926
0927         CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
0928
0929         diff_omega12 = -f * A3 * sin_alpha0 * (sigma12 + B312);
0930         lam12 = eta + diff_omega12;
0931
0932         if (diffp)
0933         {
0934             if (cos_alpha2 == c0)
0935             {
0936                 diff_lam12 = - c2 * one_minus_f * dn1 / sin_beta1;
0937             }
0938             else
0939             {
0940                 CT dummy;
0941                 meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
0942                                                    sin_sigma2, cos_sigma2, dn2,
0943                                                    cos_beta1, cos_beta2, dummy,
0944                                                    diff_lam12, dummy, dummy,
0945                                                    dummy, coeffs_C1);
0946
0947                 diff_lam12 *= one_minus_f / (cos_alpha2 * cos_beta2);
0948             }
0949         }
0950         return lam12;
0951     }
0952
0953 };
0954
0955 } // namespace detail
0956
0957 /*!
0958 \brief The solution of the inverse problem of geodesics on latlong coordinates,
0959        after Karney (2011).
0960 \author See
0961 - Charles F.F Karney, Algorithms for geodesics, 2011
0962 https://arxiv.org/pdf/1109.4448.pdf
0963 */
0964
0965 template <
0966     typename CT,
0967     bool EnableDistance,
0968     bool EnableAzimuth,
0969     bool EnableReverseAzimuth = false,
0970     bool EnableReducedLength = false,
0971     bool EnableGeodesicScale = false
0972 >
0973 struct karney_inverse
0974     : detail::karney_inverse
0975         <
0976             CT,
0977             EnableDistance,
0978             EnableAzimuth,
0979             EnableReverseAzimuth,
0980             EnableReducedLength,
0981             EnableGeodesicScale
0982         >
0983 {};
0984
0985 }}} // namespace boost::geometry::formula
0986
0987
0988 #endif // BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP