ceph/src/boost/boost/geometry/formulas/sjoberg_intersection.hpp

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