GeographicLib 1.52
Loading...
Searching...
No Matches
GeodesicExact.cpp
Go to the documentation of this file.
1/**
2 * \file GeodesicExact.cpp
3 * \brief Implementation for GeographicLib::GeodesicExact class
4 *
5 * Copyright (c) Charles Karney (2012-2021) <charles@karney.com> and licensed
6 * under the MIT/X11 License. For more information, see
7 * https://geographiclib.sourceforge.io/
8 *
9 * This is a reformulation of the geodesic problem. The notation is as
10 * follows:
11 * - at a general point (no suffix or 1 or 2 as suffix)
12 * - phi = latitude
13 * - beta = latitude on auxiliary sphere
14 * - omega = longitude on auxiliary sphere
15 * - lambda = longitude
16 * - alpha = azimuth of great circle
17 * - sigma = arc length along great circle
18 * - s = distance
19 * - tau = scaled distance (= sigma at multiples of pi/2)
20 * - at northwards equator crossing
21 * - beta = phi = 0
22 * - omega = lambda = 0
23 * - alpha = alpha0
24 * - sigma = s = 0
25 * - a 12 suffix means a difference, e.g., s12 = s2 - s1.
26 * - s and c prefixes mean sin and cos
27 **********************************************************************/
28
31
32#if defined(_MSC_VER)
33// Squelch warnings about potentially uninitialized local variables and
34// constant conditional expressions
35# pragma warning (disable: 4701 4127)
36#endif
37
38namespace GeographicLib {
39
40 using namespace std;
41
43 : maxit2_(maxit1_ + Math::digits() + 10)
44 // Underflow guard. We require
45 // tiny_ * epsilon() > 0
46 // tiny_ + epsilon() == epsilon()
47 , tiny_(sqrt(numeric_limits<real>::min()))
48 , tol0_(numeric_limits<real>::epsilon())
49 // Increase multiplier in defn of tol1_ from 100 to 200 to fix inverse
50 // case 52.784459512564 0 -52.784459512563990912 179.634407464943777557
51 // which otherwise failed for Visual Studio 10 (Release and Debug)
52 , tol1_(200 * tol0_)
53 , tol2_(sqrt(tol0_))
54 , tolb_(tol0_ * tol2_) // Check on bisection interval
55 , xthresh_(1000 * tol2_)
56 , _a(a)
57 , _f(f)
58 , _f1(1 - _f)
59 , _e2(_f * (2 - _f))
60 , _ep2(_e2 / Math::sq(_f1)) // e2 / (1 - e2)
61 , _n(_f / ( 2 - _f))
62 , _b(_a * _f1)
63 // The Geodesic class substitutes atanh(sqrt(e2)) for asinh(sqrt(ep2)) in
64 // the definition of _c2. The latter is more accurate for very oblate
65 // ellipsoids (which the Geodesic class does not attempt to handle). Of
66 // course, the area calculation in GeodesicExact is still based on a
67 // series and so only holds for moderately oblate (or prolate)
68 // ellipsoids.
69 , _c2((Math::sq(_a) + Math::sq(_b) *
70 (_f == 0 ? 1 :
71 (_f > 0 ? asinh(sqrt(_ep2)) : atan(sqrt(-_e2))) /
72 sqrt(abs(_e2))))/2) // authalic radius squared
73 // The sig12 threshold for "really short". Using the auxiliary sphere
74 // solution with dnm computed at (bet1 + bet2) / 2, the relative error in
75 // the azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2.
76 // (Error measured for 1/100 < b/a < 100 and abs(f) >= 1/1000. For a
77 // given f and sig12, the max error occurs for lines near the pole. If
78 // the old rule for computing dnm = (dn1 + dn2)/2 is used, then the error
79 // increases by a factor of 2.) Setting this equal to epsilon gives
80 // sig12 = etol2. Here 0.1 is a safety factor (error decreased by 100)
81 // and max(0.001, abs(f)) stops etol2 getting too large in the nearly
82 // spherical case.
83 , _etol2(real(0.1) * tol2_ /
84 sqrt( max(real(0.001), abs(_f)) * min(real(1), 1 - _f/2) / 2 ))
85 {
86 if (!(isfinite(_a) && _a > 0))
87 throw GeographicErr("Equatorial radius is not positive");
88 if (!(isfinite(_b) && _b > 0))
89 throw GeographicErr("Polar semi-axis is not positive");
90 C4coeff();
91 }
92
94 static const GeodesicExact wgs84(Constants::WGS84_a(),
96 return wgs84;
97 }
98
99 Math::real GeodesicExact::CosSeries(real sinx, real cosx,
100 const real c[], int n) {
101 // Evaluate
102 // y = sum(c[i] * cos((2*i+1) * x), i, 0, n-1)
103 // using Clenshaw summation.
104 // Approx operation count = (n + 5) mult and (2 * n + 2) add
105 c += n ; // Point to one beyond last element
106 real
107 ar = 2 * (cosx - sinx) * (cosx + sinx), // 2 * cos(2 * x)
108 y0 = n & 1 ? *--c : 0, y1 = 0; // accumulators for sum
109 // Now n is even
110 n /= 2;
111 while (n--) {
112 // Unroll loop x 2, so accumulators return to their original role
113 y1 = ar * y0 - y1 + *--c;
114 y0 = ar * y1 - y0 + *--c;
115 }
116 return cosx * (y0 - y1); // cos(x) * (y0 - y1)
117 }
118
119 GeodesicLineExact GeodesicExact::Line(real lat1, real lon1, real azi1,
120 unsigned caps) const {
121 return GeodesicLineExact(*this, lat1, lon1, azi1, caps);
122 }
123
124 Math::real GeodesicExact::GenDirect(real lat1, real lon1, real azi1,
125 bool arcmode, real s12_a12,
126 unsigned outmask,
127 real& lat2, real& lon2, real& azi2,
128 real& s12, real& m12,
129 real& M12, real& M21,
130 real& S12) const {
131 // Automatically supply DISTANCE_IN if necessary
132 if (!arcmode) outmask |= DISTANCE_IN;
133 return GeodesicLineExact(*this, lat1, lon1, azi1, outmask)
134 . // Note the dot!
135 GenPosition(arcmode, s12_a12, outmask,
136 lat2, lon2, azi2, s12, m12, M12, M21, S12);
137 }
138
140 real azi1,
141 bool arcmode, real s12_a12,
142 unsigned caps) const {
143 azi1 = Math::AngNormalize(azi1);
144 real salp1, calp1;
145 // Guard against underflow in salp0. Also -0 is converted to +0.
146 Math::sincosd(Math::AngRound(azi1), salp1, calp1);
147 // Automatically supply DISTANCE_IN if necessary
148 if (!arcmode) caps |= DISTANCE_IN;
149 return GeodesicLineExact(*this, lat1, lon1, azi1, salp1, calp1,
150 caps, arcmode, s12_a12);
151 }
152
154 real azi1, real s12,
155 unsigned caps) const {
156 return GenDirectLine(lat1, lon1, azi1, false, s12, caps);
157 }
158
160 real azi1, real a12,
161 unsigned caps) const {
162 return GenDirectLine(lat1, lon1, azi1, true, a12, caps);
163 }
164
165 Math::real GeodesicExact::GenInverse(real lat1, real lon1,
166 real lat2, real lon2,
167 unsigned outmask, real& s12,
168 real& salp1, real& calp1,
169 real& salp2, real& calp2,
170 real& m12, real& M12, real& M21,
171 real& S12) const {
172 // Compute longitude difference (AngDiff does this carefully). Result is
173 // in [-180, 180] but -180 is only for west-going geodesics. 180 is for
174 // east-going and meridional geodesics.
175 real lon12s, lon12 = Math::AngDiff(lon1, lon2, lon12s);
176 // Make longitude difference positive.
177 int lonsign = lon12 >= 0 ? 1 : -1;
178 // If very close to being on the same half-meridian, then make it so.
179 lon12 = lonsign * Math::AngRound(lon12);
180 lon12s = Math::AngRound((180 - lon12) - lonsign * lon12s);
181 real
182 lam12 = lon12 * Math::degree(),
183 slam12, clam12;
184 if (lon12 > 90) {
185 Math::sincosd(lon12s, slam12, clam12);
186 clam12 = -clam12;
187 } else
188 Math::sincosd(lon12, slam12, clam12);
189
190 // If really close to the equator, treat as on equator.
191 lat1 = Math::AngRound(Math::LatFix(lat1));
192 lat2 = Math::AngRound(Math::LatFix(lat2));
193 // Swap points so that point with higher (abs) latitude is point 1
194 // If one latitude is a nan, then it becomes lat1.
195 int swapp = abs(lat1) < abs(lat2) ? -1 : 1;
196 if (swapp < 0) {
197 lonsign *= -1;
198 swap(lat1, lat2);
199 }
200 // Make lat1 <= 0
201 int latsign = lat1 < 0 ? 1 : -1;
202 lat1 *= latsign;
203 lat2 *= latsign;
204 // Now we have
205 //
206 // 0 <= lon12 <= 180
207 // -90 <= lat1 <= 0
208 // lat1 <= lat2 <= -lat1
209 //
210 // longsign, swapp, latsign register the transformation to bring the
211 // coordinates to this canonical form. In all cases, 1 means no change was
212 // made. We make these transformations so that there are few cases to
213 // check, e.g., on verifying quadrants in atan2. In addition, this
214 // enforces some symmetries in the results returned.
215
216 real sbet1, cbet1, sbet2, cbet2, s12x, m12x;
217 // Initialize for the meridian. No longitude calculation is done in this
218 // case to let the parameter default to 0.
219 EllipticFunction E(-_ep2);
220
221 Math::sincosd(lat1, sbet1, cbet1); sbet1 *= _f1;
222 // Ensure cbet1 = +epsilon at poles; doing the fix on beta means that sig12
223 // will be <= 2*tiny for two points at the same pole.
224 Math::norm(sbet1, cbet1); cbet1 = max(tiny_, cbet1);
225
226 Math::sincosd(lat2, sbet2, cbet2); sbet2 *= _f1;
227 // Ensure cbet2 = +epsilon at poles
228 Math::norm(sbet2, cbet2); cbet2 = max(tiny_, cbet2);
229
230 // If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the
231 // |bet1| - |bet2|. Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is
232 // a better measure. This logic is used in assigning calp2 in Lambda12.
233 // Sometimes these quantities vanish and in that case we force bet2 = +/-
234 // bet1 exactly. An example where is is necessary is the inverse problem
235 // 48.522876735459 0 -48.52287673545898293 179.599720456223079643
236 // which failed with Visual Studio 10 (Release and Debug)
237
238 if (cbet1 < -sbet1) {
239 if (cbet2 == cbet1)
240 sbet2 = sbet2 < 0 ? sbet1 : -sbet1;
241 } else {
242 if (abs(sbet2) == -sbet1)
243 cbet2 = cbet1;
244 }
245
246 real
247 dn1 = (_f >= 0 ? sqrt(1 + _ep2 * Math::sq(sbet1)) :
248 sqrt(1 - _e2 * Math::sq(cbet1)) / _f1),
249 dn2 = (_f >= 0 ? sqrt(1 + _ep2 * Math::sq(sbet2)) :
250 sqrt(1 - _e2 * Math::sq(cbet2)) / _f1);
251
252 real a12, sig12;
253
254 bool meridian = lat1 == -90 || slam12 == 0;
255
256 if (meridian) {
257
258 // Endpoints are on a single full meridian, so the geodesic might lie on
259 // a meridian.
260
261 calp1 = clam12; salp1 = slam12; // Head to the target longitude
262 calp2 = 1; salp2 = 0; // At the target we're heading north
263
264 real
265 // tan(bet) = tan(sig) * cos(alp)
266 ssig1 = sbet1, csig1 = calp1 * cbet1,
267 ssig2 = sbet2, csig2 = calp2 * cbet2;
268
269 // sig12 = sig2 - sig1
270 sig12 = atan2(max(real(0), csig1 * ssig2 - ssig1 * csig2),
271 csig1 * csig2 + ssig1 * ssig2);
272 {
273 real dummy;
274 Lengths(E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
275 cbet1, cbet2, outmask | REDUCEDLENGTH,
276 s12x, m12x, dummy, M12, M21);
277 }
278 // Add the check for sig12 since zero length geodesics might yield m12 <
279 // 0. Test case was
280 //
281 // echo 20.001 0 20.001 0 | GeodSolve -i
282 //
283 // In fact, we will have sig12 > pi/2 for meridional geodesic which is
284 // not a shortest path.
285 if (sig12 < 1 || m12x >= 0) {
286 // Need at least 2, to handle 90 0 90 180
287 if (sig12 < 3 * tiny_ ||
288 // Prevent negative s12 or m12 for short lines
289 (sig12 < tol0_ && (s12x < 0 || m12x < 0)))
290 sig12 = m12x = s12x = 0;
291 m12x *= _b;
292 s12x *= _b;
293 a12 = sig12 / Math::degree();
294 } else
295 // m12 < 0, i.e., prolate and too close to anti-podal
296 meridian = false;
297 }
298
299 // somg12 > 1 marks that it needs to be calculated
300 real omg12 = 0, somg12 = 2, comg12 = 0;
301 if (!meridian &&
302 sbet1 == 0 && // and sbet2 == 0
303 (_f <= 0 || lon12s >= _f * 180)) {
304
305 // Geodesic runs along equator
306 calp1 = calp2 = 0; salp1 = salp2 = 1;
307 s12x = _a * lam12;
308 sig12 = omg12 = lam12 / _f1;
309 m12x = _b * sin(sig12);
310 if (outmask & GEODESICSCALE)
311 M12 = M21 = cos(sig12);
312 a12 = lon12 / _f1;
313
314 } else if (!meridian) {
315
316 // Now point1 and point2 belong within a hemisphere bounded by a
317 // meridian and geodesic is neither meridional or equatorial.
318
319 // Figure a starting point for Newton's method
320 real dnm;
321 sig12 = InverseStart(E, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
322 lam12, slam12, clam12,
323 salp1, calp1, salp2, calp2, dnm);
324
325 if (sig12 >= 0) {
326 // Short lines (InverseStart sets salp2, calp2, dnm)
327 s12x = sig12 * _b * dnm;
328 m12x = Math::sq(dnm) * _b * sin(sig12 / dnm);
329 if (outmask & GEODESICSCALE)
330 M12 = M21 = cos(sig12 / dnm);
331 a12 = sig12 / Math::degree();
332 omg12 = lam12 / (_f1 * dnm);
333 } else {
334
335 // Newton's method. This is a straightforward solution of f(alp1) =
336 // lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one
337 // root in the interval (0, pi) and its derivative is positive at the
338 // root. Thus f(alp) is positive for alp > alp1 and negative for alp <
339 // alp1. During the course of the iteration, a range (alp1a, alp1b) is
340 // maintained which brackets the root and with each evaluation of
341 // f(alp) the range is shrunk, if possible. Newton's method is
342 // restarted whenever the derivative of f is negative (because the new
343 // value of alp1 is then further from the solution) or if the new
344 // estimate of alp1 lies outside (0,pi); in this case, the new starting
345 // guess is taken to be (alp1a + alp1b) / 2.
346 //
347 // initial values to suppress warnings (if loop is executed 0 times)
348 real ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, domg12 = 0;
349 unsigned numit = 0;
350 // Bracketing range
351 real salp1a = tiny_, calp1a = 1, salp1b = tiny_, calp1b = -1;
352 for (bool tripn = false, tripb = false;
353 numit < maxit2_ || GEOGRAPHICLIB_PANIC;
354 ++numit) {
355 // 1/4 meridian = 10e6 m and random input. max err is estimated max
356 // error in nm (checking solution of inverse problem by direct
357 // solution). iter is mean and sd of number of iterations
358 //
359 // max iter
360 // log2(b/a) err mean sd
361 // -7 387 5.33 3.68
362 // -6 345 5.19 3.43
363 // -5 269 5.00 3.05
364 // -4 210 4.76 2.44
365 // -3 115 4.55 1.87
366 // -2 69 4.35 1.38
367 // -1 36 4.05 1.03
368 // 0 15 0.01 0.13
369 // 1 25 5.10 1.53
370 // 2 96 5.61 2.09
371 // 3 318 6.02 2.74
372 // 4 985 6.24 3.22
373 // 5 2352 6.32 3.44
374 // 6 6008 6.30 3.45
375 // 7 19024 6.19 3.30
376 real dv;
377 real v = Lambda12(sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
378 slam12, clam12,
379 salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
380 E, domg12, numit < maxit1_, dv);
381 // Reversed test to allow escape with NaNs
382 if (tripb || !(abs(v) >= (tripn ? 8 : 1) * tol0_)) break;
383 // Update bracketing values
384 if (v > 0 && (numit > maxit1_ || calp1/salp1 > calp1b/salp1b))
385 { salp1b = salp1; calp1b = calp1; }
386 else if (v < 0 && (numit > maxit1_ || calp1/salp1 < calp1a/salp1a))
387 { salp1a = salp1; calp1a = calp1; }
388 if (numit < maxit1_ && dv > 0) {
389 real
390 dalp1 = -v/dv;
391 real
392 sdalp1 = sin(dalp1), cdalp1 = cos(dalp1),
393 nsalp1 = salp1 * cdalp1 + calp1 * sdalp1;
394 if (nsalp1 > 0 && abs(dalp1) < Math::pi()) {
395 calp1 = calp1 * cdalp1 - salp1 * sdalp1;
396 salp1 = nsalp1;
397 Math::norm(salp1, calp1);
398 // In some regimes we don't get quadratic convergence because
399 // slope -> 0. So use convergence conditions based on epsilon
400 // instead of sqrt(epsilon).
401 tripn = abs(v) <= 16 * tol0_;
402 continue;
403 }
404 }
405 // Either dv was not positive or updated value was outside legal
406 // range. Use the midpoint of the bracket as the next estimate.
407 // This mechanism is not needed for the WGS84 ellipsoid, but it does
408 // catch problems with more eccentric ellipsoids. Its efficacy is
409 // such for the WGS84 test set with the starting guess set to alp1 =
410 // 90deg:
411 // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
412 // WGS84 and random input: mean = 4.74, sd = 0.99
413 salp1 = (salp1a + salp1b)/2;
414 calp1 = (calp1a + calp1b)/2;
415 Math::norm(salp1, calp1);
416 tripn = false;
417 tripb = (abs(salp1a - salp1) + (calp1a - calp1) < tolb_ ||
418 abs(salp1 - salp1b) + (calp1 - calp1b) < tolb_);
419 }
420 {
421 real dummy;
422 Lengths(E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
423 cbet1, cbet2, outmask, s12x, m12x, dummy, M12, M21);
424 }
425 m12x *= _b;
426 s12x *= _b;
427 a12 = sig12 / Math::degree();
428 if (outmask & AREA) {
429 // omg12 = lam12 - domg12
430 real sdomg12 = sin(domg12), cdomg12 = cos(domg12);
431 somg12 = slam12 * cdomg12 - clam12 * sdomg12;
432 comg12 = clam12 * cdomg12 + slam12 * sdomg12;
433 }
434 }
435 }
436
437 if (outmask & DISTANCE)
438 s12 = 0 + s12x; // Convert -0 to 0
439
440 if (outmask & REDUCEDLENGTH)
441 m12 = 0 + m12x; // Convert -0 to 0
442
443 if (outmask & AREA) {
444 real
445 // From Lambda12: sin(alp1) * cos(bet1) = sin(alp0)
446 salp0 = salp1 * cbet1,
447 calp0 = hypot(calp1, salp1 * sbet1); // calp0 > 0
448 real alp12;
449 if (calp0 != 0 && salp0 != 0) {
450 real
451 // From Lambda12: tan(bet) = tan(sig) * cos(alp)
452 ssig1 = sbet1, csig1 = calp1 * cbet1,
453 ssig2 = sbet2, csig2 = calp2 * cbet2,
454 k2 = Math::sq(calp0) * _ep2,
455 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2),
456 // Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0).
457 A4 = Math::sq(_a) * calp0 * salp0 * _e2;
458 Math::norm(ssig1, csig1);
459 Math::norm(ssig2, csig2);
460 real C4a[nC4_];
461 C4f(eps, C4a);
462 real
463 B41 = CosSeries(ssig1, csig1, C4a, nC4_),
464 B42 = CosSeries(ssig2, csig2, C4a, nC4_);
465 S12 = A4 * (B42 - B41);
466 } else
467 // Avoid problems with indeterminate sig1, sig2 on equator
468 S12 = 0;
469
470 if (!meridian) {
471 if (somg12 > 1) {
472 somg12 = sin(omg12); comg12 = cos(omg12);
473 }
474 }
475
476 if (!meridian &&
477 // omg12 < 3/4 * pi
478 comg12 > -real(0.7071) && // Long difference not too big
479 sbet2 - sbet1 < real(1.75)) { // Lat difference not too big
480 // Use tan(Gamma/2) = tan(omg12/2)
481 // * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
482 // with tan(x/2) = sin(x)/(1+cos(x))
483 real domg12 = 1 + comg12, dbet1 = 1 + cbet1, dbet2 = 1 + cbet2;
484 alp12 = 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
485 domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) );
486 } else {
487 // alp12 = alp2 - alp1, used in atan2 so no need to normalize
488 real
489 salp12 = salp2 * calp1 - calp2 * salp1,
490 calp12 = calp2 * calp1 + salp2 * salp1;
491 // The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
492 // salp12 = -0 and alp12 = -180. However this depends on the sign
493 // being attached to 0 correctly. The following ensures the correct
494 // behavior.
495 if (salp12 == 0 && calp12 < 0) {
496 salp12 = tiny_ * calp1;
497 calp12 = -1;
498 }
499 alp12 = atan2(salp12, calp12);
500 }
501 S12 += _c2 * alp12;
502 S12 *= swapp * lonsign * latsign;
503 // Convert -0 to 0
504 S12 += 0;
505 }
506
507 // Convert calp, salp to azimuth accounting for lonsign, swapp, latsign.
508 if (swapp < 0) {
509 swap(salp1, salp2);
510 swap(calp1, calp2);
511 if (outmask & GEODESICSCALE)
512 swap(M12, M21);
513 }
514
515 salp1 *= swapp * lonsign; calp1 *= swapp * latsign;
516 salp2 *= swapp * lonsign; calp2 *= swapp * latsign;
517
518 // Returned value in [0, 180]
519 return a12;
520 }
521
522 Math::real GeodesicExact::GenInverse(real lat1, real lon1,
523 real lat2, real lon2,
524 unsigned outmask,
525 real& s12, real& azi1, real& azi2,
526 real& m12, real& M12, real& M21,
527 real& S12) const {
528 outmask &= OUT_MASK;
529 real salp1, calp1, salp2, calp2,
530 a12 = GenInverse(lat1, lon1, lat2, lon2,
531 outmask, s12, salp1, calp1, salp2, calp2,
532 m12, M12, M21, S12);
533 if (outmask & AZIMUTH) {
534 azi1 = Math::atan2d(salp1, calp1);
535 azi2 = Math::atan2d(salp2, calp2);
536 }
537 return a12;
538 }
539
541 real lat2, real lon2,
542 unsigned caps) const {
543 real t, salp1, calp1, salp2, calp2,
544 a12 = GenInverse(lat1, lon1, lat2, lon2,
545 // No need to specify AZIMUTH here
546 0u, t, salp1, calp1, salp2, calp2,
547 t, t, t, t),
548 azi1 = Math::atan2d(salp1, calp1);
549 // Ensure that a12 can be converted to a distance
550 if (caps & (OUT_MASK & DISTANCE_IN)) caps |= DISTANCE;
551 return GeodesicLineExact(*this, lat1, lon1, azi1, salp1, calp1, caps,
552 true, a12);
553 }
554
555 void GeodesicExact::Lengths(const EllipticFunction& E,
556 real sig12,
557 real ssig1, real csig1, real dn1,
558 real ssig2, real csig2, real dn2,
559 real cbet1, real cbet2, unsigned outmask,
560 real& s12b, real& m12b, real& m0,
561 real& M12, real& M21) const {
562 // Return m12b = (reduced length)/_b; also calculate s12b = distance/_b,
563 // and m0 = coefficient of secular term in expression for reduced length.
564
565 outmask &= OUT_ALL;
566 // outmask & DISTANCE: set s12b
567 // outmask & REDUCEDLENGTH: set m12b & m0
568 // outmask & GEODESICSCALE: set M12 & M21
569
570 // It's OK to have repeated dummy arguments,
571 // e.g., s12b = m0 = M12 = M21 = dummy
572
573 if (outmask & DISTANCE)
574 // Missing a factor of _b
575 s12b = E.E() / (Math::pi() / 2) *
576 (sig12 + (E.deltaE(ssig2, csig2, dn2) - E.deltaE(ssig1, csig1, dn1)));
577 if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
578 real
579 m0x = - E.k2() * E.D() / (Math::pi() / 2),
580 J12 = m0x *
581 (sig12 + (E.deltaD(ssig2, csig2, dn2) - E.deltaD(ssig1, csig1, dn1)));
582 if (outmask & REDUCEDLENGTH) {
583 m0 = m0x;
584 // Missing a factor of _b. Add parens around (csig1 * ssig2) and
585 // (ssig1 * csig2) to ensure accurate cancellation in the case of
586 // coincident points.
587 m12b = dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) -
588 csig1 * csig2 * J12;
589 }
590 if (outmask & GEODESICSCALE) {
591 real csig12 = csig1 * csig2 + ssig1 * ssig2;
592 real t = _ep2 * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2);
593 M12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1;
594 M21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2;
595 }
596 }
597 }
598
599 Math::real GeodesicExact::Astroid(real x, real y) {
600 // Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
601 // This solution is adapted from Geocentric::Reverse.
602 real k;
603 real
604 p = Math::sq(x),
605 q = Math::sq(y),
606 r = (p + q - 1) / 6;
607 if ( !(q == 0 && r <= 0) ) {
608 real
609 // Avoid possible division by zero when r = 0 by multiplying equations
610 // for s and t by r^3 and r, resp.
611 S = p * q / 4, // S = r^3 * s
612 r2 = Math::sq(r),
613 r3 = r * r2,
614 // The discriminant of the quadratic equation for T3. This is zero on
615 // the evolute curve p^(1/3)+q^(1/3) = 1
616 disc = S * (S + 2 * r3);
617 real u = r;
618 if (disc >= 0) {
619 real T3 = S + r3;
620 // Pick the sign on the sqrt to maximize abs(T3). This minimizes loss
621 // of precision due to cancellation. The result is unchanged because
622 // of the way the T is used in definition of u.
623 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
624 // N.B. cbrt always returns the real root. cbrt(-8) = -2.
625 real T = cbrt(T3); // T = r * t
626 // T can be zero; but then r2 / T -> 0.
627 u += T + (T != 0 ? r2 / T : 0);
628 } else {
629 // T is complex, but the way u is defined the result is real.
630 real ang = atan2(sqrt(-disc), -(S + r3));
631 // There are three possible cube roots. We choose the root which
632 // avoids cancellation. Note that disc < 0 implies that r < 0.
633 u += 2 * r * cos(ang / 3);
634 }
635 real
636 v = sqrt(Math::sq(u) + q), // guaranteed positive
637 // Avoid loss of accuracy when u < 0.
638 uv = u < 0 ? q / (v - u) : u + v, // u+v, guaranteed positive
639 w = (uv - q) / (2 * v); // positive?
640 // Rearrange expression for k to avoid loss of accuracy due to
641 // subtraction. Division by 0 not possible because uv > 0, w >= 0.
642 k = uv / (sqrt(uv + Math::sq(w)) + w); // guaranteed positive
643 } else { // q == 0 && r <= 0
644 // y = 0 with |x| <= 1. Handle this case directly.
645 // for y small, positive root is k = abs(y)/sqrt(1-x^2)
646 k = 0;
647 }
648 return k;
649 }
650
651 Math::real GeodesicExact::InverseStart(EllipticFunction& E,
652 real sbet1, real cbet1, real dn1,
653 real sbet2, real cbet2, real dn2,
654 real lam12, real slam12, real clam12,
655 real& salp1, real& calp1,
656 // Only updated if return val >= 0
657 real& salp2, real& calp2,
658 // Only updated for short lines
659 real& dnm) const {
660 // Return a starting point for Newton's method in salp1 and calp1 (function
661 // value is -1). If Newton's method doesn't need to be used, return also
662 // salp2 and calp2 and function value is sig12.
663 real
664 sig12 = -1, // Return value
665 // bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0]
666 sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
667 cbet12 = cbet2 * cbet1 + sbet2 * sbet1;
668 real sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
669 bool shortline = cbet12 >= 0 && sbet12 < real(0.5) &&
670 cbet2 * lam12 < real(0.5);
671 real somg12, comg12;
672 if (shortline) {
673 real sbetm2 = Math::sq(sbet1 + sbet2);
674 // sin((bet1+bet2)/2)^2
675 // = (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2)
676 sbetm2 /= sbetm2 + Math::sq(cbet1 + cbet2);
677 dnm = sqrt(1 + _ep2 * sbetm2);
678 real omg12 = lam12 / (_f1 * dnm);
679 somg12 = sin(omg12); comg12 = cos(omg12);
680 } else {
681 somg12 = slam12; comg12 = clam12;
682 }
683
684 salp1 = cbet2 * somg12;
685 calp1 = comg12 >= 0 ?
686 sbet12 + cbet2 * sbet1 * Math::sq(somg12) / (1 + comg12) :
687 sbet12a - cbet2 * sbet1 * Math::sq(somg12) / (1 - comg12);
688
689 real
690 ssig12 = hypot(salp1, calp1),
691 csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
692
693 if (shortline && ssig12 < _etol2) {
694 // really short lines
695 salp2 = cbet1 * somg12;
696 calp2 = sbet12 - cbet1 * sbet2 *
697 (comg12 >= 0 ? Math::sq(somg12) / (1 + comg12) : 1 - comg12);
698 Math::norm(salp2, calp2);
699 // Set return value
700 sig12 = atan2(ssig12, csig12);
701 } else if (abs(_n) > real(0.1) || // Skip astroid calc if too eccentric
702 csig12 >= 0 ||
703 ssig12 >= 6 * abs(_n) * Math::pi() * Math::sq(cbet1)) {
704 // Nothing to do, zeroth order spherical approximation is OK
705 } else {
706 // Scale lam12 and bet2 to x, y coordinate system where antipodal point
707 // is at origin and singular point is at y = 0, x = -1.
708 real y, lamscale, betscale;
709 // Volatile declaration needed to fix inverse case
710 // 56.320923501171 0 -56.320923501171 179.664747671772880215
711 // which otherwise fails with g++ 4.4.4 x86 -O3
713 real lam12x = atan2(-slam12, -clam12); // lam12 - pi
714 if (_f >= 0) { // In fact f == 0 does not get here
715 // x = dlong, y = dlat
716 {
717 real k2 = Math::sq(sbet1) * _ep2;
718 E.Reset(-k2, -_ep2, 1 + k2, 1 + _ep2);
719 lamscale = _e2/_f1 * cbet1 * 2 * E.H();
720 }
721 betscale = lamscale * cbet1;
722
723 x = lam12x / lamscale;
724 y = sbet12a / betscale;
725 } else { // _f < 0
726 // x = dlat, y = dlong
727 real
728 cbet12a = cbet2 * cbet1 - sbet2 * sbet1,
729 bet12a = atan2(sbet12a, cbet12a);
730 real m12b, m0, dummy;
731 // In the case of lon12 = 180, this repeats a calculation made in
732 // Inverse.
733 Lengths(E, Math::pi() + bet12a,
734 sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
735 cbet1, cbet2, REDUCEDLENGTH, dummy, m12b, m0, dummy, dummy);
736 x = -1 + m12b / (cbet1 * cbet2 * m0 * Math::pi());
737 betscale = x < -real(0.01) ? sbet12a / x :
738 -_f * Math::sq(cbet1) * Math::pi();
739 lamscale = betscale / cbet1;
740 y = lam12x / lamscale;
741 }
742
743 if (y > -tol1_ && x > -1 - xthresh_) {
744 // strip near cut
745 // Need real(x) here to cast away the volatility of x for min/max
746 if (_f >= 0) {
747 salp1 = min(real(1), -real(x)); calp1 = - sqrt(1 - Math::sq(salp1));
748 } else {
749 calp1 = max(real(x > -tol1_ ? 0 : -1), real(x));
750 salp1 = sqrt(1 - Math::sq(calp1));
751 }
752 } else {
753 // Estimate alp1, by solving the astroid problem.
754 //
755 // Could estimate alpha1 = theta + pi/2, directly, i.e.,
756 // calp1 = y/k; salp1 = -x/(1+k); for _f >= 0
757 // calp1 = x/(1+k); salp1 = -y/k; for _f < 0 (need to check)
758 //
759 // However, it's better to estimate omg12 from astroid and use
760 // spherical formula to compute alp1. This reduces the mean number of
761 // Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
762 // (min 0 max 5). The changes in the number of iterations are as
763 // follows:
764 //
765 // change percent
766 // 1 5
767 // 0 78
768 // -1 16
769 // -2 0.6
770 // -3 0.04
771 // -4 0.002
772 //
773 // The histogram of iterations is (m = number of iterations estimating
774 // alp1 directly, n = number of iterations estimating via omg12, total
775 // number of trials = 148605):
776 //
777 // iter m n
778 // 0 148 186
779 // 1 13046 13845
780 // 2 93315 102225
781 // 3 36189 32341
782 // 4 5396 7
783 // 5 455 1
784 // 6 56 0
785 //
786 // Because omg12 is near pi, estimate work with omg12a = pi - omg12
787 real k = Astroid(x, y);
788 real
789 omg12a = lamscale * ( _f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k );
790 somg12 = sin(omg12a); comg12 = -cos(omg12a);
791 // Update spherical estimate of alp1 using omg12 instead of lam12
792 salp1 = cbet2 * somg12;
793 calp1 = sbet12a - cbet2 * sbet1 * Math::sq(somg12) / (1 - comg12);
794 }
795 }
796 // Sanity check on starting guess. Backwards check allows NaN through.
797 if (!(salp1 <= 0))
798 Math::norm(salp1, calp1);
799 else {
800 salp1 = 1; calp1 = 0;
801 }
802 return sig12;
803 }
804
805 Math::real GeodesicExact::Lambda12(real sbet1, real cbet1, real dn1,
806 real sbet2, real cbet2, real dn2,
807 real salp1, real calp1,
808 real slam120, real clam120,
809 real& salp2, real& calp2,
810 real& sig12,
811 real& ssig1, real& csig1,
812 real& ssig2, real& csig2,
813 EllipticFunction& E,
814 real& domg12,
815 bool diffp, real& dlam12) const
816 {
817
818 if (sbet1 == 0 && calp1 == 0)
819 // Break degeneracy of equatorial line. This case has already been
820 // handled.
821 calp1 = -tiny_;
822
823 real
824 // sin(alp1) * cos(bet1) = sin(alp0)
825 salp0 = salp1 * cbet1,
826 calp0 = hypot(calp1, salp1 * sbet1); // calp0 > 0
827
828 real somg1, comg1, somg2, comg2, somg12, comg12, cchi1, cchi2, lam12;
829 // tan(bet1) = tan(sig1) * cos(alp1)
830 // tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1)
831 ssig1 = sbet1; somg1 = salp0 * sbet1;
832 csig1 = comg1 = calp1 * cbet1;
833 // Without normalization we have schi1 = somg1.
834 cchi1 = _f1 * dn1 * comg1;
835 Math::norm(ssig1, csig1);
836 // Math::norm(somg1, comg1); -- don't need to normalize!
837 // Math::norm(schi1, cchi1); -- don't need to normalize!
838
839 // Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful
840 // about this case, since this can yield singularities in the Newton
841 // iteration.
842 // sin(alp2) * cos(bet2) = sin(alp0)
843 salp2 = cbet2 != cbet1 ? salp0 / cbet2 : salp1;
844 // calp2 = sqrt(1 - sq(salp2))
845 // = sqrt(sq(calp0) - sq(sbet2)) / cbet2
846 // and subst for calp0 and rearrange to give (choose positive sqrt
847 // to give alp2 in [0, pi/2]).
848 calp2 = cbet2 != cbet1 || abs(sbet2) != -sbet1 ?
849 sqrt(Math::sq(calp1 * cbet1) +
850 (cbet1 < -sbet1 ?
851 (cbet2 - cbet1) * (cbet1 + cbet2) :
852 (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
853 abs(calp1);
854 // tan(bet2) = tan(sig2) * cos(alp2)
855 // tan(omg2) = sin(alp0) * tan(sig2).
856 ssig2 = sbet2; somg2 = salp0 * sbet2;
857 csig2 = comg2 = calp2 * cbet2;
858 // Without normalization we have schi2 = somg2.
859 cchi2 = _f1 * dn2 * comg2;
860 Math::norm(ssig2, csig2);
861 // Math::norm(somg2, comg2); -- don't need to normalize!
862 // Math::norm(schi2, cchi2); -- don't need to normalize!
863
864 // sig12 = sig2 - sig1, limit to [0, pi]
865 sig12 = atan2(max(real(0), csig1 * ssig2 - ssig1 * csig2),
866 csig1 * csig2 + ssig1 * ssig2);
867
868 // omg12 = omg2 - omg1, limit to [0, pi]
869 somg12 = max(real(0), comg1 * somg2 - somg1 * comg2);
870 comg12 = comg1 * comg2 + somg1 * somg2;
871 real k2 = Math::sq(calp0) * _ep2;
872 E.Reset(-k2, -_ep2, 1 + k2, 1 + _ep2);
873 // chi12 = chi2 - chi1, limit to [0, pi]
874 real
875 schi12 = max(real(0), cchi1 * somg2 - somg1 * cchi2),
876 cchi12 = cchi1 * cchi2 + somg1 * somg2;
877 // eta = chi12 - lam120
878 real eta = atan2(schi12 * clam120 - cchi12 * slam120,
879 cchi12 * clam120 + schi12 * slam120);
880 real deta12 = -_e2/_f1 * salp0 * E.H() / (Math::pi() / 2) *
881 (sig12 + (E.deltaH(ssig2, csig2, dn2) - E.deltaH(ssig1, csig1, dn1)));
882 lam12 = eta + deta12;
883 // domg12 = deta12 + chi12 - omg12
884 domg12 = deta12 + atan2(schi12 * comg12 - cchi12 * somg12,
885 cchi12 * comg12 + schi12 * somg12);
886 if (diffp) {
887 if (calp2 == 0)
888 dlam12 = - 2 * _f1 * dn1 / sbet1;
889 else {
890 real dummy;
891 Lengths(E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
892 cbet1, cbet2, REDUCEDLENGTH,
893 dummy, dlam12, dummy, dummy, dummy);
894 dlam12 *= _f1 / (calp2 * cbet2);
895 }
896 }
897
898 return lam12;
899 }
900
901 void GeodesicExact::C4f(real eps, real c[]) const {
902 // Evaluate C4 coeffs
903 // Elements c[0] thru c[nC4_ - 1] are set
904 real mult = 1;
905 int o = 0;
906 for (int l = 0; l < nC4_; ++l) { // l is index of C4[l]
907 int m = nC4_ - l - 1; // order of polynomial in eps
908 c[l] = mult * Math::polyval(m, _C4x + o, eps);
909 o += m + 1;
910 mult *= eps;
911 }
912 // Post condition: o == nC4x_
913 if (!(o == nC4x_))
914 throw GeographicErr("C4 misalignment");
915 }
916
917} // namespace GeographicLib
GeographicLib::Math::real real
Definition: GeodSolve.cpp:31
Header for GeographicLib::GeodesicExact class.
Header for GeographicLib::GeodesicLineExact class.
#define GEOGRAPHICLIB_VOLATILE
Definition: Math.hpp:58
#define GEOGRAPHICLIB_PANIC
Definition: Math.hpp:61
Elliptic integrals and functions.
Math::real deltaE(real sn, real cn, real dn) const
Math::real deltaD(real sn, real cn, real dn) const
Exact geodesic calculations.
GeodesicLineExact InverseLine(real lat1, real lon1, real lat2, real lon2, unsigned caps=ALL) const
GeodesicLineExact GenDirectLine(real lat1, real lon1, real azi1, bool arcmode, real s12_a12, unsigned caps=ALL) const
GeodesicLineExact DirectLine(real lat1, real lon1, real azi1, real s12, unsigned caps=ALL) const
Math::real GenDirect(real lat1, real lon1, real azi1, bool arcmode, real s12_a12, unsigned outmask, real &lat2, real &lon2, real &azi2, real &s12, real &m12, real &M12, real &M21, real &S12) const
GeodesicLineExact Line(real lat1, real lon1, real azi1, unsigned caps=ALL) const
static const GeodesicExact & WGS84()
GeodesicLineExact ArcDirectLine(real lat1, real lon1, real azi1, real a12, unsigned caps=ALL) const
Exception handling for GeographicLib.
Definition: Constants.hpp:315
Mathematical functions needed by GeographicLib.
Definition: Math.hpp:76
static T AngNormalize(T x)
Definition: Math.hpp:420
static T degree()
Definition: Math.hpp:159
static T LatFix(T x)
Definition: Math.hpp:433
static void sincosd(T x, T &sinx, T &cosx)
Definition: Math.cpp:126
static T atan2d(T y, T x)
Definition: Math.cpp:180
static void norm(T &x, T &y)
Definition: Math.hpp:355
static T AngRound(T x)
Definition: Math.cpp:117
static T sq(T x)
Definition: Math.hpp:171
static T pi()
Definition: Math.hpp:149
static T polyval(int N, const T p[], T x)
Definition: Math.hpp:402
static T AngDiff(T x, T y, T &e)
Definition: Math.hpp:452
Namespace for GeographicLib.
Definition: Accumulator.cpp:12
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)