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
applyboost::geometry::formula::sjoberg_intersection_spherical_0242 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
apply_altboost::geometry::formula::sjoberg_intersection_spherical_0259 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:
applyboost::geometry::formula::sjoberg_intersection_spherical_0283 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>
sjoberg_d_lambda_e_sqr(CT const & sin_betaj,CT const & sin_beta,CT const & Cj,CT const & sqrt_1_Cj_sqr,CT const & e_sqr)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 {
sjoberg_geodesic()291 sjoberg_geodesic() {}
292
sign_C(CT const & alphaj)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:
sjoberg_geodesic(CT const & lon,CT const & lat,CT const & alpha,CT const & f)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
get_vertex_data() const359 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
is_sin_beta_ok(CT const & sin_beta) const387 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
k_diff(CT const & sin_beta,CT & delta_k) const393 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
lon_diff(CT const & sin_beta,CT const & t,CT & delta_lon) const416 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
k_diffs(CT const & sin_beta,vertex_data const & vd,CT & delta_k_before,CT & delta_k_behind,bool check_sin_beta=true) const442 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
lon_diffs(CT const & sin_beta,CT const & t,vertex_data const & vd,CT & delta_lon_before,CT & delta_lon_behind) const479 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
lon(CT const & sin_beta,CT const & t,vertex_data const & vd,CT & lon_before,CT & lon_behind) const509 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
lon(CT const & delta_lon) const539 CT lon(CT const& delta_lon) const
540 {
541 return lonj + delta_lon;
542 }
543
lat(CT const & t) const544 CT lat(CT const& t) const
545 {
546 // t = tan(beta) = (1-f)tan(lat)
547 return atan(t / one_minus_f);
548 }
549
vertex(CT & lon,CT & lat) const550 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
lon_of_equator_intersection() const564 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
d_lambda(CT const & sin_beta) const572 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>
apply(T1 const & lona1,T1 const & lata1,T1 const & lona2,T1 const & lata2,T2 const & lonb1,T2 const & latb1,T2 const & lonb2,T2 const & latb2,CT & lon,CT & lat,Spheroid const & 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>
apply(CT const & lon_a1,CT const & lat_a1,CT const & lon_a2,CT const & lat_a2,CT const & alpha_a1,CT const & lon_b1,CT const & lat_b1,CT const & lon_b2,CT const & lat_b2,CT const & alpha_b1,CT & lon,CT & lat,Spheroid const & 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:
newton_method(geodesic_type const & geod1,geodesic_type const & geod2,CT beta,CT t,CT const & lon1_minus_lon2,CT const & lon_sph,CT & lon,CT & lat)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
is_result_longitude_ok(geodesic_type const & geod,CT const & lon1,CT const & lon2,CT const & lon)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 {
geodesics_typeboost::geometry::formula::sjoberg_intersection::geodesics_type954 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 {
converge_07_resultboost::geometry::formula::sjoberg_intersection::converge_07_result969 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
converge_07(geodesic_type const & geod1,geodesic_type const & geod2,CT beta,CT t,CT const & lon1_minus_lon2,CT const & lon_sph,CT & lon,CT & lat)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
converge_07_update(CT & t,CT & beta,CT const & t_new)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
pick_t(CT const & t1,CT const & t2,int direction)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
converge_07_step_one(CT const & sin_beta,CT const & t,CT const & lon1_minus_lon2,geodesics_type const & geodesics,CT const & lon_sph,converge_07_result & result,bool check_sin_beta=true)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
converge_07_one_geod(CT const & sin_beta,CT const & t,geodesic_type const & geod,typename geodesic_type::vertex_data const & vertex,CT const & lon_sph,CT & lon,CT & k_diff,bool check_sin_beta)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
calc_ts(CT const & t,CT const & k,geodesic_type const & geod1,geodesic_type const & geod2,CT & t1,CT & t2)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
fj(CT const & cos_beta,CT const & cos2_beta,CT const & Cj,CT const & e_sqr)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
1288