src/GeodesicExact.cpp
Go to the documentation of this file.
1 
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 
38 namespace 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 ? Math::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 (!(Math::isfinite(_a) && _a > 0))
87  throw GeographicErr("Equatorial radius is not positive");
88  if (!(Math::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 
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 
120  unsigned caps) const {
121  return GeodesicLineExact(*this, lat1, lon1, azi1, caps);
122  }
123 
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 
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.
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  sig12 = m12x = s12x = 0;
289  m12x *= _b;
290  s12x *= _b;
291  a12 = sig12 / Math::degree();
292  } else
293  // m12 < 0, i.e., prolate and too close to anti-podal
294  meridian = false;
295  }
296 
297  // somg12 > 1 marks that it needs to be calculated
298  real omg12 = 0, somg12 = 2, comg12 = 0;
299  if (!meridian &&
300  sbet1 == 0 && // and sbet2 == 0
301  (_f <= 0 || lon12s >= _f * 180)) {
302 
303  // Geodesic runs along equator
304  calp1 = calp2 = 0; salp1 = salp2 = 1;
305  s12x = _a * lam12;
306  sig12 = omg12 = lam12 / _f1;
307  m12x = _b * sin(sig12);
308  if (outmask & GEODESICSCALE)
309  M12 = M21 = cos(sig12);
310  a12 = lon12 / _f1;
311 
312  } else if (!meridian) {
313 
314  // Now point1 and point2 belong within a hemisphere bounded by a
315  // meridian and geodesic is neither meridional or equatorial.
316 
317  // Figure a starting point for Newton's method
318  real dnm;
319  sig12 = InverseStart(E, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
320  lam12, slam12, clam12,
321  salp1, calp1, salp2, calp2, dnm);
322 
323  if (sig12 >= 0) {
324  // Short lines (InverseStart sets salp2, calp2, dnm)
325  s12x = sig12 * _b * dnm;
326  m12x = Math::sq(dnm) * _b * sin(sig12 / dnm);
327  if (outmask & GEODESICSCALE)
328  M12 = M21 = cos(sig12 / dnm);
329  a12 = sig12 / Math::degree();
330  omg12 = lam12 / (_f1 * dnm);
331  } else {
332 
333  // Newton's method. This is a straightforward solution of f(alp1) =
334  // lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one
335  // root in the interval (0, pi) and its derivative is positive at the
336  // root. Thus f(alp) is positive for alp > alp1 and negative for alp <
337  // alp1. During the course of the iteration, a range (alp1a, alp1b) is
338  // maintained which brackets the root and with each evaluation of
339  // f(alp) the range is shrunk, if possible. Newton's method is
340  // restarted whenever the derivative of f is negative (because the new
341  // value of alp1 is then further from the solution) or if the new
342  // estimate of alp1 lies outside (0,pi); in this case, the new starting
343  // guess is taken to be (alp1a + alp1b) / 2.
344  //
345  // initial values to suppress warnings (if loop is executed 0 times)
346  real ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, domg12 = 0;
347  unsigned numit = 0;
348  // Bracketing range
349  real salp1a = tiny_, calp1a = 1, salp1b = tiny_, calp1b = -1;
350  for (bool tripn = false, tripb = false;
351  numit < maxit2_ || GEOGRAPHICLIB_PANIC;
352  ++numit) {
353  // 1/4 meridian = 10e6 m and random input. max err is estimated max
354  // error in nm (checking solution of inverse problem by direct
355  // solution). iter is mean and sd of number of iterations
356  //
357  // max iter
358  // log2(b/a) err mean sd
359  // -7 387 5.33 3.68
360  // -6 345 5.19 3.43
361  // -5 269 5.00 3.05
362  // -4 210 4.76 2.44
363  // -3 115 4.55 1.87
364  // -2 69 4.35 1.38
365  // -1 36 4.05 1.03
366  // 0 15 0.01 0.13
367  // 1 25 5.10 1.53
368  // 2 96 5.61 2.09
369  // 3 318 6.02 2.74
370  // 4 985 6.24 3.22
371  // 5 2352 6.32 3.44
372  // 6 6008 6.30 3.45
373  // 7 19024 6.19 3.30
374  real dv;
375  real v = Lambda12(sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
376  slam12, clam12,
377  salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
378  E, domg12, numit < maxit1_, dv);
379  // Reversed test to allow escape with NaNs
380  if (tripb || !(abs(v) >= (tripn ? 8 : 1) * tol0_)) break;
381  // Update bracketing values
382  if (v > 0 && (numit > maxit1_ || calp1/salp1 > calp1b/salp1b))
383  { salp1b = salp1; calp1b = calp1; }
384  else if (v < 0 && (numit > maxit1_ || calp1/salp1 < calp1a/salp1a))
385  { salp1a = salp1; calp1a = calp1; }
386  if (numit < maxit1_ && dv > 0) {
387  real
388  dalp1 = -v/dv;
389  real
390  sdalp1 = sin(dalp1), cdalp1 = cos(dalp1),
391  nsalp1 = salp1 * cdalp1 + calp1 * sdalp1;
392  if (nsalp1 > 0 && abs(dalp1) < Math::pi()) {
393  calp1 = calp1 * cdalp1 - salp1 * sdalp1;
394  salp1 = nsalp1;
395  Math::norm(salp1, calp1);
396  // In some regimes we don't get quadratic convergence because
397  // slope -> 0. So use convergence conditions based on epsilon
398  // instead of sqrt(epsilon).
399  tripn = abs(v) <= 16 * tol0_;
400  continue;
401  }
402  }
403  // Either dv was not positive or updated value was outside legal
404  // range. Use the midpoint of the bracket as the next estimate.
405  // This mechanism is not needed for the WGS84 ellipsoid, but it does
406  // catch problems with more eccentric ellipsoids. Its efficacy is
407  // such for the WGS84 test set with the starting guess set to alp1 =
408  // 90deg:
409  // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
410  // WGS84 and random input: mean = 4.74, sd = 0.99
411  salp1 = (salp1a + salp1b)/2;
412  calp1 = (calp1a + calp1b)/2;
413  Math::norm(salp1, calp1);
414  tripn = false;
415  tripb = (abs(salp1a - salp1) + (calp1a - calp1) < tolb_ ||
416  abs(salp1 - salp1b) + (calp1 - calp1b) < tolb_);
417  }
418  {
419  real dummy;
420  Lengths(E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
421  cbet1, cbet2, outmask, s12x, m12x, dummy, M12, M21);
422  }
423  m12x *= _b;
424  s12x *= _b;
425  a12 = sig12 / Math::degree();
426  if (outmask & AREA) {
427  // omg12 = lam12 - domg12
428  real sdomg12 = sin(domg12), cdomg12 = cos(domg12);
429  somg12 = slam12 * cdomg12 - clam12 * sdomg12;
430  comg12 = clam12 * cdomg12 + slam12 * sdomg12;
431  }
432  }
433  }
434 
435  if (outmask & DISTANCE)
436  s12 = 0 + s12x; // Convert -0 to 0
437 
438  if (outmask & REDUCEDLENGTH)
439  m12 = 0 + m12x; // Convert -0 to 0
440 
441  if (outmask & AREA) {
442  real
443  // From Lambda12: sin(alp1) * cos(bet1) = sin(alp0)
444  salp0 = salp1 * cbet1,
445  calp0 = Math::hypot(calp1, salp1 * sbet1); // calp0 > 0
446  real alp12;
447  if (calp0 != 0 && salp0 != 0) {
448  real
449  // From Lambda12: tan(bet) = tan(sig) * cos(alp)
450  ssig1 = sbet1, csig1 = calp1 * cbet1,
451  ssig2 = sbet2, csig2 = calp2 * cbet2,
452  k2 = Math::sq(calp0) * _ep2,
453  eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2),
454  // Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0).
455  A4 = Math::sq(_a) * calp0 * salp0 * _e2;
456  Math::norm(ssig1, csig1);
457  Math::norm(ssig2, csig2);
458  real C4a[nC4_];
459  C4f(eps, C4a);
460  real
461  B41 = CosSeries(ssig1, csig1, C4a, nC4_),
462  B42 = CosSeries(ssig2, csig2, C4a, nC4_);
463  S12 = A4 * (B42 - B41);
464  } else
465  // Avoid problems with indeterminate sig1, sig2 on equator
466  S12 = 0;
467 
468  if (!meridian) {
469  if (somg12 > 1) {
470  somg12 = sin(omg12); comg12 = cos(omg12);
471  }
472  }
473 
474  if (!meridian &&
475  // omg12 < 3/4 * pi
476  comg12 > -real(0.7071) && // Long difference not too big
477  sbet2 - sbet1 < real(1.75)) { // Lat difference not too big
478  // Use tan(Gamma/2) = tan(omg12/2)
479  // * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
480  // with tan(x/2) = sin(x)/(1+cos(x))
481  real domg12 = 1 + comg12, dbet1 = 1 + cbet1, dbet2 = 1 + cbet2;
482  alp12 = 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
483  domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) );
484  } else {
485  // alp12 = alp2 - alp1, used in atan2 so no need to normalize
486  real
487  salp12 = salp2 * calp1 - calp2 * salp1,
488  calp12 = calp2 * calp1 + salp2 * salp1;
489  // The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
490  // salp12 = -0 and alp12 = -180. However this depends on the sign
491  // being attached to 0 correctly. The following ensures the correct
492  // behavior.
493  if (salp12 == 0 && calp12 < 0) {
494  salp12 = tiny_ * calp1;
495  calp12 = -1;
496  }
497  alp12 = atan2(salp12, calp12);
498  }
499  S12 += _c2 * alp12;
500  S12 *= swapp * lonsign * latsign;
501  // Convert -0 to 0
502  S12 += 0;
503  }
504 
505  // Convert calp, salp to azimuth accounting for lonsign, swapp, latsign.
506  if (swapp < 0) {
507  swap(salp1, salp2);
508  swap(calp1, calp2);
509  if (outmask & GEODESICSCALE)
510  swap(M12, M21);
511  }
512 
513  salp1 *= swapp * lonsign; calp1 *= swapp * latsign;
514  salp2 *= swapp * lonsign; calp2 *= swapp * latsign;
515 
516  // Returned value in [0, 180]
517  return a12;
518  }
519 
521  real lat2, real lon2,
522  unsigned outmask,
523  real& s12, real& azi1, real& azi2,
524  real& m12, real& M12, real& M21,
525  real& S12) const {
526  outmask &= OUT_MASK;
527  real salp1, calp1, salp2, calp2,
528  a12 = GenInverse(lat1, lon1, lat2, lon2,
529  outmask, s12, salp1, calp1, salp2, calp2,
530  m12, M12, M21, S12);
531  if (outmask & AZIMUTH) {
532  azi1 = Math::atan2d(salp1, calp1);
533  azi2 = Math::atan2d(salp2, calp2);
534  }
535  return a12;
536  }
537 
539  real lat2, real lon2,
540  unsigned caps) const {
541  real t, salp1, calp1, salp2, calp2,
542  a12 = GenInverse(lat1, lon1, lat2, lon2,
543  // No need to specify AZIMUTH here
544  0u, t, salp1, calp1, salp2, calp2,
545  t, t, t, t),
546  azi1 = Math::atan2d(salp1, calp1);
547  // Ensure that a12 can be converted to a distance
548  if (caps & (OUT_MASK & DISTANCE_IN)) caps |= DISTANCE;
549  return GeodesicLineExact(*this, lat1, lon1, azi1, salp1, calp1, caps,
550  true, a12);
551  }
552 
554  real sig12,
555  real ssig1, real csig1, real dn1,
556  real ssig2, real csig2, real dn2,
557  real cbet1, real cbet2, unsigned outmask,
558  real& s12b, real& m12b, real& m0,
559  real& M12, real& M21) const {
560  // Return m12b = (reduced length)/_b; also calculate s12b = distance/_b,
561  // and m0 = coefficient of secular term in expression for reduced length.
562 
563  outmask &= OUT_ALL;
564  // outmask & DISTANCE: set s12b
565  // outmask & REDUCEDLENGTH: set m12b & m0
566  // outmask & GEODESICSCALE: set M12 & M21
567 
568  // It's OK to have repeated dummy arguments,
569  // e.g., s12b = m0 = M12 = M21 = dummy
570 
571  if (outmask & DISTANCE)
572  // Missing a factor of _b
573  s12b = E.E() / (Math::pi() / 2) *
574  (sig12 + (E.deltaE(ssig2, csig2, dn2) - E.deltaE(ssig1, csig1, dn1)));
575  if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
576  real
577  m0x = - E.k2() * E.D() / (Math::pi() / 2),
578  J12 = m0x *
579  (sig12 + (E.deltaD(ssig2, csig2, dn2) - E.deltaD(ssig1, csig1, dn1)));
580  if (outmask & REDUCEDLENGTH) {
581  m0 = m0x;
582  // Missing a factor of _b. Add parens around (csig1 * ssig2) and
583  // (ssig1 * csig2) to ensure accurate cancellation in the case of
584  // coincident points.
585  m12b = dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) -
586  csig1 * csig2 * J12;
587  }
588  if (outmask & GEODESICSCALE) {
589  real csig12 = csig1 * csig2 + ssig1 * ssig2;
590  real t = _ep2 * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2);
591  M12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1;
592  M21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2;
593  }
594  }
595  }
596 
598  // Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
599  // This solution is adapted from Geocentric::Reverse.
600  real k;
601  real
602  p = Math::sq(x),
603  q = Math::sq(y),
604  r = (p + q - 1) / 6;
605  if ( !(q == 0 && r <= 0) ) {
606  real
607  // Avoid possible division by zero when r = 0 by multiplying equations
608  // for s and t by r^3 and r, resp.
609  S = p * q / 4, // S = r^3 * s
610  r2 = Math::sq(r),
611  r3 = r * r2,
612  // The discriminant of the quadratic equation for T3. This is zero on
613  // the evolute curve p^(1/3)+q^(1/3) = 1
614  disc = S * (S + 2 * r3);
615  real u = r;
616  if (disc >= 0) {
617  real T3 = S + r3;
618  // Pick the sign on the sqrt to maximize abs(T3). This minimizes loss
619  // of precision due to cancellation. The result is unchanged because
620  // of the way the T is used in definition of u.
621  T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
622  // N.B. cbrt always returns the real root. cbrt(-8) = -2.
623  real T = Math::cbrt(T3); // T = r * t
624  // T can be zero; but then r2 / T -> 0.
625  u += T + (T != 0 ? r2 / T : 0);
626  } else {
627  // T is complex, but the way u is defined the result is real.
628  real ang = atan2(sqrt(-disc), -(S + r3));
629  // There are three possible cube roots. We choose the root which
630  // avoids cancellation. Note that disc < 0 implies that r < 0.
631  u += 2 * r * cos(ang / 3);
632  }
633  real
634  v = sqrt(Math::sq(u) + q), // guaranteed positive
635  // Avoid loss of accuracy when u < 0.
636  uv = u < 0 ? q / (v - u) : u + v, // u+v, guaranteed positive
637  w = (uv - q) / (2 * v); // positive?
638  // Rearrange expression for k to avoid loss of accuracy due to
639  // subtraction. Division by 0 not possible because uv > 0, w >= 0.
640  k = uv / (sqrt(uv + Math::sq(w)) + w); // guaranteed positive
641  } else { // q == 0 && r <= 0
642  // y = 0 with |x| <= 1. Handle this case directly.
643  // for y small, positive root is k = abs(y)/sqrt(1-x^2)
644  k = 0;
645  }
646  return k;
647  }
648 
650  real sbet1, real cbet1, real dn1,
651  real sbet2, real cbet2, real dn2,
652  real lam12, real slam12, real clam12,
653  real& salp1, real& calp1,
654  // Only updated if return val >= 0
655  real& salp2, real& calp2,
656  // Only updated for short lines
657  real& dnm) const {
658  // Return a starting point for Newton's method in salp1 and calp1 (function
659  // value is -1). If Newton's method doesn't need to be used, return also
660  // salp2 and calp2 and function value is sig12.
661  real
662  sig12 = -1, // Return value
663  // bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0]
664  sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
665  cbet12 = cbet2 * cbet1 + sbet2 * sbet1;
666 #if defined(__GNUC__) && __GNUC__ == 4 && \
667  (__GNUC_MINOR__ < 6 || defined(__MINGW32__))
668  // Volatile declaration needed to fix inverse cases
669  // 88.202499451857 0 -88.202499451857 179.981022032992859592
670  // 89.262080389218 0 -89.262080389218 179.992207982775375662
671  // 89.333123580033 0 -89.333123580032997687 179.99295812360148422
672  // which otherwise fail with g++ 4.4.4 x86 -O3 (Linux)
673  // and g++ 4.4.0 (mingw) and g++ 4.6.1 (tdm mingw).
674  real sbet12a;
675  {
676  GEOGRAPHICLIB_VOLATILE real xx1 = sbet2 * cbet1;
677  GEOGRAPHICLIB_VOLATILE real xx2 = cbet2 * sbet1;
678  sbet12a = xx1 + xx2;
679  }
680 #else
681  real sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
682 #endif
683  bool shortline = cbet12 >= 0 && sbet12 < real(0.5) &&
684  cbet2 * lam12 < real(0.5);
685  real somg12, comg12;
686  if (shortline) {
687  real sbetm2 = Math::sq(sbet1 + sbet2);
688  // sin((bet1+bet2)/2)^2
689  // = (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2)
690  sbetm2 /= sbetm2 + Math::sq(cbet1 + cbet2);
691  dnm = sqrt(1 + _ep2 * sbetm2);
692  real omg12 = lam12 / (_f1 * dnm);
693  somg12 = sin(omg12); comg12 = cos(omg12);
694  } else {
695  somg12 = slam12; comg12 = clam12;
696  }
697 
698  salp1 = cbet2 * somg12;
699  calp1 = comg12 >= 0 ?
700  sbet12 + cbet2 * sbet1 * Math::sq(somg12) / (1 + comg12) :
701  sbet12a - cbet2 * sbet1 * Math::sq(somg12) / (1 - comg12);
702 
703  real
704  ssig12 = Math::hypot(salp1, calp1),
705  csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
706 
707  if (shortline && ssig12 < _etol2) {
708  // really short lines
709  salp2 = cbet1 * somg12;
710  calp2 = sbet12 - cbet1 * sbet2 *
711  (comg12 >= 0 ? Math::sq(somg12) / (1 + comg12) : 1 - comg12);
712  Math::norm(salp2, calp2);
713  // Set return value
714  sig12 = atan2(ssig12, csig12);
715  } else if (abs(_n) > real(0.1) || // Skip astroid calc if too eccentric
716  csig12 >= 0 ||
717  ssig12 >= 6 * abs(_n) * Math::pi() * Math::sq(cbet1)) {
718  // Nothing to do, zeroth order spherical approximation is OK
719  } else {
720  // Scale lam12 and bet2 to x, y coordinate system where antipodal point
721  // is at origin and singular point is at y = 0, x = -1.
722  real y, lamscale, betscale;
723  // Volatile declaration needed to fix inverse case
724  // 56.320923501171 0 -56.320923501171 179.664747671772880215
725  // which otherwise fails with g++ 4.4.4 x86 -O3
727  real lam12x = atan2(-slam12, -clam12); // lam12 - pi
728  if (_f >= 0) { // In fact f == 0 does not get here
729  // x = dlong, y = dlat
730  {
731  real k2 = Math::sq(sbet1) * _ep2;
732  E.Reset(-k2, -_ep2, 1 + k2, 1 + _ep2);
733  lamscale = _e2/_f1 * cbet1 * 2 * E.H();
734  }
735  betscale = lamscale * cbet1;
736 
737  x = lam12x / lamscale;
738  y = sbet12a / betscale;
739  } else { // _f < 0
740  // x = dlat, y = dlong
741  real
742  cbet12a = cbet2 * cbet1 - sbet2 * sbet1,
743  bet12a = atan2(sbet12a, cbet12a);
744  real m12b, m0, dummy;
745  // In the case of lon12 = 180, this repeats a calculation made in
746  // Inverse.
747  Lengths(E, Math::pi() + bet12a,
748  sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
749  cbet1, cbet2, REDUCEDLENGTH, dummy, m12b, m0, dummy, dummy);
750  x = -1 + m12b / (cbet1 * cbet2 * m0 * Math::pi());
751  betscale = x < -real(0.01) ? sbet12a / x :
752  -_f * Math::sq(cbet1) * Math::pi();
753  lamscale = betscale / cbet1;
754  y = lam12x / lamscale;
755  }
756 
757  if (y > -tol1_ && x > -1 - xthresh_) {
758  // strip near cut
759  // Need real(x) here to cast away the volatility of x for min/max
760  if (_f >= 0) {
761  salp1 = min(real(1), -real(x)); calp1 = - sqrt(1 - Math::sq(salp1));
762  } else {
763  calp1 = max(real(x > -tol1_ ? 0 : -1), real(x));
764  salp1 = sqrt(1 - Math::sq(calp1));
765  }
766  } else {
767  // Estimate alp1, by solving the astroid problem.
768  //
769  // Could estimate alpha1 = theta + pi/2, directly, i.e.,
770  // calp1 = y/k; salp1 = -x/(1+k); for _f >= 0
771  // calp1 = x/(1+k); salp1 = -y/k; for _f < 0 (need to check)
772  //
773  // However, it's better to estimate omg12 from astroid and use
774  // spherical formula to compute alp1. This reduces the mean number of
775  // Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
776  // (min 0 max 5). The changes in the number of iterations are as
777  // follows:
778  //
779  // change percent
780  // 1 5
781  // 0 78
782  // -1 16
783  // -2 0.6
784  // -3 0.04
785  // -4 0.002
786  //
787  // The histogram of iterations is (m = number of iterations estimating
788  // alp1 directly, n = number of iterations estimating via omg12, total
789  // number of trials = 148605):
790  //
791  // iter m n
792  // 0 148 186
793  // 1 13046 13845
794  // 2 93315 102225
795  // 3 36189 32341
796  // 4 5396 7
797  // 5 455 1
798  // 6 56 0
799  //
800  // Because omg12 is near pi, estimate work with omg12a = pi - omg12
801  real k = Astroid(x, y);
802  real
803  omg12a = lamscale * ( _f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k );
804  somg12 = sin(omg12a); comg12 = -cos(omg12a);
805  // Update spherical estimate of alp1 using omg12 instead of lam12
806  salp1 = cbet2 * somg12;
807  calp1 = sbet12a - cbet2 * sbet1 * Math::sq(somg12) / (1 - comg12);
808  }
809  }
810  // Sanity check on starting guess. Backwards check allows NaN through.
811  if (!(salp1 <= 0))
812  Math::norm(salp1, calp1);
813  else {
814  salp1 = 1; calp1 = 0;
815  }
816  return sig12;
817  }
818 
820  real sbet2, real cbet2, real dn2,
821  real salp1, real calp1,
822  real slam120, real clam120,
823  real& salp2, real& calp2,
824  real& sig12,
825  real& ssig1, real& csig1,
826  real& ssig2, real& csig2,
828  real& domg12,
829  bool diffp, real& dlam12) const
830  {
831 
832  if (sbet1 == 0 && calp1 == 0)
833  // Break degeneracy of equatorial line. This case has already been
834  // handled.
835  calp1 = -tiny_;
836 
837  real
838  // sin(alp1) * cos(bet1) = sin(alp0)
839  salp0 = salp1 * cbet1,
840  calp0 = Math::hypot(calp1, salp1 * sbet1); // calp0 > 0
841 
842  real somg1, comg1, somg2, comg2, somg12, comg12, cchi1, cchi2, lam12;
843  // tan(bet1) = tan(sig1) * cos(alp1)
844  // tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1)
845  ssig1 = sbet1; somg1 = salp0 * sbet1;
846  csig1 = comg1 = calp1 * cbet1;
847  // Without normalization we have schi1 = somg1.
848  cchi1 = _f1 * dn1 * comg1;
849  Math::norm(ssig1, csig1);
850  // Math::norm(somg1, comg1); -- don't need to normalize!
851  // Math::norm(schi1, cchi1); -- don't need to normalize!
852 
853  // Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful
854  // about this case, since this can yield singularities in the Newton
855  // iteration.
856  // sin(alp2) * cos(bet2) = sin(alp0)
857  salp2 = cbet2 != cbet1 ? salp0 / cbet2 : salp1;
858  // calp2 = sqrt(1 - sq(salp2))
859  // = sqrt(sq(calp0) - sq(sbet2)) / cbet2
860  // and subst for calp0 and rearrange to give (choose positive sqrt
861  // to give alp2 in [0, pi/2]).
862  calp2 = cbet2 != cbet1 || abs(sbet2) != -sbet1 ?
863  sqrt(Math::sq(calp1 * cbet1) +
864  (cbet1 < -sbet1 ?
865  (cbet2 - cbet1) * (cbet1 + cbet2) :
866  (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
867  abs(calp1);
868  // tan(bet2) = tan(sig2) * cos(alp2)
869  // tan(omg2) = sin(alp0) * tan(sig2).
870  ssig2 = sbet2; somg2 = salp0 * sbet2;
871  csig2 = comg2 = calp2 * cbet2;
872  // Without normalization we have schi2 = somg2.
873  cchi2 = _f1 * dn2 * comg2;
874  Math::norm(ssig2, csig2);
875  // Math::norm(somg2, comg2); -- don't need to normalize!
876  // Math::norm(schi2, cchi2); -- don't need to normalize!
877 
878  // sig12 = sig2 - sig1, limit to [0, pi]
879  sig12 = atan2(max(real(0), csig1 * ssig2 - ssig1 * csig2),
880  csig1 * csig2 + ssig1 * ssig2);
881 
882  // omg12 = omg2 - omg1, limit to [0, pi]
883  somg12 = max(real(0), comg1 * somg2 - somg1 * comg2);
884  comg12 = comg1 * comg2 + somg1 * somg2;
885  real k2 = Math::sq(calp0) * _ep2;
886  E.Reset(-k2, -_ep2, 1 + k2, 1 + _ep2);
887  // chi12 = chi2 - chi1, limit to [0, pi]
888  real
889  schi12 = max(real(0), cchi1 * somg2 - somg1 * cchi2),
890  cchi12 = cchi1 * cchi2 + somg1 * somg2;
891  // eta = chi12 - lam120
892  real eta = atan2(schi12 * clam120 - cchi12 * slam120,
893  cchi12 * clam120 + schi12 * slam120);
894  real deta12 = -_e2/_f1 * salp0 * E.H() / (Math::pi() / 2) *
895  (sig12 + (E.deltaH(ssig2, csig2, dn2) - E.deltaH(ssig1, csig1, dn1)));
896  lam12 = eta + deta12;
897  // domg12 = deta12 + chi12 - omg12
898  domg12 = deta12 + atan2(schi12 * comg12 - cchi12 * somg12,
899  cchi12 * comg12 + schi12 * somg12);
900  if (diffp) {
901  if (calp2 == 0)
902  dlam12 = - 2 * _f1 * dn1 / sbet1;
903  else {
904  real dummy;
905  Lengths(E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
906  cbet1, cbet2, REDUCEDLENGTH,
907  dummy, dlam12, dummy, dummy, dummy);
908  dlam12 *= _f1 / (calp2 * cbet2);
909  }
910  }
911 
912  return lam12;
913  }
914 
915  void GeodesicExact::C4f(real eps, real c[]) const {
916  // Evaluate C4 coeffs
917  // Elements c[0] thru c[nC4_ - 1] are set
918  real mult = 1;
919  int o = 0;
920  for (int l = 0; l < nC4_; ++l) { // l is index of C4[l]
921  int m = nC4_ - l - 1; // order of polynomial in eps
922  c[l] = mult * Math::polyval(m, _C4x + o, eps);
923  o += m + 1;
924  mult *= eps;
925  }
926  // Post condition: o == nC4x_
927  if (!(o == nC4x_))
928  throw GeographicErr("C4 misalignment");
929  }
930 
931 } // namespace GeographicLib
static T AngNormalize(T x)
Definition: Math.hpp:440
Matrix3f m
#define max(a, b)
Definition: datatypes.h:20
static T pi()
Definition: Math.hpp:202
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
real GenInverse(real lat1, real lon1, real lat2, real lon2, unsigned outmask, real &s12, real &salp1, real &calp1, real &salp2, real &calp2, real &m12, real &M12, real &M21, real &S12) const
Scalar * y
static real CosSeries(real sinx, real cosx, const real c[], int n)
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
void Reset(real k2=0, real alpha2=0)
static T cbrt(T x)
Definition: Math.hpp:345
real InverseStart(EllipticFunction &E, real sbet1, real cbet1, real dn1, real sbet2, real cbet2, real dn2, real lam12, real slam12, real clam12, real &salp1, real &calp1, real &salp2, real &calp2, real &dnm) const
#define min(a, b)
Definition: datatypes.h:19
static bool isfinite(T x)
Definition: Math.hpp:806
GeodesicLineExact InverseLine(real lat1, real lon1, real lat2, real lon2, unsigned caps=ALL) const
int n
static T LatFix(T x)
Definition: Math.hpp:467
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Math::real deltaE(real sn, real cn, real dn) const
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Mathematical functions needed by GeographicLib.
Definition: Math.hpp:102
void Lengths(const EllipticFunction &E, real sig12, real ssig1, real csig1, real dn1, real ssig2, real csig2, real dn2, real cbet1, real cbet2, unsigned outmask, real &s12s, real &m12a, real &m0, real &M12, real &M21) const
Definition: BFloat16.h:88
static void sincosd(T x, T &sinx, T &cosx)
Definition: Math.hpp:558
static T AngDiff(T x, T y, T &e)
Definition: Math.hpp:486
Elliptic integrals and functions.
GeodesicLineExact GenDirectLine(real lat1, real lon1, real azi1, bool arcmode, real s12_a12, unsigned caps=ALL) const
DiscreteKey S(1, 2)
static void norm(T &x, T &y)
Definition: Math.hpp:384
#define GEOGRAPHICLIB_VOLATILE
Definition: Math.hpp:84
static double epsilon
Definition: testRot3.cpp:37
EIGEN_DEVICE_FUNC const AtanReturnType atan() const
static real Astroid(real x, real y)
static const Line3 l(Rot3(), 1, 1)
int EIGEN_BLAS_FUNC() swap(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:130
real Lambda12(real sbet1, real cbet1, real dn1, real sbet2, real cbet2, real dn2, real salp1, real calp1, real slam120, real clam120, real &salp2, real &calp2, real &sig12, real &ssig1, real &csig1, real &ssig2, real &csig2, EllipticFunction &E, real &domg12, bool diffp, real &dlam12) const
static T hypot(T x, T y)
Definition: Math.hpp:243
static T sq(T x)
Definition: Math.hpp:232
Header for GeographicLib::GeodesicLineExact class.
Array< int, Dynamic, 1 > v
static T atan2d(T y, T x)
Definition: Math.hpp:691
Eigen::Triplet< double > T
static T polyval(int N, const T p[], T x)
Definition: Math.hpp:425
Definition: main.h:100
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Namespace for GeographicLib.
static const double r2
EIGEN_DEVICE_FUNC const Scalar & q
static const unsigned maxit1_
static T degree()
Definition: Math.hpp:216
static const double r3
Math::real deltaH(real sn, real cn, real dn) const
RowVector3d w
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Exact geodesic calculations.
DiscreteKey E(5, 2)
Header for GeographicLib::GeodesicExact class.
void C4f(real k2, real c[]) const
Exception handling for GeographicLib.
Definition: Constants.hpp:389
GeodesicLineExact DirectLine(real lat1, real lon1, real azi1, real s12, unsigned caps=ALL) const
float * p
Math::real deltaD(real sn, real cn, real dn) const
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
GeodesicLineExact ArcDirectLine(real lat1, real lon1, real azi1, real a12, 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 T AngRound(T x)
Definition: Math.hpp:535
Point2 t(10, 10)
#define GEOGRAPHICLIB_PANIC
Definition: Math.hpp:87
static const GeodesicExact & WGS84()


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:17