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