]>
Commit | Line | Data |
---|---|---|
b32b8144 FG |
1 | // Boost.Geometry |
2 | ||
92f5a8d4 | 3 | // Copyright (c) 2016-2019 Oracle and/or its affiliates. |
b32b8144 FG |
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> | |
b32b8144 FG |
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 | { | |
b32b8144 | 822 | CT const cos_beta = cos(beta); |
92f5a8d4 TL |
823 | |
824 | if (math::equals(cos_beta, c0)) | |
825 | { | |
826 | return false; | |
827 | } | |
828 | ||
829 | CT const sin_beta = sin(beta); | |
b32b8144 FG |
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; | |
92f5a8d4 TL |
843 | if (math::equals(H, c0)) |
844 | { | |
845 | return false; | |
846 | } | |
b32b8144 FG |
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; | |
92f5a8d4 TL |
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 | } | |
b32b8144 FG |
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 | |
92f5a8d4 TL |
886 | // 4. f1-f2 may also be 0 in other cases, e.g. |
887 | // geodesics are symmetrical wrt equator and longitude directions are different | |
b32b8144 FG |
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 | { | |
92f5a8d4 | 1196 | CT const c0 = 0; |
b32b8144 FG |
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); | |
92f5a8d4 TL |
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; | |
b32b8144 FG |
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 |