1 // Boost.Geometry
2
3 // Copyright (c) 2018 Adeel Ahmad, Islamabad, Pakistan.
4
5 // Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program.
6
7 // This file was modified by Oracle on 2019-2021.
8 // Modifications copyright (c) 2019-2021 Oracle and/or its affiliates.
9
10 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
11 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
12
13 // Use, modification and distribution is subject to the Boost Software License,
14 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
15 // http://www.boost.org/LICENSE_1_0.txt)
16
17 // This file is converted from GeographicLib, https://geographiclib.sourceforge.io
18 // GeographicLib is originally written by Charles Karney.
19
20 // Author: Charles Karney (2008-2017)
21
22 // Last updated version of GeographicLib: 1.49
23
24 // Original copyright notice:
25
26 // Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
27 // under the MIT/X11 License. For more information, see
28 // https://geographiclib.sourceforge.io
29
30 #ifndef BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
31 #define BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
32
33
34 #include <boost/math/constants/constants.hpp>
35 #include <boost/math/special_functions/hypot.hpp>
36
37 #include <boost/geometry/util/condition.hpp>
38 #include <boost/geometry/util/math.hpp>
39 #include <boost/geometry/util/precise_math.hpp>
40 #include <boost/geometry/util/series_expansion.hpp>
41 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
42
43 #include <boost/geometry/formulas/flattening.hpp>
44 #include <boost/geometry/formulas/result_inverse.hpp>
45
46
47 namespace boost { namespace geometry { namespace math {
48
49 /*!
50 \brief The exact difference of two angles reduced to (-180deg, 180deg].
51 */
52 template<typename T>
difference_angle(T const & x,T const & y,T & e)53 inline T difference_angle(T const& x, T const& y, T& e)
54 {
55 auto res1 = boost::geometry::detail::precise_math::two_sum(
56 std::remainder(-x, T(360)), std::remainder(y, T(360)));
57
58 normalize_azimuth<degree, T>(res1[0]);
59
60 // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
61 // abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
62 // addition of t takes the result outside the range (-180,180] is d = 180
63 // and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since
64 // sum_error would have returned the exact result in such a case (i.e., given t = 0).
65 auto res2 = boost::geometry::detail::precise_math::two_sum(
66 res1[0] == 180 && res1[1] > 0 ? -180 : res1[0], res1[1]);
67 e = res2[1];
68 return res2[0];
69 }
70
71 }}} // namespace boost::geometry::math
72
73
74 namespace boost { namespace geometry { namespace formula
75 {
76
77 namespace se = series_expansion;
78
79 namespace detail
80 {
81
82 template <
83 typename CT,
84 bool EnableDistance,
85 bool EnableAzimuth,
86 bool EnableReverseAzimuth = false,
87 bool EnableReducedLength = false,
88 bool EnableGeodesicScale = false,
89 size_t SeriesOrder = 8
90 >
91 class karney_inverse
92 {
93 static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
94 static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
95 static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
96 static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
97
98 public:
99 typedef result_inverse<CT> result_type;
100
101 template <typename T1, typename T2, typename Spheroid>
apply(T1 const & lo1,T1 const & la1,T2 const & lo2,T2 const & la2,Spheroid const & spheroid)102 static inline result_type apply(T1 const& lo1,
103 T1 const& la1,
104 T2 const& lo2,
105 T2 const& la2,
106 Spheroid const& spheroid)
107 {
108 static CT const c0 = 0;
109 static CT const c0_001 = 0.001;
110 static CT const c0_1 = 0.1;
111 static CT const c1 = 1;
112 static CT const c2 = 2;
113 static CT const c3 = 3;
114 static CT const c8 = 8;
115 static CT const c16 = 16;
116 static CT const c90 = 90;
117 static CT const c180 = 180;
118 static CT const c200 = 200;
119 static CT const pi = math::pi<CT>();
120 static CT const d2r = math::d2r<CT>();
121 static CT const r2d = math::r2d<CT>();
122
123 result_type result;
124
125 CT lat1 = la1 * r2d;
126 CT lat2 = la2 * r2d;
127
128 CT lon1 = lo1 * r2d;
129 CT lon2 = lo2 * r2d;
130
131 CT const a = CT(get_radius<0>(spheroid));
132 CT const b = CT(get_radius<2>(spheroid));
133 CT const f = formula::flattening<CT>(spheroid);
134 CT const one_minus_f = c1 - f;
135 CT const two_minus_f = c2 - f;
136
137 CT const tol0 = std::numeric_limits<CT>::epsilon();
138 CT const tol1 = c200 * tol0;
139 CT const tol2 = sqrt(tol0);
140
141 // Check on bisection interval.
142 CT const tol_bisection = tol0 * tol2;
143
144 CT const etol2 = c0_1 * tol2 /
145 sqrt((std::max)(c0_001, std::abs(f)) * (std::min)(c1, c1 - f / c2) / c2);
146
147 CT tiny = std::sqrt((std::numeric_limits<CT>::min)());
148
149 CT const n = f / two_minus_f;
150 CT const e2 = f * two_minus_f;
151 CT const ep2 = e2 / math::sqr(one_minus_f);
152
153 // Compute the longitudinal difference.
154 CT lon12_error;
155 CT lon12 = math::difference_angle(lon1, lon2, lon12_error);
156
157 int lon12_sign = lon12 >= 0 ? 1 : -1;
158
159 // Make points close to the meridian to lie on it.
160 lon12 = lon12_sign * lon12;
161 lon12_error = (c180 - lon12) - lon12_sign * lon12_error;
162
163 // Convert to radians.
164 CT lam12 = lon12 * d2r;
165 CT sin_lam12;
166 CT cos_lam12;
167
168 if (lon12 > c90)
169 {
170 math::sin_cos_degrees(lon12_error, sin_lam12, cos_lam12);
171 cos_lam12 *= -c1;
172 }
173 else
174 {
175 math::sin_cos_degrees(lon12, sin_lam12, cos_lam12);
176 }
177
178 // Make points close to the equator to lie on it.
179 lat1 = math::round_angle(std::abs(lat1) > c90 ? c90 : lat1);
180 lat2 = math::round_angle(std::abs(lat2) > c90 ? c90 : lat2);
181
182 // Arrange points in a canonical form, as explained in
183 // paper, Algorithms for geodesics, Eq. (44):
184 //
185 // 0 <= lon12 <= 180
186 // -90 <= lat1 <= 0
187 // lat1 <= lat2 <= -lat1
188 int swap_point = std::abs(lat1) < std::abs(lat2) ? -1 : 1;
189
190 if (swap_point < 0)
191 {
192 lon12_sign *= -1;
193 swap(lat1, lat2);
194 }
195
196 // Enforce lat1 to be <= 0.
197 int lat_sign = lat1 < 0 ? 1 : -1;
198 lat1 *= lat_sign;
199 lat2 *= lat_sign;
200
201 CT sin_beta1, cos_beta1;
202 math::sin_cos_degrees(lat1, sin_beta1, cos_beta1);
203 sin_beta1 *= one_minus_f;
204
205 math::normalize_unit_vector<CT>(sin_beta1, cos_beta1);
206 cos_beta1 = (std::max)(tiny, cos_beta1);
207
208 CT sin_beta2, cos_beta2;
209 math::sin_cos_degrees(lat2, sin_beta2, cos_beta2);
210 sin_beta2 *= one_minus_f;
211
212 math::normalize_unit_vector<CT>(sin_beta2, cos_beta2);
213 cos_beta2 = (std::max)(tiny, cos_beta2);
214
215 // If cos_beta1 < -sin_beta1, then cos_beta2 - cos_beta1 is a
216 // sensitive measure of the |beta1| - |beta2|. Alternatively,
217 // (cos_beta1 >= -sin_beta1), abs(sin_beta2) + sin_beta1 is
218 // a better measure.
219 // Sometimes these quantities vanish and in that case we
220 // force beta2 = +/- bet1a exactly.
221 if (cos_beta1 < -sin_beta1)
222 {
223 if (cos_beta1 == cos_beta2)
224 {
225 sin_beta2 = sin_beta2 < 0 ? sin_beta1 : -sin_beta1;
226 }
227 }
228 else
229 {
230 if (std::abs(sin_beta2) == -sin_beta1)
231 {
232 cos_beta2 = cos_beta1;
233 }
234 }
235
236 CT const dn1 = sqrt(c1 + ep2 * math::sqr(sin_beta1));
237 CT const dn2 = sqrt(c1 + ep2 * math::sqr(sin_beta2));
238
239 CT sigma12;
240 CT m12x = c0;
241 CT s12x;
242 CT M21;
243
244 // Index zero element of coeffs_C1 is unused.
245 se::coeffs_C1<SeriesOrder, CT> const coeffs_C1(n);
246
247 bool meridian = lat1 == -90 || sin_lam12 == 0;
248
249 CT cos_alpha1, sin_alpha1;
250 CT cos_alpha2, sin_alpha2;
251
252 if (meridian)
253 {
254 // Endpoints lie on a single full meridian.
255
256 // Point to the target latitude.
257 cos_alpha1 = cos_lam12;
258 sin_alpha1 = sin_lam12;
259
260 // Heading north at the target.
261 cos_alpha2 = c1;
262 sin_alpha2 = c0;
263
264 CT sin_sigma1 = sin_beta1;
265 CT cos_sigma1 = cos_alpha1 * cos_beta1;
266
267 CT sin_sigma2 = sin_beta2;
268 CT cos_sigma2 = cos_alpha2 * cos_beta2;
269
270 CT sigma12 = std::atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
271 cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);
272
273 CT dummy;
274 meridian_length(n, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
275 sin_sigma2, cos_sigma2, dn2,
276 cos_beta1, cos_beta2, s12x,
277 m12x, dummy, result.geodesic_scale,
278 M21, coeffs_C1);
279
280 if (sigma12 < c1 || m12x >= c0)
281 {
282 if (sigma12 < c3 * tiny)
283 {
284 sigma12 = m12x = s12x = c0;
285 }
286
287 m12x *= b;
288 s12x *= b;
289 }
290 else
291 {
292 // m12 < 0, i.e., prolate and too close to anti-podal.
293 meridian = false;
294 }
295 }
296
297 CT omega12;
298
299 if (!meridian && sin_beta1 == c0 &&
300 (f <= c0 || lon12_error >= f * c180))
301 {
302 // Points lie on the equator.
303 cos_alpha1 = cos_alpha2 = c0;
304 sin_alpha1 = sin_alpha2 = c1;
305
306 s12x = a * lam12;
307 sigma12 = omega12 = lam12 / one_minus_f;
308 m12x = b * sin(sigma12);
309
310 if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
311 {
312 result.geodesic_scale = cos(sigma12);
313 }
314 }
315 else if (!meridian)
316 {
317 // If point1 and point2 belong within a hemisphere bounded by a
318 // meridian and geodesic is neither meridional nor equatorial.
319
320 // Find the starting point for Newton's method.
321 CT dnm = c1;
322 sigma12 = newton_start(sin_beta1, cos_beta1, dn1,
323 sin_beta2, cos_beta2, dn2,
324 lam12, sin_lam12, cos_lam12,
325 sin_alpha1, cos_alpha1,
326 sin_alpha2, cos_alpha2,
327 dnm, coeffs_C1, ep2,
328 tol1, tol2, etol2,
329 n, f);
330
331 if (sigma12 >= c0)
332 {
333 // Short lines case (newton_start sets sin_alpha2, cos_alpha2, dnm).
334 s12x = sigma12 * b * dnm;
335 m12x = math::sqr(dnm) * b * sin(sigma12 / dnm);
336 if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
337 {
338 result.geodesic_scale = cos(sigma12 / dnm);
339 }
340
341 // Convert to radians.
342 omega12 = lam12 / (one_minus_f * dnm);
343 }
344 else
345 {
346 // Apply the Newton's method.
347 CT sin_sigma1 = c0, cos_sigma1 = c0;
348 CT sin_sigma2 = c0, cos_sigma2 = c0;
349 CT eps = c0, diff_omega12 = c0;
350
351 // Bracketing range.
352 CT sin_alpha1a = tiny, cos_alpha1a = c1;
353 CT sin_alpha1b = tiny, cos_alpha1b = -c1;
354
355 size_t iteration = 0;
356 size_t max_iterations = 20 + std::numeric_limits<size_t>::digits + 10;
357
358 for (bool tripn = false, tripb = false;
359 iteration < max_iterations;
360 ++iteration)
361 {
362 CT dv = c0;
363 CT v = lambda12(sin_beta1, cos_beta1, dn1,
364 sin_beta2, cos_beta2, dn2,
365 sin_alpha1, cos_alpha1,
366 sin_lam12, cos_lam12,
367 sin_alpha2, cos_alpha2,
368 sigma12,
369 sin_sigma1, cos_sigma1,
370 sin_sigma2, cos_sigma2,
371 eps, diff_omega12,
372 iteration < max_iterations,
373 dv, f, n, ep2, tiny, coeffs_C1);
374
375 // Reversed test to allow escape with NaNs.
376 if (tripb || !(std::abs(v) >= (tripn ? c8 : c1) * tol0))
377 break;
378
379 // Update bracketing values.
380 if (v > c0 && (iteration > max_iterations ||
381 cos_alpha1 / sin_alpha1 > cos_alpha1b / sin_alpha1b))
382 {
383 sin_alpha1b = sin_alpha1;
384 cos_alpha1b = cos_alpha1;
385 }
386 else if (v < c0 && (iteration > max_iterations ||
387 cos_alpha1 / sin_alpha1 < cos_alpha1a / sin_alpha1a))
388 {
389 sin_alpha1a = sin_alpha1;
390 cos_alpha1a = cos_alpha1;
391 }
392
393 if (iteration < max_iterations && dv > c0)
394 {
395 CT diff_alpha1 = -v / dv;
396
397 CT sin_diff_alpha1 = sin(diff_alpha1);
398 CT cos_diff_alpha1 = cos(diff_alpha1);
399
400 CT nsin_alpha1 = sin_alpha1 * cos_diff_alpha1 +
401 cos_alpha1 * sin_diff_alpha1;
402
403 if (nsin_alpha1 > c0 && std::abs(diff_alpha1) < pi)
404 {
405 cos_alpha1 = cos_alpha1 * cos_diff_alpha1 - sin_alpha1 * sin_diff_alpha1;
406 sin_alpha1 = nsin_alpha1;
407 math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
408
409 // In some regimes we don't get quadratic convergence because
410 // slope -> 0. So use convergence conditions based on epsilon
411 // instead of sqrt(epsilon).
412 tripn = std::abs(v) <= c16 * tol0;
413 continue;
414 }
415 }
416
417 // Either dv was not positive or updated value was outside legal
418 // range. Use the midpoint of the bracket as the next estimate.
419 // This mechanism is not needed for the WGS84 ellipsoid, but it does
420 // catch problems with more eeccentric ellipsoids. Its efficacy is
421 // such for the WGS84 test set with the starting guess set to alp1 =
422 // 90deg:
423 // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
424 // WGS84 and random input: mean = 4.74, sd = 0.99
425 sin_alpha1 = (sin_alpha1a + sin_alpha1b) / c2;
426 cos_alpha1 = (cos_alpha1a + cos_alpha1b) / c2;
427 math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
428 tripn = false;
429 tripb = (std::abs(sin_alpha1a - sin_alpha1) + (cos_alpha1a - cos_alpha1) < tol_bisection ||
430 std::abs(sin_alpha1 - sin_alpha1b) + (cos_alpha1 - cos_alpha1b) < tol_bisection);
431 }
432
433 CT dummy;
434 se::coeffs_C1<SeriesOrder, CT> const coeffs_C1_eps(eps);
435 // Ensure that the reduced length and geodesic scale are computed in
436 // a "canonical" way, with the I2 integral.
437 meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
438 sin_sigma2, cos_sigma2, dn2,
439 cos_beta1, cos_beta2, s12x,
440 m12x, dummy, result.geodesic_scale,
441 M21, coeffs_C1_eps);
442
443 m12x *= b;
444 s12x *= b;
445 }
446 }
447
448 if (swap_point < 0)
449 {
450 swap(sin_alpha1, sin_alpha2);
451 swap(cos_alpha1, cos_alpha2);
452 swap(result.geodesic_scale, M21);
453 }
454
455 sin_alpha1 *= swap_point * lon12_sign;
456 cos_alpha1 *= swap_point * lat_sign;
457
458 sin_alpha2 *= swap_point * lon12_sign;
459 cos_alpha2 *= swap_point * lat_sign;
460
461 if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
462 {
463 result.reduced_length = m12x;
464 }
465
466 if (BOOST_GEOMETRY_CONDITION(CalcAzimuths))
467 {
468 if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
469 {
470 result.azimuth = atan2(sin_alpha1, cos_alpha1);
471 }
472
473 if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
474 {
475 result.reverse_azimuth = atan2(sin_alpha2, cos_alpha2);
476 }
477 }
478
479 if (BOOST_GEOMETRY_CONDITION(EnableDistance))
480 {
481 result.distance = s12x;
482 }
483
484 return result;
485 }
486
487 template <typename CoeffsC1>
meridian_length(CT const & epsilon,CT const & ep2,CT const & sigma12,CT const & sin_sigma1,CT const & cos_sigma1,CT const & dn1,CT const & sin_sigma2,CT const & cos_sigma2,CT const & dn2,CT const & cos_beta1,CT const & cos_beta2,CT & s12x,CT & m12x,CT & m0,CT & M12,CT & M21,CoeffsC1 const & coeffs_C1)488 static inline void meridian_length(CT const& epsilon, CT const& ep2, CT const& sigma12,
489 CT const& sin_sigma1, CT const& cos_sigma1, CT const& dn1,
490 CT const& sin_sigma2, CT const& cos_sigma2, CT const& dn2,
491 CT const& cos_beta1, CT const& cos_beta2,
492 CT& s12x, CT& m12x, CT& m0,
493 CT& M12, CT& M21,
494 CoeffsC1 const& coeffs_C1)
495 {
496 static CT const c1 = 1;
497
498 CT A12x = 0, J12 = 0;
499 CT expansion_A1, expansion_A2;
500
501 // Evaluate the coefficients for C2.
502 se::coeffs_C2<SeriesOrder, CT> coeffs_C2(epsilon);
503
504 if (BOOST_GEOMETRY_CONDITION(EnableDistance) ||
505 BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
506 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
507 {
508 // Find the coefficients for A1 by computing the
509 // series expansion using Horner scehme.
510 expansion_A1 = se::evaluate_A1<SeriesOrder>(epsilon);
511
512 if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
513 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
514 {
515 // Find the coefficients for A2 by computing the
516 // series expansion using Horner scehme.
517 expansion_A2 = se::evaluate_A2<SeriesOrder>(epsilon);
518
519 A12x = expansion_A1 - expansion_A2;
520 expansion_A2 += c1;
521 }
522 expansion_A1 += c1;
523 }
524
525 if (BOOST_GEOMETRY_CONDITION(EnableDistance))
526 {
527 CT B1 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C1)
528 - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C1);
529
530 s12x = expansion_A1 * (sigma12 + B1);
531
532 if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
533 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
534 {
535 CT B2 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
536 - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2);
537
538 J12 = A12x * sigma12 + (expansion_A1 * B1 - expansion_A2 * B2);
539 }
540 }
541 else if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
542 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
543 {
544 for (size_t i = 1; i <= SeriesOrder; ++i)
545 {
546 coeffs_C2[i] = expansion_A1 * coeffs_C1[i] -
547 expansion_A2 * coeffs_C2[i];
548 }
549
550 J12 = A12x * sigma12 +
551 (se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
552 - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2));
553 }
554
555 if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
556 {
557 m0 = A12x;
558
559 m12x = dn2 * (cos_sigma1 * sin_sigma2) -
560 dn1 * (sin_sigma1 * cos_sigma2) -
561 cos_sigma1 * cos_sigma2 * J12;
562 }
563
564 if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
565 {
566 CT cos_sigma12 = cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2;
567 CT t = ep2 * (cos_beta1 - cos_beta2) *
568 (cos_beta1 + cos_beta2) / (dn1 + dn2);
569
570 M12 = cos_sigma12 + (t * sin_sigma2 - cos_sigma2 * J12) * sin_sigma1 / dn1;
571 M21 = cos_sigma12 - (t * sin_sigma1 - cos_sigma1 * J12) * sin_sigma2 / dn2;
572 }
573 }
574
575 /*
576 Return a starting point for Newton's method in sin_alpha1 and
577 cos_alpha1 (function value is -1). If Newton's method
578 doesn't need to be used, return also sin_alpha2 and
579 cos_alpha2 and function value is sig12.
580 */
581 template <typename CoeffsC1>
newton_start(CT const & sin_beta1,CT const & cos_beta1,CT const & dn1,CT const & sin_beta2,CT const & cos_beta2,CT dn2,CT const & lam12,CT const & sin_lam12,CT const & cos_lam12,CT & sin_alpha1,CT & cos_alpha1,CT & sin_alpha2,CT & cos_alpha2,CT & dnm,CoeffsC1 const & coeffs_C1,CT const & ep2,CT const & tol1,CT const & tol2,CT const & etol2,CT const & n,CT const & f)582 static inline CT newton_start(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
583 CT const& sin_beta2, CT const& cos_beta2, CT dn2,
584 CT const& lam12, CT const& sin_lam12, CT const& cos_lam12,
585 CT& sin_alpha1, CT& cos_alpha1,
586 CT& sin_alpha2, CT& cos_alpha2,
587 CT& dnm, CoeffsC1 const& coeffs_C1, CT const& ep2,
588 CT const& tol1, CT const& tol2, CT const& etol2, CT const& n,
589 CT const& f)
590 {
591 static CT const c0 = 0;
592 static CT const c0_01 = 0.01;
593 static CT const c0_1 = 0.1;
594 static CT const c0_5 = 0.5;
595 static CT const c1 = 1;
596 static CT const c2 = 2;
597 static CT const c6 = 6;
598 static CT const c1000 = 1000;
599 static CT const pi = math::pi<CT>();
600
601 CT const one_minus_f = c1 - f;
602 CT const x_thresh = c1000 * tol2;
603
604 // Return a starting point for Newton's method in sin_alpha1
605 // and cos_alpha1 (function value is -1). If Newton's method
606 // doesn't need to be used, return also sin_alpha2 and
607 // cos_alpha2 and function value is sig12.
608 CT sig12 = -c1;
609
610 // bet12 = bet2 - bet1 in [0, pi); beta12a = bet2 + bet1 in (-pi, 0]
611 CT sin_beta12 = sin_beta2 * cos_beta1 - cos_beta2 * sin_beta1;
612 CT cos_beta12 = cos_beta2 * cos_beta1 + sin_beta2 * sin_beta1;
613
614 CT sin_beta12a = sin_beta2 * cos_beta1 + cos_beta2 * sin_beta1;
615
616 bool shortline = cos_beta12 >= c0 && sin_beta12 < c0_5 &&
617 cos_beta2 * lam12 < c0_5;
618
619 CT sin_omega12, cos_omega12;
620
621 if (shortline)
622 {
623 CT sin_beta_m2 = math::sqr(sin_beta1 + sin_beta2);
624
625 sin_beta_m2 /= sin_beta_m2 + math::sqr(cos_beta1 + cos_beta2);
626 dnm = math::sqrt(c1 + ep2 * sin_beta_m2);
627
628 CT omega12 = lam12 / (one_minus_f * dnm);
629
630 sin_omega12 = sin(omega12);
631 cos_omega12 = cos(omega12);
632 }
633 else
634 {
635 sin_omega12 = sin_lam12;
636 cos_omega12 = cos_lam12;
637 }
638
639 sin_alpha1 = cos_beta2 * sin_omega12;
640 cos_alpha1 = cos_omega12 >= c0 ?
641 sin_beta12 + cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 + cos_omega12) :
642 sin_beta12a - cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 - cos_omega12);
643
644 CT sin_sigma12 = boost::math::hypot(sin_alpha1, cos_alpha1);
645 CT cos_sigma12 = sin_beta1 * sin_beta2 + cos_beta1 * cos_beta2 * cos_omega12;
646
647 if (shortline && sin_sigma12 < etol2)
648 {
649 sin_alpha2 = cos_beta1 * sin_omega12;
650 cos_alpha2 = sin_beta12 - cos_beta1 * sin_beta2 *
651 (cos_omega12 >= c0 ? math::sqr(sin_omega12) /
652 (c1 + cos_omega12) : c1 - cos_omega12);
653
654 math::normalize_unit_vector<CT>(sin_alpha2, cos_alpha2);
655 // Set return value.
656 sig12 = atan2(sin_sigma12, cos_sigma12);
657 }
658 // Skip astroid calculation if too eccentric.
659 else if (std::abs(n) > c0_1 ||
660 cos_sigma12 >= c0 ||
661 sin_sigma12 >= c6 * std::abs(n) * pi *
662 math::sqr(cos_beta1))
663 {
664 // Nothing to do, zeroth order spherical approximation will do.
665 }
666 else
667 {
668 // Scale lam12 and bet2 to x, y coordinate system where antipodal
669 // point is at origin and singular point is at y = 0, x = -1.
670 CT lambda_scale, beta_scale;
671
672 CT y;
673 volatile CT x;
674
675 CT lam12x = atan2(-sin_lam12, -cos_lam12);
676 if (f >= c0)
677 {
678 CT k2 = math::sqr(sin_beta1) * ep2;
679 CT eps = k2 / (c2 * (c1 + sqrt(c1 + k2)) + k2);
680
681 se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
682
683 CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
684
685 lambda_scale = f * cos_beta1 * A3 * pi;
686 beta_scale = lambda_scale * cos_beta1;
687
688 x = lam12x / lambda_scale;
689 y = sin_beta12a / beta_scale;
690 }
691 else
692 {
693 CT cos_beta12a = cos_beta2 * cos_beta1 - sin_beta2 * sin_beta1;
694 CT beta12a = atan2(sin_beta12a, cos_beta12a);
695
696 CT m12b = c0;
697 CT m0 = c1;
698 CT dummy;
699 meridian_length(n, ep2, pi + beta12a,
700 sin_beta1, -cos_beta1, dn1,
701 sin_beta2, cos_beta2, dn2,
702 cos_beta1, cos_beta2, dummy,
703 m12b, m0, dummy, dummy, coeffs_C1);
704
705 x = -c1 + m12b / (cos_beta1 * cos_beta2 * m0 * pi);
706 beta_scale = x < -c0_01
707 ? sin_beta12a / x
708 : -f * math::sqr(cos_beta1) * pi;
709 lambda_scale = beta_scale / cos_beta1;
710
711 y = lam12x / lambda_scale;
712 }
713
714 if (y > -tol1 && x > -c1 - x_thresh)
715 {
716 // Strip near cut.
717 if (f >= c0)
718 {
719 sin_alpha1 = (std::min)(c1, -CT(x));
720 cos_alpha1 = - math::sqrt(c1 - math::sqr(sin_alpha1));
721 }
722 else
723 {
724 cos_alpha1 = (std::max)(CT(x > -tol1 ? c0 : -c1), CT(x));
725 sin_alpha1 = math::sqrt(c1 - math::sqr(cos_alpha1));
726 }
727 }
728 else
729 {
730 // Solve the astroid problem.
731 CT k = astroid(CT(x), y);
732
733 CT omega12a = lambda_scale * (f >= c0 ? -x * k /
734 (c1 + k) : -y * (c1 + k) / k);
735
736 sin_omega12 = sin(omega12a);
737 cos_omega12 = -cos(omega12a);
738
739 // Update spherical estimate of alpha1 using omgega12 instead of lam12.
740 sin_alpha1 = cos_beta2 * sin_omega12;
741 cos_alpha1 = sin_beta12a - cos_beta2 * sin_beta1 *
742 math::sqr(sin_omega12) / (c1 - cos_omega12);
743 }
744 }
745
746 // Sanity check on starting guess. Backwards check allows NaN through.
747 if (!(sin_alpha1 <= c0))
748 {
749 math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
750 }
751 else
752 {
753 sin_alpha1 = c1;
754 cos_alpha1 = c0;
755 }
756
757 return sig12;
758 }
759
760 /*
761 Solve the astroid problem using the equation:
762 κ4 + 2κ3 + (1 − x2 − y 2 )κ2 − 2y 2 κ − y 2 = 0.
763
764 For details, please refer to Eq. (65) in,
765 Geodesics on an ellipsoid of revolution, Charles F.F Karney,
766 https://arxiv.org/abs/1102.1215
767 */
astroid(CT const & x,CT const & y)768 static inline CT astroid(CT const& x, CT const& y)
769 {
770 static CT const c0 = 0;
771 static CT const c1 = 1;
772 static CT const c2 = 2;
773 static CT const c3 = 3;
774 static CT const c4 = 4;
775 static CT const c6 = 6;
776
777 CT k;
778
779 CT p = math::sqr(x);
780 CT q = math::sqr(y);
781 CT r = (p + q - c1) / c6;
782
783 if (!(q == c0 && r <= c0))
784 {
785 // Avoid possible division by zero when r = 0 by multiplying
786 // equations for s and t by r^3 and r, respectively.
787 CT S = p * q / c4;
788 CT r2 = math::sqr(r);
789 CT r3 = r * r2;
790
791 // The discriminant of the quadratic equation for T3. This is
792 // zero on the evolute curve p^(1/3)+q^(1/3) = 1.
793 CT discriminant = S * (S + c2 * r3);
794
795 CT u = r;
796
797 if (discriminant >= c0)
798 {
799 CT T3 = S + r3;
800
801 // Pick the sign on the sqrt to maximize abs(T3). This minimizes
802 // loss of precision due to cancellation. The result is unchanged
803 // because of the way the T is used in definition of u.
804 T3 += T3 < c0 ? -std::sqrt(discriminant) : std::sqrt(discriminant);
805
806 CT T = std::cbrt(T3);
807
808 // T can be zero; but then r2 / T -> 0.
809 u += T + (T != c0 ? r2 / T : c0);
810 }
811 else
812 {
813 CT ang = std::atan2(std::sqrt(-discriminant), -(S + r3));
814
815 // There are three possible cube roots. We choose the root which avoids
816 // cancellation. Note that discriminant < 0 implies that r < 0.
817 u += c2 * r * cos(ang / c3);
818 }
819
820 CT v = std::sqrt(math::sqr(u) + q);
821
822 // Avoid loss of accuracy when u < 0.
823 CT uv = u < c0 ? q / (v - u) : u + v;
824 CT w = (uv - q) / (c2 * v);
825
826 // Rearrange expression for k to avoid loss of accuracy due to
827 // subtraction. Division by 0 not possible because uv > 0, w >= 0.
828 k = uv / (std::sqrt(uv + math::sqr(w)) + w);
829 }
830 else // q == 0 && r <= 0
831 {
832 // y = 0 with |x| <= 1. Handle this case directly.
833 // For y small, positive root is k = abs(y)/sqrt(1-x^2).
834 k = c0;
835 }
836 return k;
837 }
838
839 template <typename CoeffsC1>
lambda12(CT const & sin_beta1,CT const & cos_beta1,CT const & dn1,CT const & sin_beta2,CT const & cos_beta2,CT const & dn2,CT const & sin_alpha1,CT cos_alpha1,CT const & sin_lam120,CT const & cos_lam120,CT & sin_alpha2,CT & cos_alpha2,CT & sigma12,CT & sin_sigma1,CT & cos_sigma1,CT & sin_sigma2,CT & cos_sigma2,CT & eps,CT & diff_omega12,bool diffp,CT & diff_lam12,CT const & f,CT const & n,CT const & ep2,CT const & tiny,CoeffsC1 const & coeffs_C1)840 static inline CT lambda12(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
841 CT const& sin_beta2, CT const& cos_beta2, CT const& dn2,
842 CT const& sin_alpha1, CT cos_alpha1,
843 CT const& sin_lam120, CT const& cos_lam120,
844 CT& sin_alpha2, CT& cos_alpha2,
845 CT& sigma12,
846 CT& sin_sigma1, CT& cos_sigma1,
847 CT& sin_sigma2, CT& cos_sigma2,
848 CT& eps, CT& diff_omega12,
849 bool diffp, CT& diff_lam12,
850 CT const& f, CT const& n, CT const& ep2, CT const& tiny,
851 CoeffsC1 const& coeffs_C1)
852 {
853 static CT const c0 = 0;
854 static CT const c1 = 1;
855 static CT const c2 = 2;
856
857 CT const one_minus_f = c1 - f;
858
859 if (sin_beta1 == c0 && cos_alpha1 == c0)
860 {
861 // Break degeneracy of equatorial line.
862 cos_alpha1 = -tiny;
863 }
864
865
866 CT sin_alpha0 = sin_alpha1 * cos_beta1;
867 CT cos_alpha0 = boost::math::hypot(cos_alpha1, sin_alpha1 * sin_beta1);
868
869 CT sin_omega1, cos_omega1;
870 CT sin_omega2, cos_omega2;
871 CT sin_omega12, cos_omega12;
872
873 CT lam12;
874
875 sin_sigma1 = sin_beta1;
876 sin_omega1 = sin_alpha0 * sin_beta1;
877
878 cos_sigma1 = cos_omega1 = cos_alpha1 * cos_beta1;
879
880 math::normalize_unit_vector<CT>(sin_sigma1, cos_sigma1);
881
882 // Enforce symmetries in the case abs(beta2) = -beta1.
883 // Otherwise, this can yield singularities in the Newton iteration.
884
885 // sin(alpha2) * cos(beta2) = sin(alpha0).
886 sin_alpha2 = cos_beta2 != cos_beta1 ?
887 sin_alpha0 / cos_beta2 : sin_alpha1;
888
889 cos_alpha2 = cos_beta2 != cos_beta1 || std::abs(sin_beta2) != -sin_beta1 ?
890 sqrt(math::sqr(cos_alpha1 * cos_beta1) +
891 (cos_beta1 < -sin_beta1 ?
892 (cos_beta2 - cos_beta1) * (cos_beta1 + cos_beta2) :
893 (sin_beta1 - sin_beta2) * (sin_beta1 + sin_beta2))) / cos_beta2 :
894 std::abs(cos_alpha1);
895
896 sin_sigma2 = sin_beta2;
897 sin_omega2 = sin_alpha0 * sin_beta2;
898
899 cos_sigma2 = cos_omega2 =
900 (cos_alpha2 * cos_beta2);
901
902 // Break degeneracy of equatorial line.
903 math::normalize_unit_vector<CT>(sin_sigma2, cos_sigma2);
904
905
906 // sig12 = sig2 - sig1, limit to [0, pi].
907 sigma12 = atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
908 cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);
909
910 // omg12 = omg2 - omg1, limit to [0, pi].
911 sin_omega12 = (std::max)(c0, cos_omega1 * sin_omega2 - sin_omega1 * cos_omega2);
912 cos_omega12 = cos_omega1 * cos_omega2 + sin_omega1 * sin_omega2;
913
914 // eta = omg12 - lam120.
915 CT eta = atan2(sin_omega12 * cos_lam120 - cos_omega12 * sin_lam120,
916 cos_omega12 * cos_lam120 + sin_omega12 * sin_lam120);
917
918 CT B312;
919 CT k2 = math::sqr(cos_alpha0) * ep2;
920
921 eps = k2 / (c2 * (c1 + std::sqrt(c1 + k2)) + k2);
922
923 se::coeffs_C3<SeriesOrder, CT> const coeffs_C3(n, eps);
924
925 B312 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C3)
926 - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C3);
927
928 se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);
929
930 CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());
931
932 diff_omega12 = -f * A3 * sin_alpha0 * (sigma12 + B312);
933 lam12 = eta + diff_omega12;
934
935 if (diffp)
936 {
937 if (cos_alpha2 == c0)
938 {
939 diff_lam12 = - c2 * one_minus_f * dn1 / sin_beta1;
940 }
941 else
942 {
943 CT dummy;
944 meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
945 sin_sigma2, cos_sigma2, dn2,
946 cos_beta1, cos_beta2, dummy,
947 diff_lam12, dummy, dummy,
948 dummy, coeffs_C1);
949
950 diff_lam12 *= one_minus_f / (cos_alpha2 * cos_beta2);
951 }
952 }
953 return lam12;
954 }
955
956 };
957
958 } // namespace detail
959
960 /*!
961 \brief The solution of the inverse problem of geodesics on latlong coordinates,
962 after Karney (2011).
963 \author See
964 - Charles F.F Karney, Algorithms for geodesics, 2011
965 https://arxiv.org/pdf/1109.4448.pdf
966 */
967
968 template <
969 typename CT,
970 bool EnableDistance,
971 bool EnableAzimuth,
972 bool EnableReverseAzimuth = false,
973 bool EnableReducedLength = false,
974 bool EnableGeodesicScale = false
975 >
976 struct karney_inverse
977 : detail::karney_inverse
978 <
979 CT,
980 EnableDistance,
981 EnableAzimuth,
982 EnableReverseAzimuth,
983 EnableReducedLength,
984 EnableGeodesicScale
985 >
986 {};
987
988 }}} // namespace boost::geometry::formula
989
990
991 #endif // BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
992