]>
Commit | Line | Data |
---|---|---|
b32b8144 FG |
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 |