boost/geometry/formulas/sjoberg_intersection.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2016-2019 Oracle and/or its affiliates.
   4
   5 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
   6
   7 // Use, modification and distribution is subject to the Boost Software License,
   8 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
   9 // http://www.boost.org/LICENSE_1_0.txt)
  10
  11 #ifndef BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
  12 #define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
  13
  14
  15 #include <boost/math/constants/constants.hpp>
  16
  17 #include <boost/geometry/core/radius.hpp>
  18
  19 #include <boost/geometry/util/condition.hpp>
  20 #include <boost/geometry/util/math.hpp>
  21 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
  22
  23 #include <boost/geometry/formulas/flattening.hpp>
  24 #include <boost/geometry/formulas/spherical.hpp>
  25
  26
  27 namespace boost { namespace geometry { namespace formula
  28 {
  29
  30 /*!
  31 \brief The intersection of two great circles as proposed by Sjoberg.
  32 \see See
  33     - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
  34       http://link.springer.com/article/10.1007/s00190-001-0230-9
  35 */
  36 template <typename CT>
  37 struct sjoberg_intersection_spherical_02
  38 {
  39     // TODO: if it will be used as standalone formula
  40     //       support segments on equator and endpoints on poles
  41
  42     static inline bool apply(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
  43                              CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
  44                              CT & lon, CT & lat)
  45     {
  46         CT tan_lat = 0;
  47         bool res = apply_alt(lon1, lat1, lon_a2, lat_a2,
  48                              lon2, lat2, lon_b2, lat_b2,
  49                              lon, tan_lat);
  50
  51         if (res)
  52         {
  53             lat = atan(tan_lat);
  54         }
  55
  56         return res;
  57     }
  58
  59     static inline bool apply_alt(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
  60                                  CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
  61                                  CT & lon, CT & tan_lat)
  62     {
  63         CT const cos_lon1 = cos(lon1);
  64         CT const sin_lon1 = sin(lon1);
  65         CT const cos_lon2 = cos(lon2);
  66         CT const sin_lon2 = sin(lon2);
  67         CT const sin_lat1 = sin(lat1);
  68         CT const sin_lat2 = sin(lat2);
  69         CT const cos_lat1 = cos(lat1);
  70         CT const cos_lat2 = cos(lat2);
  71
  72         CT const tan_lat_a2 = tan(lat_a2);
  73         CT const tan_lat_b2 = tan(lat_b2);
  74
  75         return apply(lon1, lon_a2, lon2, lon_b2,
  76                      sin_lon1, cos_lon1, sin_lat1, cos_lat1,
  77                      sin_lon2, cos_lon2, sin_lat2, cos_lat2,
  78                      tan_lat_a2, tan_lat_b2,
  79                      lon, tan_lat);
  80     }
  81
  82 private:
  83     static inline bool apply(CT const& lon1, CT const& lon_a2, CT const& lon2, CT const& lon_b2,
  84                              CT const& sin_lon1, CT const& cos_lon1, CT const& sin_lat1, CT const& cos_lat1,
  85                              CT const& sin_lon2, CT const& cos_lon2, CT const& sin_lat2, CT const& cos_lat2,
  86                              CT const& tan_lat_a2, CT const& tan_lat_b2,
  87                              CT & lon, CT & tan_lat)
  88     {
  89         // NOTE:
  90         // cos_lat_ = 0 <=> segment on equator
  91         // tan_alpha_ = 0 <=> segment vertical
  92
  93         CT const tan_lat1 = sin_lat1 / cos_lat1; //tan(lat1);
  94         CT const tan_lat2 = sin_lat2 / cos_lat2; //tan(lat2);
  95
  96         CT const dlon1 = lon_a2 - lon1;
  97         CT const sin_dlon1 = sin(dlon1);
  98         CT const dlon2 = lon_b2 - lon2;
  99         CT const sin_dlon2 = sin(dlon2);
 100
 101         CT const cos_dlon1 = cos(dlon1);
 102         CT const cos_dlon2 = cos(dlon2);
 103
 104         CT const tan_alpha1_x = cos_lat1 * tan_lat_a2 - sin_lat1 * cos_dlon1;
 105         CT const tan_alpha2_x = cos_lat2 * tan_lat_b2 - sin_lat2 * cos_dlon2;
 106
 107         CT const c0 = 0;
 108         bool const is_vertical1 = math::equals(sin_dlon1, c0) || math::equals(tan_alpha1_x, c0);
 109         bool const is_vertical2 = math::equals(sin_dlon2, c0) || math::equals(tan_alpha2_x, c0);
 110
 111         CT tan_alpha1 = 0;
 112         CT tan_alpha2 = 0;
 113
 114         if (is_vertical1 && is_vertical2)
 115         {
 116             // circles intersect at one of the poles or are collinear
 117             return false;
 118         }
 119         else if (is_vertical1)
 120         {
 121             tan_alpha2 = sin_dlon2 / tan_alpha2_x;
 122
 123             lon = lon1;
 124         }
 125         else if (is_vertical2)
 126         {
 127             tan_alpha1 = sin_dlon1 / tan_alpha1_x;
 128
 129             lon = lon2;
 130         }
 131         else
 132         {
 133             tan_alpha1 = sin_dlon1 / tan_alpha1_x;
 134             tan_alpha2 = sin_dlon2 / tan_alpha2_x;
 135
 136             CT const T1 = tan_alpha1 * cos_lat1;
 137             CT const T2 = tan_alpha2 * cos_lat2;
 138             CT const T1T2 = T1*T2;
 139             CT const tan_lon_y = T1 * sin_lon2 - T2 * sin_lon1 + T1T2 * (tan_lat1 * cos_lon1 - tan_lat2 * cos_lon2);
 140             CT const tan_lon_x = T1 * cos_lon2 - T2 * cos_lon1 - T1T2 * (tan_lat1 * sin_lon1 - tan_lat2 * sin_lon2);
 141
 142             lon = atan2(tan_lon_y, tan_lon_x);
 143         }
 144
 145         // choose closer result
 146         CT const pi = math::pi<CT>();
 147         CT const lon_2 = lon > c0 ? lon - pi : lon + pi;
 148         CT const lon_dist1 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon),
 149                                                    math::longitude_difference<radian>(lon_a2, lon)),
 150                                         (std::min)(math::longitude_difference<radian>(lon2, lon),
 151                                                    math::longitude_difference<radian>(lon_b2, lon)));
 152         CT const lon_dist2 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon_2),
 153                                                    math::longitude_difference<radian>(lon_a2, lon_2)),
 154                                         (std::min)(math::longitude_difference<radian>(lon2, lon_2),
 155                                                    math::longitude_difference<radian>(lon_b2, lon_2)));
 156         if (lon_dist2 < lon_dist1)
 157         {
 158             lon = lon_2;
 159         }
 160
 161         CT const sin_lon = sin(lon);
 162         CT const cos_lon = cos(lon);
 163
 164         if (math::abs(tan_alpha1) >= math::abs(tan_alpha2)) // pick less vertical segment
 165         {
 166             CT const sin_dlon_1 = sin_lon * cos_lon1 - cos_lon * sin_lon1;
 167             CT const cos_dlon_1 = cos_lon * cos_lon1 + sin_lon * sin_lon1;
 168             CT const lat_y_1 = sin_dlon_1 + tan_alpha1 * sin_lat1 * cos_dlon_1;
 169             CT const lat_x_1 = tan_alpha1 * cos_lat1;
 170             tan_lat = lat_y_1 / lat_x_1;
 171         }
 172         else
 173         {
 174             CT const sin_dlon_2 = sin_lon * cos_lon2 - cos_lon * sin_lon2;
 175             CT const cos_dlon_2 = cos_lon * cos_lon2 + sin_lon * sin_lon2;
 176             CT const lat_y_2 = sin_dlon_2 + tan_alpha2 * sin_lat2 * cos_dlon_2;
 177             CT const lat_x_2 = tan_alpha2 * cos_lat2;
 178             tan_lat = lat_y_2 / lat_x_2;
 179         }
 180
 181         return true;
 182     }
 183 };
 184
 185
 186 /*! Approximation of dLambda_j [Sjoberg07], expanded into taylor series in e^2
 187     Maxima script:
 188     dLI_j(c_j, sinB_j, sinB) := integrate(1 / (sqrt(1 - c_j ^ 2 - x ^ 2)*(1 + sqrt(1 - e2*(1 - x ^ 2)))), x, sinB_j, sinB);
 189     dL_j(c_j, B_j, B) := -e2 * c_j * dLI_j(c_j, B_j, B);
 190     S: taylor(dLI_j(c_j, sinB_j, sinB), e2, 0, 3);
 191     assume(c_j < 1);
 192     assume(c_j > 0);
 193     L1: factor(integrate(sqrt(-x ^ 2 - c_j ^ 2 + 1) / (x ^ 2 + c_j ^ 2 - 1), x));
 194     L2: factor(integrate(((x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
 195     L3: factor(integrate(((x ^ 4 - 2 * x ^ 2 + 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
 196     L4: factor(integrate(((x ^ 6 - 3 * x ^ 4 + 3 * x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
 197
 198 \see See
 199     - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
 200       http://link.springer.com/article/10.1007/s00190-007-0204-7
 201 */
 202 template <unsigned int Order, typename CT>
 203 inline CT sjoberg_d_lambda_e_sqr(CT const& sin_betaj, CT const& sin_beta,
 204                                  CT const& Cj, CT const& sqrt_1_Cj_sqr,
 205                                  CT const& e_sqr)
 206 {
 207     using math::detail::bounded;
 208
 209     if (Order == 0)
 210     {
 211         return 0;
 212     }
 213
 214     CT const c1 = 1;
 215     CT const c2 = 2;
 216
 217     CT const asin_B = asin(bounded(sin_beta / sqrt_1_Cj_sqr, -c1, c1));
 218     CT const asin_Bj = asin(sin_betaj / sqrt_1_Cj_sqr);
 219     CT const L0 = (asin_B - asin_Bj) / c2;
 220
 221     if (Order == 1)
 222     {
 223         return -Cj * e_sqr * L0;
 224     }
 225
 226     CT const c0 = 0;
 227     CT const c16 = 16;
 228
 229     CT const X = sin_beta;
 230     CT const Xj = sin_betaj;
 231     CT const X_sqr = math::sqr(X);
 232     CT const Xj_sqr = math::sqr(Xj);
 233     CT const Cj_sqr = math::sqr(Cj);
 234     CT const Cj_sqr_plus_one = Cj_sqr + c1;
 235     CT const one_minus_Cj_sqr = c1 - Cj_sqr;
 236     CT const sqrt_Y = math::sqrt(bounded(-X_sqr + one_minus_Cj_sqr, c0));
 237     CT const sqrt_Yj = math::sqrt(-Xj_sqr + one_minus_Cj_sqr);
 238     CT const L1 = (Cj_sqr_plus_one * (asin_B - asin_Bj) + X * sqrt_Y - Xj * sqrt_Yj) / c16;
 239
 240     if (Order == 2)
 241     {
 242         return -Cj * e_sqr * (L0 + e_sqr * L1);
 243     }
 244
 245     CT const c3 = 3;
 246     CT const c5 = 5;
 247     CT const c128 = 128;
 248
 249     CT const E = Cj_sqr * (c3 * Cj_sqr + c2) + c3;
 250     CT const F = X * (-c2 * X_sqr + c3 * Cj_sqr + c5);
 251     CT const Fj = Xj * (-c2 * Xj_sqr + c3 * Cj_sqr + c5);
 252     CT const L2 = (E * (asin_B - asin_Bj) + F * sqrt_Y - Fj * sqrt_Yj) / c128;
 253
 254     if (Order == 3)
 255     {
 256         return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * L2));
 257     }
 258
 259     CT const c8 = 8;
 260     CT const c9 = 9;
 261     CT const c10 = 10;
 262     CT const c15 = 15;
 263     CT const c24 = 24;
 264     CT const c26 = 26;
 265     CT const c33 = 33;
 266     CT const c6144 = 6144;
 267
 268     CT const G = Cj_sqr * (Cj_sqr * (Cj_sqr * c15 + c9) + c9) + c15;
 269     CT const H = -c10 * Cj_sqr - c26;
 270     CT const I = Cj_sqr * (Cj_sqr * c15 + c24) + c33;
 271     CT const J = X_sqr * (X * (c8 * X_sqr + H)) + X * I;
 272     CT const Jj = Xj_sqr * (Xj * (c8 * Xj_sqr + H)) + Xj * I;
 273     CT const L3 = (G * (asin_B - asin_Bj) + J * sqrt_Y - Jj * sqrt_Yj) / c6144;
 274
 275     // Order 4 and higher
 276     return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * (L2 + e_sqr * L3)));
 277 }
 278
 279 /*!
 280 \brief The representation of geodesic as proposed by Sjoberg.
 281 \see See
 282     - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
 283       http://link.springer.com/article/10.1007/s00190-007-0204-7
 284     - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
 285       and the inverse geodetic problem by numerical integration, 2012
 286       https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
 287 */
 288 template <typename CT, unsigned int Order>
 289 class sjoberg_geodesic
 290 {
 291     sjoberg_geodesic() {}
 292
 293     static int sign_C(CT const& alphaj)
 294     {
 295         CT const c0 = 0;
 296         CT const c2 = 2;
 297         CT const pi = math::pi<CT>();
 298         CT const pi_half = pi / c2;
 299
 300         return (pi_half < alphaj && alphaj < pi) || (-pi_half < alphaj && alphaj < c0) ? -1 : 1;
 301     }
 302
 303 public:
 304     sjoberg_geodesic(CT const& lon, CT const& lat, CT const& alpha, CT const& f)
 305         : lonj(lon)
 306         , latj(lat)
 307         , alphaj(alpha)
 308     {
 309         CT const c0 = 0;
 310         CT const c1 = 1;
 311         CT const c2 = 2;
 312         //CT const pi = math::pi<CT>();
 313         //CT const pi_half = pi / c2;
 314
 315         one_minus_f = c1 - f;
 316         e_sqr = f * (c2 - f);
 317
 318         tan_latj = tan(lat);
 319         tan_betaj = one_minus_f * tan_latj;
 320         betaj = atan(tan_betaj);
 321         sin_betaj = sin(betaj);
 322
 323         cos_betaj = cos(betaj);
 324         sin_alphaj = sin(alphaj);
 325         // Clairaut constant (lower-case in the paper)
 326         Cj = sign_C(alphaj) * cos_betaj * sin_alphaj;
 327         Cj_sqr = math::sqr(Cj);
 328         sqrt_1_Cj_sqr = math::sqrt(c1 - Cj_sqr);
 329
 330         sign_lon_diff = alphaj >= 0 ? 1 : -1; // || alphaj == -pi ?
 331         //sign_lon_diff = 1;
 332
 333         is_on_equator = math::equals(sqrt_1_Cj_sqr, c0);
 334         is_Cj_zero = math::equals(Cj, c0);
 335
 336         t0j = c0;
 337         asin_tj_t0j = c0;
 338
 339         if (! is_Cj_zero)
 340         {
 341             t0j = sqrt_1_Cj_sqr / Cj;
 342         }
 343
 344         if (! is_on_equator)
 345         {
 346             //asin_tj_t0j = asin(tan_betaj / t0j);
 347             asin_tj_t0j = asin(tan_betaj * Cj / sqrt_1_Cj_sqr);
 348         }
 349     }
 350
 351     struct vertex_data
 352     {
 353         //CT beta0j;
 354         CT sin_beta0j;
 355         CT dL0j;
 356         CT lon0j;
 357     };
 358
 359     vertex_data get_vertex_data() const
 360     {
 361         CT const c2 = 2;
 362         CT const pi = math::pi<CT>();
 363         CT const pi_half = pi / c2;
 364
 365         vertex_data res;
 366
 367         if (! is_Cj_zero)
 368         {
 369             //res.beta0j = atan(t0j);
 370             //res.sin_beta0j = sin(res.beta0j);
 371             res.sin_beta0j = math::sign(t0j) * sqrt_1_Cj_sqr;
 372             res.dL0j = d_lambda(res.sin_beta0j);
 373             res.lon0j = lonj + sign_lon_diff * (pi_half - asin_tj_t0j + res.dL0j);
 374         }
 375         else
 376         {
 377             //res.beta0j = pi_half;
 378             //res.sin_beta0j = betaj >= 0 ? 1 : -1;
 379             res.sin_beta0j = 1;
 380             res.dL0j = 0;
 381             res.lon0j = lonj;
 382         }
 383
 384         return res;
 385     }
 386
 387     bool is_sin_beta_ok(CT const& sin_beta) const
 388     {
 389         CT const c1 = 1;
 390         return math::abs(sin_beta / sqrt_1_Cj_sqr) <= c1;
 391     }
 392
 393     bool k_diff(CT const& sin_beta,
 394                 CT & delta_k) const
 395     {
 396         if (is_Cj_zero)
 397         {
 398             delta_k = 0;
 399             return true;
 400         }
 401
 402         // beta out of bounds and not close
 403         if (! (is_sin_beta_ok(sin_beta)
 404                 || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
 405         {
 406             return false;
 407         }
 408
 409         // NOTE: beta may be slightly out of bounds here but d_lambda handles that
 410         CT const dLj = d_lambda(sin_beta);
 411         delta_k = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);
 412
 413         return true;
 414     }
 415
 416     bool lon_diff(CT const& sin_beta, CT const& t,
 417                   CT & delta_lon) const
 418     {
 419         using math::detail::bounded;
 420         CT const c1 = 1;
 421
 422         if (is_Cj_zero)
 423         {
 424             delta_lon = 0;
 425             return true;
 426         }
 427
 428         CT delta_k = 0;
 429         if (! k_diff(sin_beta, delta_k))
 430         {
 431             return false;
 432         }
 433
 434         CT const t_t0j = t / t0j;
 435         // NOTE: t may be slightly out of bounds here
 436         CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
 437         delta_lon = sign_lon_diff * asin_t_t0j + delta_k;
 438
 439         return true;
 440     }
 441
 442     bool k_diffs(CT const& sin_beta, vertex_data const& vd,
 443                  CT & delta_k_before, CT & delta_k_behind,
 444                  bool check_sin_beta = true) const
 445     {
 446         CT const pi = math::pi<CT>();
 447
 448         if (is_Cj_zero)
 449         {
 450             delta_k_before = 0;
 451             delta_k_behind = sign_lon_diff * pi;
 452             return true;
 453         }
 454
 455         // beta out of bounds and not close
 456         if (check_sin_beta
 457             && ! (is_sin_beta_ok(sin_beta)
 458                     || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
 459         {
 460             return false;
 461         }
 462
 463         // NOTE: beta may be slightly out of bounds here but d_lambda handles that
 464         CT const dLj = d_lambda(sin_beta);
 465         delta_k_before = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);
 466
 467         // This version require no additional dLj calculation
 468         delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + vd.dL0j + (vd.dL0j - dLj));
 469
 470         // [Sjoberg12]
 471         //CT const dL101 = d_lambda(sin_betaj, vd.sin_beta0j);
 472         // WARNING: the following call might not work if beta was OoB because only the second argument is bounded
 473         //CT const dL_01 = d_lambda(sin_beta, vd.sin_beta0j);
 474         //delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + dL101 + dL_01);
 475
 476         return true;
 477     }
 478
 479     bool lon_diffs(CT const& sin_beta, CT const& t, vertex_data const& vd,
 480                    CT & delta_lon_before, CT & delta_lon_behind) const
 481     {
 482         using math::detail::bounded;
 483         CT const c1 = 1;
 484         CT const pi = math::pi<CT>();
 485
 486         if (is_Cj_zero)
 487         {
 488             delta_lon_before = 0;
 489             delta_lon_behind = sign_lon_diff * pi;
 490             return true;
 491         }
 492
 493         CT delta_k_before = 0, delta_k_behind = 0;
 494         if (! k_diffs(sin_beta, vd, delta_k_before, delta_k_behind))
 495         {
 496             return false;
 497         }
 498
 499         CT const t_t0j = t / t0j;
 500         // NOTE: t may be slightly out of bounds here
 501         CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
 502         CT const sign_asin_t_t0j = sign_lon_diff * asin_t_t0j;
 503         delta_lon_before = sign_asin_t_t0j + delta_k_before;
 504         delta_lon_behind = -sign_asin_t_t0j + delta_k_behind;
 505
 506         return true;
 507     }
 508
 509     bool lon(CT const& sin_beta, CT const& t, vertex_data const& vd,
 510              CT & lon_before, CT & lon_behind) const
 511     {
 512         using math::detail::bounded;
 513         CT const c1 = 1;
 514         CT const pi = math::pi<CT>();
 515
 516         if (is_Cj_zero)
 517         {
 518             lon_before = lonj;
 519             lon_behind = lonj + sign_lon_diff * pi;
 520             return true;
 521         }
 522
 523         if (! (is_sin_beta_ok(sin_beta)
 524                 || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
 525         {
 526             return false;
 527         }
 528
 529         CT const t_t0j = t / t0j;
 530         CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
 531         CT const dLj = d_lambda(sin_beta);
 532         lon_before = lonj + sign_lon_diff * (asin_t_t0j - asin_tj_t0j + dLj);
 533         lon_behind = vd.lon0j + (vd.lon0j - lon_before);
 534
 535         return true;
 536     }
 537
 538
 539     CT lon(CT const& delta_lon) const
 540     {
 541         return lonj + delta_lon;
 542     }
 543
 544     CT lat(CT const& t) const
 545     {
 546         // t = tan(beta) = (1-f)tan(lat)
 547         return atan(t / one_minus_f);
 548     }
 549
 550     void vertex(CT & lon, CT & lat) const
 551     {
 552         lon = get_vertex_data().lon0j;
 553         if (! is_Cj_zero)
 554         {
 555             lat = sjoberg_geodesic::lat(t0j);
 556         }
 557         else
 558         {
 559             CT const c2 = 2;
 560             lat = math::pi<CT>() / c2;
 561         }
 562     }
 563
 564     CT lon_of_equator_intersection() const
 565     {
 566         CT const c0 = 0;
 567         CT const dLj = d_lambda(c0);
 568         CT const asin_tj_t0j = asin(Cj * tan_betaj / sqrt_1_Cj_sqr);
 569         return lonj - asin_tj_t0j + dLj;
 570     }
 571
 572     CT d_lambda(CT const& sin_beta) const
 573     {
 574         return sjoberg_d_lambda_e_sqr<Order>(sin_betaj, sin_beta, Cj, sqrt_1_Cj_sqr, e_sqr);
 575     }
 576
 577     // [Sjoberg12]
 578     /*CT d_lambda(CT const& sin_beta1, CT const& sin_beta2) const
 579     {
 580         return sjoberg_d_lambda_e_sqr<Order>(sin_beta1, sin_beta2, Cj, sqrt_1_Cj_sqr, e_sqr);
 581     }*/
 582
 583     CT lonj;
 584     CT latj;
 585     CT alphaj;
 586
 587     CT one_minus_f;
 588     CT e_sqr;
 589
 590     CT tan_latj;
 591     CT tan_betaj;
 592     CT betaj;
 593     CT sin_betaj;
 594     CT cos_betaj;
 595     CT sin_alphaj;
 596     CT Cj;
 597     CT Cj_sqr;
 598     CT sqrt_1_Cj_sqr;
 599
 600     int sign_lon_diff;
 601
 602     bool is_on_equator;
 603     bool is_Cj_zero;
 604
 605     CT t0j;
 606     CT asin_tj_t0j;
 607 };
 608
 609
 610 /*!
 611 \brief The intersection of two geodesics as proposed by Sjoberg.
 612 \see See
 613     - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
 614       http://link.springer.com/article/10.1007/s00190-001-0230-9
 615     - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
 616       http://link.springer.com/article/10.1007/s00190-007-0204-7
 617     - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
 618       and the inverse geodetic problem by numerical integration, 2012
 619       https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
 620 */
 621 template
 622 <
 623     typename CT,
 624     template <typename, bool, bool, bool, bool, bool> class Inverse,
 625     unsigned int Order = 4
 626 >
 627 class sjoberg_intersection
 628 {
 629     typedef sjoberg_geodesic<CT, Order> geodesic_type;
 630     typedef Inverse<CT, false, true, false, false, false> inverse_type;
 631     typedef typename inverse_type::result_type inverse_result;
 632
 633     static bool const enable_02 = true;
 634     static int const max_iterations_02 = 10;
 635     static int const max_iterations_07 = 20;
 636
 637 public:
 638     template <typename T1, typename T2, typename Spheroid>
 639     static inline bool apply(T1 const& lona1, T1 const& lata1,
 640                              T1 const& lona2, T1 const& lata2,
 641                              T2 const& lonb1, T2 const& latb1,
 642                              T2 const& lonb2, T2 const& latb2,
 643                              CT & lon, CT & lat,
 644                              Spheroid const& spheroid)
 645     {
 646         CT const lon_a1 = lona1;
 647         CT const lat_a1 = lata1;
 648         CT const lon_a2 = lona2;
 649         CT const lat_a2 = lata2;
 650         CT const lon_b1 = lonb1;
 651         CT const lat_b1 = latb1;
 652         CT const lon_b2 = lonb2;
 653         CT const lat_b2 = latb2;
 654
 655         inverse_result const res1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid);
 656         inverse_result const res2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid);
 657
 658         return apply(lon_a1, lat_a1, lon_a2, lat_a2, res1.azimuth,
 659                      lon_b1, lat_b1, lon_b2, lat_b2, res2.azimuth,
 660                      lon, lat, spheroid);
 661     }
 662
 663     // TODO: Currently may not work correctly if one of the endpoints is the pole
 664     template <typename Spheroid>
 665     static inline bool apply(CT const& lon_a1, CT const& lat_a1, CT const& lon_a2, CT const& lat_a2, CT const& alpha_a1,
 666                              CT const& lon_b1, CT const& lat_b1, CT const& lon_b2, CT const& lat_b2, CT const& alpha_b1,
 667                              CT & lon, CT & lat,
 668                              Spheroid const& spheroid)
 669     {
 670         // coordinates in radians
 671
 672         CT const c0 = 0;
 673         CT const c1 = 1;
 674
 675         CT const f = formula::flattening<CT>(spheroid);
 676         CT const one_minus_f = c1 - f;
 677
 678         geodesic_type geod1(lon_a1, lat_a1, alpha_a1, f);
 679         geodesic_type geod2(lon_b1, lat_b1, alpha_b1, f);
 680
 681         // Cj = 1 if on equator <=> sqrt_1_Cj_sqr = 0
 682         // Cj = 0 if vertical <=> sqrt_1_Cj_sqr = 1
 683
 684         if (geod1.is_on_equator && geod2.is_on_equator)
 685         {
 686             return false;
 687         }
 688         else if (geod1.is_on_equator)
 689         {
 690             lon = geod2.lon_of_equator_intersection();
 691             lat = c0;
 692             return true;
 693         }
 694         else if (geod2.is_on_equator)
 695         {
 696             lon = geod1.lon_of_equator_intersection();
 697             lat = c0;
 698             return true;
 699         }
 700
 701         // (lon1 - lon2) normalized to (-180, 180]
 702         CT const lon1_minus_lon2 = math::longitude_distance_signed<radian>(geod2.lonj, geod1.lonj);
 703
 704         // vertical segments
 705         if (geod1.is_Cj_zero && geod2.is_Cj_zero)
 706         {
 707             CT const pi = math::pi<CT>();
 708
 709             // the geodesics are parallel, the intersection point cannot be calculated
 710             if ( math::equals(lon1_minus_lon2, c0)
 711               || math::equals(lon1_minus_lon2 + (lon1_minus_lon2 < c0 ? pi : -pi), c0) )
 712             {
 713                 return false;
 714             }
 715
 716             lon = c0;
 717
 718             // the geodesics intersect at one of the poles
 719             CT const pi_half = pi / CT(2);
 720             CT const abs_lat_a1 = math::abs(lat_a1);
 721             CT const abs_lat_a2 = math::abs(lat_a2);
 722             if (math::equals(abs_lat_a1, abs_lat_a2))
 723             {
 724                 lat = pi_half;
 725             }
 726             else
 727             {
 728                 // pick the pole closest to one of the points of the first segment
 729                 CT const& closer_lat = abs_lat_a1 > abs_lat_a2 ? lat_a1 : lat_a2;
 730                 lat = closer_lat >= 0 ? pi_half : -pi_half;
 731             }
 732
 733             return true;
 734         }
 735
 736         CT lon_sph = 0;
 737
 738         // Starting tan(beta)
 739         CT t = 0;
 740
 741         /*if (geod1.is_Cj_zero)
 742         {
 743             CT const k_base = lon1_minus_lon2 + geod2.sign_lon_diff * geod2.asin_tj_t0j;
 744             t = sin(k_base) * geod2.t0j;
 745             lon_sph = vertical_intersection_longitude(geod1.lonj, lon_b1, lon_b2);
 746         }
 747         else if (geod2.is_Cj_zero)
 748         {
 749             CT const k_base = lon1_minus_lon2 - geod1.sign_lon_diff * geod1.asin_tj_t0j;
 750             t = sin(-k_base) * geod1.t0j;
 751             lon_sph = vertical_intersection_longitude(geod2.lonj, lon_a1, lon_a2);
 752         }
 753         else*/
 754         {
 755             // TODO: Consider using betas instead of latitudes.
 756             //       Some function calls might be saved this way.
 757             CT tan_lat_sph = 0;
 758             sjoberg_intersection_spherical_02<CT>::apply_alt(lon_a1, lat_a1, lon_a2, lat_a2,
 759                                                              lon_b1, lat_b1, lon_b2, lat_b2,
 760                                                              lon_sph, tan_lat_sph);
 761
 762             // Return for sphere
 763             if (math::equals(f, c0))
 764             {
 765                 lon = lon_sph;
 766                 lat = atan(tan_lat_sph);
 767                 return true;
 768             }
 769
 770             t = one_minus_f * tan_lat_sph; // tan(beta)
 771         }
 772
 773         // TODO: no need to calculate atan here if reduced latitudes were used
 774         //       instead of latitudes above, in sjoberg_intersection_spherical_02
 775         CT const beta = atan(t);
 776
 777         if (enable_02 && newton_method(geod1, geod2, beta, t, lon1_minus_lon2, lon_sph, lon, lat))
 778         {
 779             // TODO: Newton's method may return wrong result in some specific cases
 780             // Detected for sphere and nearly sphere, e.g. A=6371228, B=6371227
 781             // and segments s1=(-121 -19,37 8) and s2=(-19 -15,-104 -58)
 782             // It's unclear if this is a bug or a characteristic of this method
 783             // so until this is investigated check if the resulting longitude is
 784             // between endpoints of the segments. It should be since before calling
 785             // this formula sides of endpoints WRT other segments are checked.
 786             if ( is_result_longitude_ok(geod1, lon_a1, lon_a2, lon)
 787               && is_result_longitude_ok(geod2, lon_b1, lon_b2, lon) )
 788             {
 789                 return true;
 790             }
 791         }
 792
 793         return converge_07(geod1, geod2, beta, t, lon1_minus_lon2, lon_sph, lon, lat);
 794     }
 795
 796 private:
 797     static inline bool newton_method(geodesic_type const& geod1, geodesic_type const& geod2, // in
 798                                      CT beta, CT t, CT const& lon1_minus_lon2, CT const& lon_sph, // in
 799                                      CT & lon, CT & lat) // out
 800     {
 801         CT const c0 = 0;
 802         CT const c1 = 1;
 803
 804         CT const e_sqr = geod1.e_sqr;
 805
 806         CT lon1_diff = 0;
 807         CT lon2_diff = 0;
 808
 809         // The segment is vertical and intersection point is behind the vertex
 810         // this method is unable to calculate correct result
 811         if (geod1.is_Cj_zero && math::abs(geod1.lonj - lon_sph) > math::half_pi<CT>())
 812             return false;
 813         if (geod2.is_Cj_zero && math::abs(geod2.lonj - lon_sph) > math::half_pi<CT>())
 814             return false;
 815
 816         CT abs_dbeta_last = 0;
 817
 818         // [Sjoberg02] converges faster than solution in [Sjoberg07]
 819         // Newton-Raphson method
 820         for (int i = 0; i < max_iterations_02; ++i)
 821         {
 822             CT const cos_beta = cos(beta);
 823
 824             if (math::equals(cos_beta, c0))
 825             {
 826                 return false;
 827             }
 828
 829             CT const sin_beta = sin(beta);
 830             CT const cos_beta_sqr = math::sqr(cos_beta);
 831             CT const G = c1 - e_sqr * cos_beta_sqr;
 832
 833             CT f1 = 0;
 834             CT f2 = 0;
 835
 836             if (!geod1.is_Cj_zero)
 837             {
 838                 bool is_beta_ok = geod1.lon_diff(sin_beta, t, lon1_diff);
 839
 840                 if (is_beta_ok)
 841                 {
 842                     CT const H = cos_beta_sqr - geod1.Cj_sqr;
 843                     if (math::equals(H, c0))
 844                     {
 845                         return false;
 846                     }
 847                     f1 = geod1.Cj / cos_beta * math::sqrt(G / H);
 848                 }
 849                 else
 850                 {
 851                     return false;
 852                 }
 853             }
 854
 855             if (!geod2.is_Cj_zero)
 856             {
 857                 bool is_beta_ok = geod2.lon_diff(sin_beta, t, lon2_diff);
 858
 859                 if (is_beta_ok)
 860                 {
 861                     CT const H = cos_beta_sqr - geod2.Cj_sqr;
 862                     if (math::equals(H, c0))
 863                     {
 864                         // NOTE: This may happen for segment nearly
 865                         // at the equator. Detected for (radian):
 866                         // (-0.0872665 -0.0872665, -0.0872665 0.0872665)
 867                         // x
 868                         // (0 1.57e-07, -0.392699 1.57e-07)
 869                         return false;
 870                     }
 871                     f2 = geod2.Cj / cos_beta * math::sqrt(G / H);
 872                 }
 873                 else
 874                 {
 875                     return false;
 876                 }
 877             }
 878
 879             // NOTE: Things may go wrong if the IP is near the vertex
 880             //   1. May converge into the wrong direction (from the other way around).
 881             //      This happens when the starting point is on the other side than the vertex
 882             //   2. During converging may "jump" into the other side of the vertex.
 883             //      In this case sin_beta/sqrt_1_Cj_sqr and t/t0j is not in [-1, 1]
 884             //   3. f1-f2 may be 0 which means that the intermediate point is on the vertex
 885             //      In this case it's not possible to check if this is the correct result
 886             //   4. f1-f2 may also be 0 in other cases, e.g.
 887             //      geodesics are symmetrical wrt equator and longitude directions are different
 888
 889             CT const dbeta_denom = f1 - f2;
 890             //CT const dbeta_denom = math::abs(f1) + math::abs(f2);
 891
 892             if (math::equals(dbeta_denom, c0))
 893             {
 894                 return false;
 895             }
 896
 897             // The sign of dbeta is changed WRT [Sjoberg02]
 898             CT const dbeta = (lon1_minus_lon2 + lon1_diff - lon2_diff) / dbeta_denom;
 899
 900             CT const abs_dbeta = math::abs(dbeta);
 901             if (i > 0 && abs_dbeta > abs_dbeta_last)
 902             {
 903                 // The algorithm is not converging
 904                 // The intersection may be on the other side of the vertex
 905                 return false;
 906             }
 907             abs_dbeta_last = abs_dbeta;
 908
 909             if (math::equals(dbeta, c0))
 910             {
 911                 // Result found
 912                 break;
 913             }
 914
 915             // Because the sign of dbeta is changed WRT [Sjoberg02] dbeta is subtracted here
 916             beta = beta - dbeta;
 917
 918             t = tan(beta);
 919         }
 920
 921         lat = geod1.lat(t);
 922         // NOTE: if Cj is 0 then the result is lonj or lonj+180
 923         lon = ! geod1.is_Cj_zero
 924                 ? geod1.lon(lon1_diff)
 925                 : geod2.lon(lon2_diff);
 926
 927         return true;
 928     }
 929
 930     static inline bool is_result_longitude_ok(geodesic_type const& geod,
 931                                               CT const& lon1, CT const& lon2, CT const& lon)
 932     {
 933         CT const c0 = 0;
 934
 935         if (geod.is_Cj_zero)
 936             return true; // don't check vertical segment
 937
 938         CT dist1p = math::longitude_distance_signed<radian>(lon1, lon);
 939         CT dist12 = math::longitude_distance_signed<radian>(lon1, lon2);
 940
 941         if (dist12 < c0)
 942         {
 943             dist1p = -dist1p;
 944             dist12 = -dist12;
 945         }
 946
 947         return (c0 <= dist1p && dist1p <= dist12)
 948             || math::equals(dist1p, c0)
 949             || math::equals(dist1p, dist12);
 950     }
 951
 952     struct geodesics_type
 953     {
 954         geodesics_type(geodesic_type const& g1, geodesic_type const& g2)
 955             : geod1(g1)
 956             , geod2(g2)
 957             , vertex1(geod1.get_vertex_data())
 958             , vertex2(geod2.get_vertex_data())
 959         {}
 960
 961         geodesic_type const& geod1;
 962         geodesic_type const& geod2;
 963         typename geodesic_type::vertex_data vertex1;
 964         typename geodesic_type::vertex_data vertex2;
 965     };
 966
 967     struct converge_07_result
 968     {
 969         converge_07_result()
 970             : lon1(0), lon2(0), k1_diff(0), k2_diff(0), t1(0), t2(0)
 971         {}
 972
 973         CT lon1, lon2;
 974         CT k1_diff, k2_diff;
 975         CT t1, t2;
 976     };
 977
 978     static inline bool converge_07(geodesic_type const& geod1, geodesic_type const& geod2,
 979                                    CT beta, CT t,
 980                                    CT const& lon1_minus_lon2, CT const& lon_sph,
 981                                    CT & lon, CT & lat)
 982     {
 983         //CT const c0 = 0;
 984         //CT const c1 = 1;
 985         //CT const c2 = 2;
 986         //CT const pi = math::pi<CT>();
 987
 988         geodesics_type geodesics(geod1, geod2);
 989         converge_07_result result;
 990
 991         // calculate first pair of longitudes
 992         if (!converge_07_step_one(CT(sin(beta)), t, lon1_minus_lon2, geodesics, lon_sph, result, false))
 993         {
 994             return false;
 995         }
 996
 997         int t_direction = 0;
 998
 999         CT lon_diff_prev = math::longitude_difference<radian>(result.lon1, result.lon2);
1000
1001         // [Sjoberg07]
1002         for (int i = 2; i < max_iterations_07; ++i)
1003         {
1004             // pick t candidates from previous result based on dir
1005             CT t_cand1 = result.t1;
1006             CT t_cand2 = result.t2;
1007             // if direction is 0 the closer one is the first
1008             if (t_direction < 0)
1009             {
1010                 t_cand1 = (std::min)(result.t1, result.t2);
1011                 t_cand2 = (std::max)(result.t1, result.t2);
1012             }
1013             else if (t_direction > 0)
1014             {
1015                 t_cand1 = (std::max)(result.t1, result.t2);
1016                 t_cand2 = (std::min)(result.t1, result.t2);
1017             }
1018             else
1019             {
1020                 t_direction = t_cand1 < t_cand2 ? -1 : 1;
1021             }
1022
1023             CT t1 = t;
1024             CT beta1 = beta;
1025             // check if the further calculation is needed
1026             if (converge_07_update(t1, beta1, t_cand1))
1027             {
1028                 break;
1029             }
1030
1031             bool try_t2 = false;
1032             converge_07_result result_curr;
1033             if (converge_07_step_one(CT(sin(beta1)), t1, lon1_minus_lon2, geodesics, lon_sph, result_curr))
1034             {
1035                 CT const lon_diff1 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
1036                 if (lon_diff_prev > lon_diff1)
1037                 {
1038                     t = t1;
1039                     beta = beta1;
1040                     lon_diff_prev = lon_diff1;
1041                     result = result_curr;
1042                 }
1043                 else if (t_cand1 != t_cand2)
1044                 {
1045                     try_t2 = true;
1046                 }
1047                 else
1048                 {
1049                     // the result is not fully correct but it won't be more accurate
1050                     break;
1051                 }
1052             }
1053             // ! converge_07_step_one
1054             else
1055             {
1056                 if (t_cand1 != t_cand2)
1057                 {
1058                     try_t2 = true;
1059                 }
1060                 else
1061                 {
1062                     return false;
1063                 }
1064             }
1065
1066
1067             if (try_t2)
1068             {
1069                 CT t2 = t;
1070                 CT beta2 = beta;
1071                 // check if the further calculation is needed
1072                 if (converge_07_update(t2, beta2, t_cand2))
1073                 {
1074                     break;
1075                 }
1076
1077                 if (! converge_07_step_one(CT(sin(beta2)), t2, lon1_minus_lon2, geodesics, lon_sph, result_curr))
1078                 {
1079                     return false;
1080                 }
1081
1082                 CT const lon_diff2 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
1083                 if (lon_diff_prev > lon_diff2)
1084                 {
1085                     t_direction *= -1;
1086                     t = t2;
1087                     beta = beta2;
1088                     lon_diff_prev = lon_diff2;
1089                     result = result_curr;
1090                 }
1091                 else
1092                 {
1093                     // the result is not fully correct but it won't be more accurate
1094                     break;
1095                 }
1096             }
1097         }
1098
1099         lat = geod1.lat(t);
1100         lon = ! geod1.is_Cj_zero ? result.lon1 : result.lon2;
1101         math::normalize_longitude<radian>(lon);
1102
1103         return true;
1104     }
1105
1106     static inline bool converge_07_update(CT & t, CT & beta, CT const& t_new)
1107     {
1108         CT const c0 = 0;
1109
1110         CT const beta_new = atan(t_new);
1111         CT const dbeta = beta_new - beta;
1112         beta = beta_new;
1113         t = t_new;
1114
1115         return math::equals(dbeta, c0);
1116     }
1117
1118     static inline CT const& pick_t(CT const& t1, CT const& t2, int direction)
1119     {
1120         return direction < 0 ? (std::min)(t1, t2) : (std::max)(t1, t2);
1121     }
1122
1123     static inline bool converge_07_step_one(CT const& sin_beta,
1124                                             CT const& t,
1125                                             CT const& lon1_minus_lon2,
1126                                             geodesics_type const& geodesics,
1127                                             CT const& lon_sph,
1128                                             converge_07_result & result,
1129                                             bool check_sin_beta = true)
1130     {
1131         bool ok = converge_07_one_geod(sin_beta, t, geodesics.geod1, geodesics.vertex1, lon_sph,
1132                                        result.lon1, result.k1_diff, check_sin_beta)
1133                && converge_07_one_geod(sin_beta, t, geodesics.geod2, geodesics.vertex2, lon_sph,
1134                                        result.lon2, result.k2_diff, check_sin_beta);
1135
1136         if (!ok)
1137         {
1138             return false;
1139         }
1140
1141         CT const k = lon1_minus_lon2 + result.k1_diff - result.k2_diff;
1142
1143         // get 2 possible ts one lesser and one greater than t
1144         // t1 is the closer one
1145         calc_ts(t, k, geodesics.geod1, geodesics.geod2, result.t1, result.t2);
1146
1147         return true;
1148     }
1149
1150     static inline bool converge_07_one_geod(CT const& sin_beta, CT const& t,
1151                                             geodesic_type const& geod,
1152                                             typename geodesic_type::vertex_data const& vertex,
1153                                             CT const& lon_sph,
1154                                             CT & lon, CT & k_diff,
1155                                             bool check_sin_beta)
1156     {
1157         using math::detail::bounded;
1158         CT const c1 = 1;
1159
1160         CT k_diff_before = 0;
1161         CT k_diff_behind = 0;
1162
1163         bool is_beta_ok = geod.k_diffs(sin_beta, vertex, k_diff_before, k_diff_behind, check_sin_beta);
1164
1165         if (! is_beta_ok)
1166         {
1167             return false;
1168         }
1169
1170         CT const asin_t_t0j = ! geod.is_Cj_zero ? asin(bounded(t / geod.t0j, -c1, c1)) : 0;
1171         CT const sign_asin_t_t0j = geod.sign_lon_diff * asin_t_t0j;
1172
1173         CT const lon_before = geod.lonj + sign_asin_t_t0j + k_diff_before;
1174         CT const lon_behind = geod.lonj - sign_asin_t_t0j + k_diff_behind;
1175
1176         CT const lon_dist_before = math::longitude_distance_signed<radian>(lon_before, lon_sph);
1177         CT const lon_dist_behind = math::longitude_distance_signed<radian>(lon_behind, lon_sph);
1178         if (math::abs(lon_dist_before) <= math::abs(lon_dist_behind))
1179         {
1180             k_diff = k_diff_before;
1181             lon = lon_before;
1182         }
1183         else
1184         {
1185             k_diff = k_diff_behind;
1186             lon = lon_behind;
1187         }
1188
1189         return true;
1190     }
1191
1192     static inline void calc_ts(CT const& t, CT const& k,
1193                                geodesic_type const& geod1, geodesic_type const& geod2,
1194                                CT & t1, CT& t2)
1195     {
1196         CT const c0 = 0;
1197         CT const c1 = 1;
1198         CT const c2 = 2;
1199
1200         CT const K = sin(k);
1201
1202         BOOST_GEOMETRY_ASSERT(!geod1.is_Cj_zero || !geod2.is_Cj_zero);
1203         if (geod1.is_Cj_zero)
1204         {
1205             t1 = K * geod2.t0j;
1206             t2 = -t1;
1207         }
1208         else if (geod2.is_Cj_zero)
1209         {
1210             t1 = -K * geod1.t0j;
1211             t2 = -t1;
1212         }
1213         else
1214         {
1215             CT const A = math::sqr(geod1.t0j) + math::sqr(geod2.t0j);
1216             CT const B = c2 * geod1.t0j * geod2.t0j * math::sqrt(c1 - math::sqr(K));
1217
1218             CT const K_t01_t02 = K * geod1.t0j * geod2.t0j;
1219             CT const D1 = math::sqrt(A + B);
1220             CT const D2 = math::sqrt(A - B);
1221             CT const t_new1 = math::equals(D1, c0) ? c0 : K_t01_t02 / D1;
1222             CT const t_new2 = math::equals(D2, c0) ? c0 : K_t01_t02 / D2;
1223             CT const t_new3 = -t_new1;
1224             CT const t_new4 = -t_new2;
1225
1226             // Pick 2 nearest t_new, one greater and one lesser than current t
1227             CT const abs_t_new1 = math::abs(t_new1);
1228             CT const abs_t_new2 = math::abs(t_new2);
1229             CT const abs_t_max = (std::max)(abs_t_new1, abs_t_new2);
1230             t1 = -abs_t_max; // lesser
1231             t2 = abs_t_max; // greater
1232             if (t1 < t)
1233             {
1234                 if (t_new1 < t && t_new1 > t1)
1235                     t1 = t_new1;
1236                 if (t_new2 < t && t_new2 > t1)
1237                     t1 = t_new2;
1238                 if (t_new3 < t && t_new3 > t1)
1239                     t1 = t_new3;
1240                 if (t_new4 < t && t_new4 > t1)
1241                     t1 = t_new4;
1242             }
1243             if (t2 > t)
1244             {
1245                 if (t_new1 > t && t_new1 < t2)
1246                     t2 = t_new1;
1247                 if (t_new2 > t && t_new2 < t2)
1248                     t2 = t_new2;
1249                 if (t_new3 > t && t_new3 < t2)
1250                     t2 = t_new3;
1251                 if (t_new4 > t && t_new4 < t2)
1252                     t2 = t_new4;
1253             }
1254         }
1255
1256         // the first one is the closer one
1257         if (math::abs(t - t2) < math::abs(t - t1))
1258         {
1259             std::swap(t2, t1);
1260         }
1261     }
1262
1263     static inline CT fj(CT const& cos_beta, CT const& cos2_beta, CT const& Cj, CT const& e_sqr)
1264     {
1265         CT const c1 = 1;
1266         CT const Cj_sqr = math::sqr(Cj);
1267         return Cj / cos_beta * math::sqrt((c1 - e_sqr * cos2_beta) / (cos2_beta - Cj_sqr));
1268     }
1269
1270     /*static inline CT vertical_intersection_longitude(CT const& ip_lon, CT const& seg_lon1, CT const& seg_lon2)
1271     {
1272         CT const c0 = 0;
1273         CT const lon_2 = ip_lon > c0 ? ip_lon - pi : ip_lon + pi;
1274
1275         return (std::min)(math::longitude_difference<radian>(ip_lon, seg_lon1),
1276                           math::longitude_difference<radian>(ip_lon, seg_lon2))
1277             <=
1278                (std::min)(math::longitude_difference<radian>(lon_2, seg_lon1),
1279                           math::longitude_difference<radian>(lon_2, seg_lon2))
1280             ? ip_lon : lon_2;
1281     }*/
1282 };
1283
1284 }}} // namespace boost::geometry::formula
1285
1286
1287 #endif // BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP