C library for Geodesics  1.44
geodesic.c
Go to the documentation of this file.
1 /**
2  * \file geodesic.c
3  * \brief Implementation of the geodesic routines in C
4  *
5  * For the full documentation see geodesic.h.
6  **********************************************************************/
7 
8 /** @cond SKIP */
9 
10 /*
11  * This is a C implementation of the geodesic algorithms described in
12  *
13  * C. F. F. Karney,
14  * Algorithms for geodesics,
15  * J. Geodesy <b>87</b>, 43--55 (2013);
16  * https://dx.doi.org/10.1007/s00190-012-0578-z
17  * Addenda: http://geographiclib.sf.net/geod-addenda.html
18  *
19  * See the comments in geodesic.h for documentation.
20  *
21  * Copyright (c) Charles Karney (2012-2015) <charles@karney.com> and licensed
22  * under the MIT/X11 License. For more information, see
23  * http://geographiclib.sourceforge.net/
24  */
25 
26 #include "geodesic.h"
27 #include <math.h>
28 
29 #define GEOGRAPHICLIB_GEODESIC_ORDER 6
30 #define nA1 GEOGRAPHICLIB_GEODESIC_ORDER
31 #define nC1 GEOGRAPHICLIB_GEODESIC_ORDER
32 #define nC1p GEOGRAPHICLIB_GEODESIC_ORDER
33 #define nA2 GEOGRAPHICLIB_GEODESIC_ORDER
34 #define nC2 GEOGRAPHICLIB_GEODESIC_ORDER
35 #define nA3 GEOGRAPHICLIB_GEODESIC_ORDER
36 #define nA3x nA3
37 #define nC3 GEOGRAPHICLIB_GEODESIC_ORDER
38 #define nC3x ((nC3 * (nC3 - 1)) / 2)
39 #define nC4 GEOGRAPHICLIB_GEODESIC_ORDER
40 #define nC4x ((nC4 * (nC4 + 1)) / 2)
41 #define nC (GEOGRAPHICLIB_GEODESIC_ORDER + 1)
42 
43 typedef double real;
44 typedef int boolx;
45 
46 static unsigned init = 0;
47 static const int FALSE = 0;
48 static const int TRUE = 1;
49 static unsigned digits, maxit1, maxit2;
50 static real epsilon, realmin, pi, degree, NaN,
51  tiny, tol0, tol1, tol2, tolb, xthresh;
52 
53 static void Init() {
54  if (!init) {
55 #if defined(__DBL_MANT_DIG__)
56  digits = __DBL_MANT_DIG__;
57 #else
58  digits = 53;
59 #endif
60 #if defined(__DBL_EPSILON__)
61  epsilon = __DBL_EPSILON__;
62 #else
63  epsilon = pow(0.5, digits - 1);
64 #endif
65 #if defined(__DBL_MIN__)
66  realmin = __DBL_MIN__;
67 #else
68  realmin = pow(0.5, 1022);
69 #endif
70 #if defined(M_PI)
71  pi = M_PI;
72 #else
73  pi = atan2(0.0, -1.0);
74 #endif
75  maxit1 = 20;
76  maxit2 = maxit1 + digits + 10;
77  tiny = sqrt(realmin);
78  tol0 = epsilon;
79  /* Increase multiplier in defn of tol1 from 100 to 200 to fix inverse case
80  * 52.784459512564 0 -52.784459512563990912 179.634407464943777557
81  * which otherwise failed for Visual Studio 10 (Release and Debug) */
82  tol1 = 200 * tol0;
83  tol2 = sqrt(tol0);
84  /* Check on bisection interval */
85  tolb = tol0 * tol2;
86  xthresh = 1000 * tol2;
87  degree = pi/180;
88  NaN = sqrt(-1.0);
89  init = 1;
90  }
91 }
92 
93 enum captype {
94  CAP_NONE = 0U,
95  CAP_C1 = 1U<<0,
96  CAP_C1p = 1U<<1,
97  CAP_C2 = 1U<<2,
98  CAP_C3 = 1U<<3,
99  CAP_C4 = 1U<<4,
100  CAP_ALL = 0x1FU,
101  OUT_ALL = 0x7F80U
102 };
103 
104 static real sq(real x) { return x * x; }
105 static real log1px(real x) {
106  volatile real
107  y = 1 + x,
108  z = y - 1;
109  /* Here's the explanation for this magic: y = 1 + z, exactly, and z
110  * approx x, thus log(y)/z (which is nearly constant near z = 0) returns
111  * a good approximation to the true log(1 + x)/x. The multiplication x *
112  * (log(y)/z) introduces little additional error. */
113  return z == 0 ? x : x * log(y) / z;
114 }
115 
116 static real atanhx(real x) {
117  real y = fabs(x); /* Enforce odd parity */
118  y = log1px(2 * y/(1 - y))/2;
119  return x < 0 ? -y : y;
120 }
121 
122 static real hypotx(real x, real y)
123 { return sqrt(x * x + y * y); }
124 
125 static real cbrtx(real x) {
126  real y = pow(fabs(x), 1/(real)(3)); /* Return the real cube root */
127  return x < 0 ? -y : y;
128 }
129 
130 static real sumx(real u, real v, real* t) {
131  volatile real s = u + v;
132  volatile real up = s - v;
133  volatile real vpp = s - up;
134  up -= u;
135  vpp -= v;
136  *t = -(up + vpp);
137  /* error-free sum:
138  * u + v = s + t
139  * = round(u + v) + t */
140  return s;
141 }
142 
143 static real polyval(int N, const real p[], real x) {
144  real y = N < 0 ? 0 : *p++;
145  while (--N >= 0) y = y * x + *p++;
146  return y;
147 }
148 
149 static real minx(real x, real y)
150 { return x < y ? x : y; }
151 
152 static real maxx(real x, real y)
153 { return x > y ? x : y; }
154 
155 static void swapx(real* x, real* y)
156 { real t = *x; *x = *y; *y = t; }
157 
158 static void norm2(real* sinx, real* cosx) {
159  real r = hypotx(*sinx, *cosx);
160  *sinx /= r;
161  *cosx /= r;
162 }
163 
164 static real AngNormalize(real x) {
165  x = fmod(x, (real)(360));
166  return x < -180 ? x + 360 : (x < 180 ? x : x - 360);
167 }
168 
169 static real LatFix(real x)
170 { return fabs(x) > 90 ? NaN : x; }
171 
172 static real AngDiff(real x, real y) {
173  real t, d = - AngNormalize(sumx(AngNormalize(x), AngNormalize(-y), &t));
174  /* Here y - x = d - t (mod 360), exactly, where d is in (-180,180] and
175  * abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
176  * addition of t takes the result outside the range (-180,180] is d = 180
177  * and t < 0. The case, d = -180 + eps, t = eps, can't happen, since
178  * sum would have returned the exact result in such a case (i.e., given t
179  * = 0). */
180  return (d == 180 && t < 0 ? -180 : d) - t;
181 }
182 
183 static real AngRound(real x) {
184  const real z = 1/(real)(16);
185  volatile real y = fabs(x);
186  /* The compiler mustn't "simplify" z - (z - y) to y */
187  y = y < z ? z - (z - y) : y;
188  return x < 0 ? 0 - y : y;
189 }
190 
191 static void sincosdx(real x, real* sinx, real* cosx) {
192  /* In order to minimize round-off errors, this function exactly reduces
193  * the argument to the range [-45, 45] before converting it to radians. */
194  real r, s, c; int q;
195  r = fmod(x, (real)(360));
196  q = (int)(floor(r / 90 + (real)(0.5)));
197  r -= 90 * q;
198  /* now abs(r) <= 45 */
199  r *= degree;
200  /* Possibly could call the gnu extension sincos */
201  s = sin(r); c = cos(r);
202  switch ((unsigned)q & 3U) {
203  case 0U: *sinx = s; *cosx = c; break;
204  case 1U: *sinx = c; *cosx = 0 - s; break;
205  case 2U: *sinx = 0 - s; *cosx = 0 - c; break;
206  default: *sinx = 0 - c; *cosx = s; break; /* case 3U */
207  }
208 }
209 
210 static real atan2dx(real y, real x) {
211  /* In order to minimize round-off errors, this function rearranges the
212  * arguments so that result of atan2 is in the range [-pi/4, pi/4] before
213  * converting it to degrees and mapping the result to the correct
214  * quadrant. */
215  int q = 0; real ang;
216  if (fabs(y) > fabs(x)) { swapx(&x, &y); q = 2; }
217  if (x < 0) { x = -x; ++q; }
218  /* here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4] */
219  ang = atan2(y, x) / degree;
220  switch (q) {
221  /* Note that atan2d(-0.0, 1.0) will return -0. However, we expect that
222  * atan2d will not be called with y = -0. If need be, include
223  *
224  * case 0: ang = 0 + ang; break;
225  */
226  case 1: ang = (y > 0 ? 180 : -180) - ang; break;
227  case 2: ang = 90 - ang; break;
228  case 3: ang = -90 + ang; break;
229  }
230  return ang;
231 }
232 
233 static void A3coeff(struct geod_geodesic* g);
234 static void C3coeff(struct geod_geodesic* g);
235 static void C4coeff(struct geod_geodesic* g);
236 static real SinCosSeries(boolx sinp,
237  real sinx, real cosx,
238  const real c[], int n);
239 static void Lengths(const struct geod_geodesic* g,
240  real eps, real sig12,
241  real ssig1, real csig1, real dn1,
242  real ssig2, real csig2, real dn2,
243  real cbet1, real cbet2,
244  real* ps12b, real* pm12b, real* pm0,
245  real* pM12, real* pM21,
246  /* Scratch area of the right size */
247  real Ca[]);
248 static real Astroid(real x, real y);
249 static real InverseStart(const struct geod_geodesic* g,
250  real sbet1, real cbet1, real dn1,
251  real sbet2, real cbet2, real dn2,
252  real lam12,
253  real* psalp1, real* pcalp1,
254  /* Only updated if return val >= 0 */
255  real* psalp2, real* pcalp2,
256  /* Only updated for short lines */
257  real* pdnm,
258  /* Scratch area of the right size */
259  real Ca[]);
260 static real Lambda12(const struct geod_geodesic* g,
261  real sbet1, real cbet1, real dn1,
262  real sbet2, real cbet2, real dn2,
263  real salp1, real calp1,
264  real* psalp2, real* pcalp2,
265  real* psig12,
266  real* pssig1, real* pcsig1,
267  real* pssig2, real* pcsig2,
268  real* peps, real* pdomg12,
269  boolx diffp, real* pdlam12,
270  /* Scratch area of the right size */
271  real Ca[]);
272 static real A3f(const struct geod_geodesic* g, real eps);
273 static void C3f(const struct geod_geodesic* g, real eps, real c[]);
274 static void C4f(const struct geod_geodesic* g, real eps, real c[]);
275 static real A1m1f(real eps);
276 static void C1f(real eps, real c[]);
277 static void C1pf(real eps, real c[]);
278 static real A2m1f(real eps);
279 static void C2f(real eps, real c[]);
280 static int transit(real lon1, real lon2);
281 static int transitdirect(real lon1, real lon2);
282 static void accini(real s[]);
283 static void acccopy(const real s[], real t[]);
284 static void accadd(real s[], real y);
285 static real accsum(const real s[], real y);
286 static void accneg(real s[]);
287 
288 void geod_init(struct geod_geodesic* g, real a, real f) {
289  if (!init) Init();
290  g->a = a;
291  g->f = f <= 1 ? f : 1/f;
292  g->f1 = 1 - g->f;
293  g->e2 = g->f * (2 - g->f);
294  g->ep2 = g->e2 / sq(g->f1); /* e2 / (1 - e2) */
295  g->n = g->f / ( 2 - g->f);
296  g->b = g->a * g->f1;
297  g->c2 = (sq(g->a) + sq(g->b) *
298  (g->e2 == 0 ? 1 :
299  (g->e2 > 0 ? atanhx(sqrt(g->e2)) : atan(sqrt(-g->e2))) /
300  sqrt(fabs(g->e2))))/2; /* authalic radius squared */
301  /* The sig12 threshold for "really short". Using the auxiliary sphere
302  * solution with dnm computed at (bet1 + bet2) / 2, the relative error in the
303  * azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2. (Error
304  * measured for 1/100 < b/a < 100 and abs(f) >= 1/1000. For a given f and
305  * sig12, the max error occurs for lines near the pole. If the old rule for
306  * computing dnm = (dn1 + dn2)/2 is used, then the error increases by a
307  * factor of 2.) Setting this equal to epsilon gives sig12 = etol2. Here
308  * 0.1 is a safety factor (error decreased by 100) and max(0.001, abs(f))
309  * stops etol2 getting too large in the nearly spherical case. */
310  g->etol2 = 0.1 * tol2 /
311  sqrt( maxx((real)(0.001), fabs(g->f)) * minx((real)(1), 1 - g->f/2) / 2 );
312 
313  A3coeff(g);
314  C3coeff(g);
315  C4coeff(g);
316 }
317 
318 void geod_lineinit(struct geod_geodesicline* l,
319  const struct geod_geodesic* g,
320  real lat1, real lon1, real azi1, unsigned caps) {
321  real cbet1, sbet1, eps;
322  l->a = g->a;
323  l->f = g->f;
324  l->b = g->b;
325  l->c2 = g->c2;
326  l->f1 = g->f1;
327  /* If caps is 0 assume the standard direct calculation */
328  l->caps = (caps ? caps : GEOD_DISTANCE_IN | GEOD_LONGITUDE) |
329  /* always allow latitude and azimuth and unrolling of longitude */
331 
332  l->lat1 = LatFix(lat1);
333  l->lon1 = lon1;
334  l->azi1 = AngNormalize(azi1);
335  /* Guard against underflow in salp0 */
336  sincosdx(AngRound(l->azi1), &l->salp1, &l->calp1);
337 
338  sincosdx(AngRound(l->lat1), &sbet1, &cbet1); sbet1 *= l->f1;
339  /* Ensure cbet1 = +epsilon at poles */
340  norm2(&sbet1, &cbet1); cbet1 = maxx(tiny, cbet1);
341  l->dn1 = sqrt(1 + g->ep2 * sq(sbet1));
342 
343  /* Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0), */
344  l->salp0 = l->salp1 * cbet1; /* alp0 in [0, pi/2 - |bet1|] */
345  /* Alt: calp0 = hypot(sbet1, calp1 * cbet1). The following
346  * is slightly better (consider the case salp1 = 0). */
347  l->calp0 = hypotx(l->calp1, l->salp1 * sbet1);
348  /* Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
349  * sig = 0 is nearest northward crossing of equator.
350  * With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
351  * With bet1 = pi/2, alp1 = -pi, sig1 = pi/2
352  * With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2
353  * Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
354  * With alp0 in (0, pi/2], quadrants for sig and omg coincide.
355  * No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
356  * With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi. */
357  l->ssig1 = sbet1; l->somg1 = l->salp0 * sbet1;
358  l->csig1 = l->comg1 = sbet1 != 0 || l->calp1 != 0 ? cbet1 * l->calp1 : 1;
359  norm2(&l->ssig1, &l->csig1); /* sig1 in (-pi, pi] */
360  /* norm2(somg1, comg1); -- don't need to normalize! */
361 
362  l->k2 = sq(l->calp0) * g->ep2;
363  eps = l->k2 / (2 * (1 + sqrt(1 + l->k2)) + l->k2);
364 
365  if (l->caps & CAP_C1) {
366  real s, c;
367  l->A1m1 = A1m1f(eps);
368  C1f(eps, l->C1a);
369  l->B11 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C1a, nC1);
370  s = sin(l->B11); c = cos(l->B11);
371  /* tau1 = sig1 + B11 */
372  l->stau1 = l->ssig1 * c + l->csig1 * s;
373  l->ctau1 = l->csig1 * c - l->ssig1 * s;
374  /* Not necessary because C1pa reverts C1a
375  * B11 = -SinCosSeries(TRUE, stau1, ctau1, C1pa, nC1p); */
376  }
377 
378  if (l->caps & CAP_C1p)
379  C1pf(eps, l->C1pa);
380 
381  if (l->caps & CAP_C2) {
382  l->A2m1 = A2m1f(eps);
383  C2f(eps, l->C2a);
384  l->B21 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C2a, nC2);
385  }
386 
387  if (l->caps & CAP_C3) {
388  C3f(g, eps, l->C3a);
389  l->A3c = -l->f * l->salp0 * A3f(g, eps);
390  l->B31 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C3a, nC3-1);
391  }
392 
393  if (l->caps & CAP_C4) {
394  C4f(g, eps, l->C4a);
395  /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0) */
396  l->A4 = sq(l->a) * l->calp0 * l->salp0 * g->e2;
397  l->B41 = SinCosSeries(FALSE, l->ssig1, l->csig1, l->C4a, nC4);
398  }
399 }
400 
401 real geod_genposition(const struct geod_geodesicline* l,
402  unsigned flags, real s12_a12,
403  real* plat2, real* plon2, real* pazi2,
404  real* ps12, real* pm12,
405  real* pM12, real* pM21,
406  real* pS12) {
407  real lat2 = 0, lon2 = 0, azi2 = 0, s12 = 0,
408  m12 = 0, M12 = 0, M21 = 0, S12 = 0;
409  /* Avoid warning about uninitialized B12. */
410  real sig12, ssig12, csig12, B12 = 0, AB1 = 0;
411  real omg12, lam12, lon12;
412  real ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2;
413  unsigned outmask =
414  (plat2 ? GEOD_LATITUDE : 0U) |
415  (plon2 ? GEOD_LONGITUDE : 0U) |
416  (pazi2 ? GEOD_AZIMUTH : 0U) |
417  (ps12 ? GEOD_DISTANCE : 0U) |
418  (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
419  (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
420  (pS12 ? GEOD_AREA : 0U);
421 
422  outmask &= l->caps & OUT_ALL;
423  if (!( TRUE /*Init()*/ &&
424  (flags & GEOD_ARCMODE || (l->caps & GEOD_DISTANCE_IN & OUT_ALL)) ))
425  /* Uninitialized or impossible distance calculation requested */
426  return NaN;
427 
428  if (flags & GEOD_ARCMODE) {
429  /* Interpret s12_a12 as spherical arc length */
430  sig12 = s12_a12 * degree;
431  sincosdx(s12_a12, &ssig12, &csig12);
432  } else {
433  /* Interpret s12_a12 as distance */
434  real
435  tau12 = s12_a12 / (l->b * (1 + l->A1m1)),
436  s = sin(tau12),
437  c = cos(tau12);
438  /* tau2 = tau1 + tau12 */
439  B12 = - SinCosSeries(TRUE,
440  l->stau1 * c + l->ctau1 * s,
441  l->ctau1 * c - l->stau1 * s,
442  l->C1pa, nC1p);
443  sig12 = tau12 - (B12 - l->B11);
444  ssig12 = sin(sig12); csig12 = cos(sig12);
445  if (fabs(l->f) > 0.01) {
446  /* Reverted distance series is inaccurate for |f| > 1/100, so correct
447  * sig12 with 1 Newton iteration. The following table shows the
448  * approximate maximum error for a = WGS_a() and various f relative to
449  * GeodesicExact.
450  * erri = the error in the inverse solution (nm)
451  * errd = the error in the direct solution (series only) (nm)
452  * errda = the error in the direct solution (series + 1 Newton) (nm)
453  *
454  * f erri errd errda
455  * -1/5 12e6 1.2e9 69e6
456  * -1/10 123e3 12e6 765e3
457  * -1/20 1110 108e3 7155
458  * -1/50 18.63 200.9 27.12
459  * -1/100 18.63 23.78 23.37
460  * -1/150 18.63 21.05 20.26
461  * 1/150 22.35 24.73 25.83
462  * 1/100 22.35 25.03 25.31
463  * 1/50 29.80 231.9 30.44
464  * 1/20 5376 146e3 10e3
465  * 1/10 829e3 22e6 1.5e6
466  * 1/5 157e6 3.8e9 280e6 */
467  real
468  ssig2 = l->ssig1 * csig12 + l->csig1 * ssig12,
469  csig2 = l->csig1 * csig12 - l->ssig1 * ssig12,
470  serr;
471  B12 = SinCosSeries(TRUE, ssig2, csig2, l->C1a, nC1);
472  serr = (1 + l->A1m1) * (sig12 + (B12 - l->B11)) - s12_a12 / l->b;
473  sig12 = sig12 - serr / sqrt(1 + l->k2 * sq(ssig2));
474  ssig12 = sin(sig12); csig12 = cos(sig12);
475  /* Update B12 below */
476  }
477  }
478 
479  /* sig2 = sig1 + sig12 */
480  ssig2 = l->ssig1 * csig12 + l->csig1 * ssig12;
481  csig2 = l->csig1 * csig12 - l->ssig1 * ssig12;
482  dn2 = sqrt(1 + l->k2 * sq(ssig2));
484  if (flags & GEOD_ARCMODE || fabs(l->f) > 0.01)
485  B12 = SinCosSeries(TRUE, ssig2, csig2, l->C1a, nC1);
486  AB1 = (1 + l->A1m1) * (B12 - l->B11);
487  }
488  /* sin(bet2) = cos(alp0) * sin(sig2) */
489  sbet2 = l->calp0 * ssig2;
490  /* Alt: cbet2 = hypot(csig2, salp0 * ssig2); */
491  cbet2 = hypotx(l->salp0, l->calp0 * csig2);
492  if (cbet2 == 0)
493  /* I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case */
494  cbet2 = csig2 = tiny;
495  /* tan(alp0) = cos(sig2)*tan(alp2) */
496  salp2 = l->salp0; calp2 = l->calp0 * csig2; /* No need to normalize */
497 
498  if (outmask & GEOD_DISTANCE)
499  s12 = flags & GEOD_ARCMODE ? l->b * ((1 + l->A1m1) * sig12 + AB1) : s12_a12;
500 
501  if (outmask & GEOD_LONGITUDE) {
502  int E = l->salp0 < 0 ? -1 : 1; /* east or west going? */
503  /* tan(omg2) = sin(alp0) * tan(sig2) */
504  somg2 = l->salp0 * ssig2; comg2 = csig2; /* No need to normalize */
505  /* omg12 = omg2 - omg1 */
506  omg12 = flags & GEOD_LONG_UNROLL
507  ? E * (sig12
508  - (atan2( ssig2, csig2) - atan2( l->ssig1, l->csig1))
509  + (atan2(E * somg2, comg2) - atan2(E * l->somg1, l->comg1)))
510  : atan2(somg2 * l->comg1 - comg2 * l->somg1,
511  comg2 * l->comg1 + somg2 * l->somg1);
512  lam12 = omg12 + l->A3c *
513  ( sig12 + (SinCosSeries(TRUE, ssig2, csig2, l->C3a, nC3-1)
514  - l->B31));
515  lon12 = lam12 / degree;
516  lon2 = flags & GEOD_LONG_UNROLL ? l->lon1 + lon12 :
517  AngNormalize(AngNormalize(l->lon1) + AngNormalize(lon12));
518  }
519 
520  if (outmask & GEOD_LATITUDE)
521  lat2 = atan2dx(sbet2, l->f1 * cbet2);
522 
523  if (outmask & GEOD_AZIMUTH)
524  azi2 = atan2dx(salp2, calp2);
525 
526  if (outmask & (GEOD_REDUCEDLENGTH | GEOD_GEODESICSCALE)) {
527  real
528  B22 = SinCosSeries(TRUE, ssig2, csig2, l->C2a, nC2),
529  AB2 = (1 + l->A2m1) * (B22 - l->B21),
530  J12 = (l->A1m1 - l->A2m1) * sig12 + (AB1 - AB2);
531  if (outmask & GEOD_REDUCEDLENGTH)
532  /* Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
533  * accurate cancellation in the case of coincident points. */
534  m12 = l->b * ((dn2 * (l->csig1 * ssig2) - l->dn1 * (l->ssig1 * csig2))
535  - l->csig1 * csig2 * J12);
536  if (outmask & GEOD_GEODESICSCALE) {
537  real t = l->k2 * (ssig2 - l->ssig1) * (ssig2 + l->ssig1) / (l->dn1 + dn2);
538  M12 = csig12 + (t * ssig2 - csig2 * J12) * l->ssig1 / l->dn1;
539  M21 = csig12 - (t * l->ssig1 - l->csig1 * J12) * ssig2 / dn2;
540  }
541  }
542 
543  if (outmask & GEOD_AREA) {
544  real
545  B42 = SinCosSeries(FALSE, ssig2, csig2, l->C4a, nC4);
546  real salp12, calp12;
547  if (l->calp0 == 0 || l->salp0 == 0) {
548  /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
549  salp12 = salp2 * l->calp1 - calp2 * l->salp1;
550  calp12 = calp2 * l->calp1 + salp2 * l->salp1;
551  /* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
552  * salp12 = -0 and alp12 = -180. However this depends on the sign being
553  * attached to 0 correctly. The following ensures the correct
554  * behavior. */
555  if (salp12 == 0 && calp12 < 0) {
556  salp12 = tiny * l->calp1;
557  calp12 = -1;
558  }
559  } else {
560  /* tan(alp) = tan(alp0) * sec(sig)
561  * tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
562  * = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
563  * If csig12 > 0, write
564  * csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
565  * else
566  * csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
567  * No need to normalize */
568  salp12 = l->calp0 * l->salp0 *
569  (csig12 <= 0 ? l->csig1 * (1 - csig12) + ssig12 * l->ssig1 :
570  ssig12 * (l->csig1 * ssig12 / (1 + csig12) + l->ssig1));
571  calp12 = sq(l->salp0) + sq(l->calp0) * l->csig1 * csig2;
572  }
573  S12 = l->c2 * atan2(salp12, calp12) + l->A4 * (B42 - l->B41);
574  }
575 
576  if (outmask & GEOD_LATITUDE)
577  *plat2 = lat2;
578  if (outmask & GEOD_LONGITUDE)
579  *plon2 = lon2;
580  if (outmask & GEOD_AZIMUTH)
581  *pazi2 = azi2;
582  if (outmask & GEOD_DISTANCE)
583  *ps12 = s12;
584  if (outmask & GEOD_REDUCEDLENGTH)
585  *pm12 = m12;
586  if (outmask & GEOD_GEODESICSCALE) {
587  if (pM12) *pM12 = M12;
588  if (pM21) *pM21 = M21;
589  }
590  if (outmask & GEOD_AREA)
591  *pS12 = S12;
592 
593  return flags & GEOD_ARCMODE ? s12_a12 : sig12 / degree;
594 }
595 
596 void geod_position(const struct geod_geodesicline* l, real s12,
597  real* plat2, real* plon2, real* pazi2) {
598  geod_genposition(l, FALSE, s12, plat2, plon2, pazi2, 0, 0, 0, 0, 0);
599 }
600 
601 real geod_gendirect(const struct geod_geodesic* g,
602  real lat1, real lon1, real azi1,
603  unsigned flags, real s12_a12,
604  real* plat2, real* plon2, real* pazi2,
605  real* ps12, real* pm12, real* pM12, real* pM21,
606  real* pS12) {
607  struct geod_geodesicline l;
608  unsigned outmask =
609  (plat2 ? GEOD_LATITUDE : 0U) |
610  (plon2 ? GEOD_LONGITUDE : 0U) |
611  (pazi2 ? GEOD_AZIMUTH : 0U) |
612  (ps12 ? GEOD_DISTANCE : 0U) |
613  (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
614  (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
615  (pS12 ? GEOD_AREA : 0U);
616 
617  geod_lineinit(&l, g, lat1, lon1, azi1,
618  /* Automatically supply GEOD_DISTANCE_IN if necessary */
619  outmask |
620  (flags & GEOD_ARCMODE ? GEOD_NONE : GEOD_DISTANCE_IN));
621  return geod_genposition(&l, flags, s12_a12,
622  plat2, plon2, pazi2, ps12, pm12, pM12, pM21, pS12);
623 }
624 
625 void geod_direct(const struct geod_geodesic* g,
626  real lat1, real lon1, real azi1,
627  real s12,
628  real* plat2, real* plon2, real* pazi2) {
629  geod_gendirect(g, lat1, lon1, azi1, GEOD_NOFLAGS, s12, plat2, plon2, pazi2,
630  0, 0, 0, 0, 0);
631 }
632 
633 real geod_geninverse(const struct geod_geodesic* g,
634  real lat1, real lon1, real lat2, real lon2,
635  real* ps12, real* pazi1, real* pazi2,
636  real* pm12, real* pM12, real* pM21, real* pS12) {
637  real s12 = 0, azi1 = 0, azi2 = 0, m12 = 0, M12 = 0, M21 = 0, S12 = 0;
638  real lon12;
639  int latsign, lonsign, swapp;
640  real sbet1, cbet1, sbet2, cbet2, s12x = 0, m12x = 0;
641  real dn1, dn2, lam12, slam12, clam12;
642  real a12 = 0, sig12, calp1 = 0, salp1 = 0, calp2 = 0, salp2 = 0;
643  real Ca[nC];
644  boolx meridian;
645  real omg12 = 0;
646 
647  unsigned outmask =
648  (ps12 ? GEOD_DISTANCE : 0U) |
649  (pazi1 || pazi2 ? GEOD_AZIMUTH : 0U) |
650  (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
651  (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
652  (pS12 ? GEOD_AREA : 0U);
653 
654  outmask &= OUT_ALL;
655  /* Compute longitude difference (AngDiff does this carefully). Result is
656  * in [-180, 180] but -180 is only for west-going geodesics. 180 is for
657  * east-going and meridional geodesics. */
658  /* If very close to being on the same half-meridian, then make it so. */
659  lon12 = AngRound(AngDiff(lon1, lon2));
660  /* Make longitude difference positive. */
661  lonsign = lon12 >= 0 ? 1 : -1;
662  lon12 *= lonsign;
663  /* If really close to the equator, treat as on equator. */
664  lat1 = AngRound(LatFix(lat1));
665  lat2 = AngRound(LatFix(lat2));
666  /* Swap points so that point with higher (abs) latitude is point 1 */
667  swapp = fabs(lat1) >= fabs(lat2) ? 1 : -1;
668  if (swapp < 0) {
669  lonsign *= -1;
670  swapx(&lat1, &lat2);
671  }
672  /* Make lat1 <= 0 */
673  latsign = lat1 < 0 ? 1 : -1;
674  lat1 *= latsign;
675  lat2 *= latsign;
676  /* Now we have
677  *
678  * 0 <= lon12 <= 180
679  * -90 <= lat1 <= 0
680  * lat1 <= lat2 <= -lat1
681  *
682  * longsign, swapp, latsign register the transformation to bring the
683  * coordinates to this canonical form. In all cases, 1 means no change was
684  * made. We make these transformations so that there are few cases to
685  * check, e.g., on verifying quadrants in atan2. In addition, this
686  * enforces some symmetries in the results returned. */
687 
688  sincosdx(lat1, &sbet1, &cbet1); sbet1 *= g->f1;
689  /* Ensure cbet1 = +epsilon at poles */
690  norm2(&sbet1, &cbet1); cbet1 = maxx(tiny, cbet1);
691 
692  sincosdx(lat2, &sbet2, &cbet2); sbet2 *= g->f1;
693  /* Ensure cbet2 = +epsilon at poles */
694  norm2(&sbet2, &cbet2); cbet2 = maxx(tiny, cbet2);
695 
696  /* If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the
697  * |bet1| - |bet2|. Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is
698  * a better measure. This logic is used in assigning calp2 in Lambda12.
699  * Sometimes these quantities vanish and in that case we force bet2 = +/-
700  * bet1 exactly. An example where is is necessary is the inverse problem
701  * 48.522876735459 0 -48.52287673545898293 179.599720456223079643
702  * which failed with Visual Studio 10 (Release and Debug) */
703 
704  if (cbet1 < -sbet1) {
705  if (cbet2 == cbet1)
706  sbet2 = sbet2 < 0 ? sbet1 : -sbet1;
707  } else {
708  if (fabs(sbet2) == -sbet1)
709  cbet2 = cbet1;
710  }
711 
712  dn1 = sqrt(1 + g->ep2 * sq(sbet1));
713  dn2 = sqrt(1 + g->ep2 * sq(sbet2));
714 
715  lam12 = lon12 * degree;
716  sincosdx(lon12, &slam12, &clam12);
717 
718  meridian = lat1 == -90 || slam12 == 0;
719 
720  if (meridian) {
721 
722  /* Endpoints are on a single full meridian, so the geodesic might lie on
723  * a meridian. */
724 
725  real ssig1, csig1, ssig2, csig2;
726  calp1 = clam12; salp1 = slam12; /* Head to the target longitude */
727  calp2 = 1; salp2 = 0; /* At the target we're heading north */
728 
729  /* tan(bet) = tan(sig) * cos(alp) */
730  ssig1 = sbet1; csig1 = calp1 * cbet1;
731  ssig2 = sbet2; csig2 = calp2 * cbet2;
732 
733  /* sig12 = sig2 - sig1 */
734  sig12 = atan2(maxx(csig1 * ssig2 - ssig1 * csig2, (real)(0)),
735  csig1 * csig2 + ssig1 * ssig2);
736  Lengths(g, g->n, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
737  cbet1, cbet2, &s12x, &m12x, 0,
738  outmask & GEOD_GEODESICSCALE ? &M12 : 0,
739  outmask & GEOD_GEODESICSCALE ? &M21 : 0,
740  Ca);
741  /* Add the check for sig12 since zero length geodesics might yield m12 <
742  * 0. Test case was
743  *
744  * echo 20.001 0 20.001 0 | GeodSolve -i
745  *
746  * In fact, we will have sig12 > pi/2 for meridional geodesic which is
747  * not a shortest path. */
748  if (sig12 < 1 || m12x >= 0) {
749  /* Need at least 2, to handle 90 0 90 180 */
750  if (sig12 < 3 * tiny)
751  sig12 = m12x = s12x = 0;
752  m12x *= g->b;
753  s12x *= g->b;
754  a12 = sig12 / degree;
755  } else
756  /* m12 < 0, i.e., prolate and too close to anti-podal */
757  meridian = FALSE;
758  }
759 
760  if (!meridian &&
761  sbet1 == 0 && /* and sbet2 == 0 */
762  /* Mimic the way Lambda12 works with calp1 = 0 */
763  (g->f <= 0 || lam12 <= pi - g->f * pi)) {
764 
765  /* Geodesic runs along equator */
766  calp1 = calp2 = 0; salp1 = salp2 = 1;
767  s12x = g->a * lam12;
768  sig12 = omg12 = lam12 / g->f1;
769  m12x = g->b * sin(sig12);
770  if (outmask & GEOD_GEODESICSCALE)
771  M12 = M21 = cos(sig12);
772  a12 = lon12 / g->f1;
773 
774  } else if (!meridian) {
775 
776  /* Now point1 and point2 belong within a hemisphere bounded by a
777  * meridian and geodesic is neither meridional or equatorial. */
778 
779  /* Figure a starting point for Newton's method */
780  real dnm = 0;
781  sig12 = InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
782  lam12,
783  &salp1, &calp1, &salp2, &calp2, &dnm,
784  Ca);
785 
786  if (sig12 >= 0) {
787  /* Short lines (InverseStart sets salp2, calp2, dnm) */
788  s12x = sig12 * g->b * dnm;
789  m12x = sq(dnm) * g->b * sin(sig12 / dnm);
790  if (outmask & GEOD_GEODESICSCALE)
791  M12 = M21 = cos(sig12 / dnm);
792  a12 = sig12 / degree;
793  omg12 = lam12 / (g->f1 * dnm);
794  } else {
795 
796  /* Newton's method. This is a straightforward solution of f(alp1) =
797  * lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one
798  * root in the interval (0, pi) and its derivative is positive at the
799  * root. Thus f(alp) is positive for alp > alp1 and negative for alp <
800  * alp1. During the course of the iteration, a range (alp1a, alp1b) is
801  * maintained which brackets the root and with each evaluation of
802  * f(alp) the range is shrunk, if possible. Newton's method is
803  * restarted whenever the derivative of f is negative (because the new
804  * value of alp1 is then further from the solution) or if the new
805  * estimate of alp1 lies outside (0,pi); in this case, the new starting
806  * guess is taken to be (alp1a + alp1b) / 2. */
807  real ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, eps = 0;
808  unsigned numit = 0;
809  /* Bracketing range */
810  real salp1a = tiny, calp1a = 1, salp1b = tiny, calp1b = -1;
811  boolx tripn, tripb;
812  for (tripn = FALSE, tripb = FALSE; numit < maxit2; ++numit) {
813  /* the WGS84 test set: mean = 1.47, sd = 1.25, max = 16
814  * WGS84 and random input: mean = 2.85, sd = 0.60 */
815  real dv = 0,
816  v = (Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
817  &salp2, &calp2, &sig12, &ssig1, &csig1, &ssig2, &csig2,
818  &eps, &omg12, numit < maxit1, &dv, Ca)
819  - lam12);
820  /* 2 * tol0 is approximately 1 ulp for a number in [0, pi]. */
821  /* Reversed test to allow escape with NaNs */
822  if (tripb || !(fabs(v) >= (tripn ? 8 : 2) * tol0)) break;
823  /* Update bracketing values */
824  if (v > 0 && (numit > maxit1 || calp1/salp1 > calp1b/salp1b))
825  { salp1b = salp1; calp1b = calp1; }
826  else if (v < 0 && (numit > maxit1 || calp1/salp1 < calp1a/salp1a))
827  { salp1a = salp1; calp1a = calp1; }
828  if (numit < maxit1 && dv > 0) {
829  real
830  dalp1 = -v/dv;
831  real
832  sdalp1 = sin(dalp1), cdalp1 = cos(dalp1),
833  nsalp1 = salp1 * cdalp1 + calp1 * sdalp1;
834  if (nsalp1 > 0 && fabs(dalp1) < pi) {
835  calp1 = calp1 * cdalp1 - salp1 * sdalp1;
836  salp1 = nsalp1;
837  norm2(&salp1, &calp1);
838  /* In some regimes we don't get quadratic convergence because
839  * slope -> 0. So use convergence conditions based on epsilon
840  * instead of sqrt(epsilon). */
841  tripn = fabs(v) <= 16 * tol0;
842  continue;
843  }
844  }
845  /* Either dv was not postive or updated value was outside legal
846  * range. Use the midpoint of the bracket as the next estimate.
847  * This mechanism is not needed for the WGS84 ellipsoid, but it does
848  * catch problems with more eccentric ellipsoids. Its efficacy is
849  * such for the WGS84 test set with the starting guess set to alp1 =
850  * 90deg:
851  * the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
852  * WGS84 and random input: mean = 4.74, sd = 0.99 */
853  salp1 = (salp1a + salp1b)/2;
854  calp1 = (calp1a + calp1b)/2;
855  norm2(&salp1, &calp1);
856  tripn = FALSE;
857  tripb = (fabs(salp1a - salp1) + (calp1a - calp1) < tolb ||
858  fabs(salp1 - salp1b) + (calp1 - calp1b) < tolb);
859  }
860  Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
861  cbet1, cbet2, &s12x, &m12x, 0,
862  outmask & GEOD_GEODESICSCALE ? &M12 : 0,
863  outmask & GEOD_GEODESICSCALE ? &M21 : 0, Ca);
864  m12x *= g->b;
865  s12x *= g->b;
866  a12 = sig12 / degree;
867  omg12 = lam12 - omg12;
868  }
869  }
870 
871  if (outmask & GEOD_DISTANCE)
872  s12 = 0 + s12x; /* Convert -0 to 0 */
873 
874  if (outmask & GEOD_REDUCEDLENGTH)
875  m12 = 0 + m12x; /* Convert -0 to 0 */
876 
877  if (outmask & GEOD_AREA) {
878  real
879  /* From Lambda12: sin(alp1) * cos(bet1) = sin(alp0) */
880  salp0 = salp1 * cbet1,
881  calp0 = hypotx(calp1, salp1 * sbet1); /* calp0 > 0 */
882  real alp12;
883  if (calp0 != 0 && salp0 != 0) {
884  real
885  /* From Lambda12: tan(bet) = tan(sig) * cos(alp) */
886  ssig1 = sbet1, csig1 = calp1 * cbet1,
887  ssig2 = sbet2, csig2 = calp2 * cbet2,
888  k2 = sq(calp0) * g->ep2,
889  eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2),
890  /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0). */
891  A4 = sq(g->a) * calp0 * salp0 * g->e2;
892  real B41, B42;
893  norm2(&ssig1, &csig1);
894  norm2(&ssig2, &csig2);
895  C4f(g, eps, Ca);
896  B41 = SinCosSeries(FALSE, ssig1, csig1, Ca, nC4);
897  B42 = SinCosSeries(FALSE, ssig2, csig2, Ca, nC4);
898  S12 = A4 * (B42 - B41);
899  } else
900  /* Avoid problems with indeterminate sig1, sig2 on equator */
901  S12 = 0;
902 
903  if (!meridian &&
904  omg12 < (real)(0.75) * pi && /* Long difference too big */
905  sbet2 - sbet1 < (real)(1.75)) { /* Lat difference too big */
906  /* Use tan(Gamma/2) = tan(omg12/2)
907  * * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
908  * with tan(x/2) = sin(x)/(1+cos(x)) */
909  real
910  somg12 = sin(omg12), domg12 = 1 + cos(omg12),
911  dbet1 = 1 + cbet1, dbet2 = 1 + cbet2;
912  alp12 = 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
913  domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) );
914  } else {
915  /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
916  real
917  salp12 = salp2 * calp1 - calp2 * salp1,
918  calp12 = calp2 * calp1 + salp2 * salp1;
919  /* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
920  * salp12 = -0 and alp12 = -180. However this depends on the sign
921  * being attached to 0 correctly. The following ensures the correct
922  * behavior. */
923  if (salp12 == 0 && calp12 < 0) {
924  salp12 = tiny * calp1;
925  calp12 = -1;
926  }
927  alp12 = atan2(salp12, calp12);
928  }
929  S12 += g->c2 * alp12;
930  S12 *= swapp * lonsign * latsign;
931  /* Convert -0 to 0 */
932  S12 += 0;
933  }
934 
935  /* Convert calp, salp to azimuth accounting for lonsign, swapp, latsign. */
936  if (swapp < 0) {
937  swapx(&salp1, &salp2);
938  swapx(&calp1, &calp2);
939  if (outmask & GEOD_GEODESICSCALE)
940  swapx(&M12, &M21);
941  }
942 
943  salp1 *= swapp * lonsign; calp1 *= swapp * latsign;
944  salp2 *= swapp * lonsign; calp2 *= swapp * latsign;
945 
946  if (outmask & GEOD_AZIMUTH) {
947  /* minus signs give range [-180, 180). 0- converts -0 to +0. */
948  azi1 = atan2dx(salp1, calp1);
949  azi2 = atan2dx(salp2, calp2);
950  }
951 
952  if (outmask & GEOD_DISTANCE)
953  *ps12 = s12;
954  if (outmask & GEOD_AZIMUTH) {
955  if (pazi1) *pazi1 = azi1;
956  if (pazi2) *pazi2 = azi2;
957  }
958  if (outmask & GEOD_REDUCEDLENGTH)
959  *pm12 = m12;
960  if (outmask & GEOD_GEODESICSCALE) {
961  if (pM12) *pM12 = M12;
962  if (pM21) *pM21 = M21;
963  }
964  if (outmask & GEOD_AREA)
965  *pS12 = S12;
966 
967  /* Returned value in [0, 180] */
968  return a12;
969 }
970 
971 void geod_inverse(const struct geod_geodesic* g,
972  real lat1, real lon1, real lat2, real lon2,
973  real* ps12, real* pazi1, real* pazi2) {
974  geod_geninverse(g, lat1, lon1, lat2, lon2, ps12, pazi1, pazi2, 0, 0, 0, 0);
975 }
976 
977 real SinCosSeries(boolx sinp, real sinx, real cosx, const real c[], int n) {
978  /* Evaluate
979  * y = sinp ? sum(c[i] * sin( 2*i * x), i, 1, n) :
980  * sum(c[i] * cos((2*i+1) * x), i, 0, n-1)
981  * using Clenshaw summation. N.B. c[0] is unused for sin series
982  * Approx operation count = (n + 5) mult and (2 * n + 2) add */
983  real ar, y0, y1;
984  c += (n + sinp); /* Point to one beyond last element */
985  ar = 2 * (cosx - sinx) * (cosx + sinx); /* 2 * cos(2 * x) */
986  y0 = n & 1 ? *--c : 0; y1 = 0; /* accumulators for sum */
987  /* Now n is even */
988  n /= 2;
989  while (n--) {
990  /* Unroll loop x 2, so accumulators return to their original role */
991  y1 = ar * y0 - y1 + *--c;
992  y0 = ar * y1 - y0 + *--c;
993  }
994  return sinp
995  ? 2 * sinx * cosx * y0 /* sin(2 * x) * y0 */
996  : cosx * (y0 - y1); /* cos(x) * (y0 - y1) */
997 }
998 
999 void Lengths(const struct geod_geodesic* g,
1000  real eps, real sig12,
1001  real ssig1, real csig1, real dn1,
1002  real ssig2, real csig2, real dn2,
1003  real cbet1, real cbet2,
1004  real* ps12b, real* pm12b, real* pm0,
1005  real* pM12, real* pM21,
1006  /* Scratch area of the right size */
1007  real Ca[]) {
1008  real m0 = 0, J12 = 0, A1 = 0, A2 = 0;
1009  real Cb[nC];
1010 
1011  /* Return m12b = (reduced length)/b; also calculate s12b = distance/b,
1012  * and m0 = coefficient of secular term in expression for reduced length. */
1013  boolx redlp = pm12b || pm0 || pM12 || pM21;
1014  if (ps12b || redlp) {
1015  A1 = A1m1f(eps);
1016  C1f(eps, Ca);
1017  if (redlp) {
1018  A2 = A2m1f(eps);
1019  C2f(eps, Cb);
1020  m0 = A1 - A2;
1021  A2 = 1 + A2;
1022  }
1023  A1 = 1 + A1;
1024  }
1025  if (ps12b) {
1026  real B1 = SinCosSeries(TRUE, ssig2, csig2, Ca, nC1) -
1027  SinCosSeries(TRUE, ssig1, csig1, Ca, nC1);
1028  /* Missing a factor of b */
1029  *ps12b = A1 * (sig12 + B1);
1030  if (redlp) {
1031  real B2 = SinCosSeries(TRUE, ssig2, csig2, Cb, nC2) -
1032  SinCosSeries(TRUE, ssig1, csig1, Cb, nC2);
1033  J12 = m0 * sig12 + (A1 * B1 - A2 * B2);
1034  }
1035  } else if (redlp) {
1036  /* Assume here that nC1 >= nC2 */
1037  int l;
1038  for (l = 1; l <= nC2; ++l)
1039  Cb[l] = A1 * Ca[l] - A2 * Cb[l];
1040  J12 = m0 * sig12 + (SinCosSeries(TRUE, ssig2, csig2, Cb, nC2) -
1041  SinCosSeries(TRUE, ssig1, csig1, Cb, nC2));
1042  }
1043  if (pm0) *pm0 = m0;
1044  if (pm12b)
1045  /* Missing a factor of b.
1046  * Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
1047  * accurate cancellation in the case of coincident points. */
1048  *pm12b = dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) -
1049  csig1 * csig2 * J12;
1050  if (pM12 || pM21) {
1051  real csig12 = csig1 * csig2 + ssig1 * ssig2;
1052  real t = g->ep2 * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2);
1053  if (pM12)
1054  *pM12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1;
1055  if (pM21)
1056  *pM21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2;
1057  }
1058 }
1059 
1060 real Astroid(real x, real y) {
1061  /* Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
1062  * This solution is adapted from Geocentric::Reverse. */
1063  real k;
1064  real
1065  p = sq(x),
1066  q = sq(y),
1067  r = (p + q - 1) / 6;
1068  if ( !(q == 0 && r <= 0) ) {
1069  real
1070  /* Avoid possible division by zero when r = 0 by multiplying equations
1071  * for s and t by r^3 and r, resp. */
1072  S = p * q / 4, /* S = r^3 * s */
1073  r2 = sq(r),
1074  r3 = r * r2,
1075  /* The discriminant of the quadratic equation for T3. This is zero on
1076  * the evolute curve p^(1/3)+q^(1/3) = 1 */
1077  disc = S * (S + 2 * r3);
1078  real u = r;
1079  real v, uv, w;
1080  if (disc >= 0) {
1081  real T3 = S + r3, T;
1082  /* Pick the sign on the sqrt to maximize abs(T3). This minimizes loss
1083  * of precision due to cancellation. The result is unchanged because
1084  * of the way the T is used in definition of u. */
1085  T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); /* T3 = (r * t)^3 */
1086  /* N.B. cbrtx always returns the real root. cbrtx(-8) = -2. */
1087  T = cbrtx(T3); /* T = r * t */
1088  /* T can be zero; but then r2 / T -> 0. */
1089  u += T + (T != 0 ? r2 / T : 0);
1090  } else {
1091  /* T is complex, but the way u is defined the result is real. */
1092  real ang = atan2(sqrt(-disc), -(S + r3));
1093  /* There are three possible cube roots. We choose the root which
1094  * avoids cancellation. Note that disc < 0 implies that r < 0. */
1095  u += 2 * r * cos(ang / 3);
1096  }
1097  v = sqrt(sq(u) + q); /* guaranteed positive */
1098  /* Avoid loss of accuracy when u < 0. */
1099  uv = u < 0 ? q / (v - u) : u + v; /* u+v, guaranteed positive */
1100  w = (uv - q) / (2 * v); /* positive? */
1101  /* Rearrange expression for k to avoid loss of accuracy due to
1102  * subtraction. Division by 0 not possible because uv > 0, w >= 0. */
1103  k = uv / (sqrt(uv + sq(w)) + w); /* guaranteed positive */
1104  } else { /* q == 0 && r <= 0 */
1105  /* y = 0 with |x| <= 1. Handle this case directly.
1106  * for y small, positive root is k = abs(y)/sqrt(1-x^2) */
1107  k = 0;
1108  }
1109  return k;
1110 }
1111 
1112 real InverseStart(const struct geod_geodesic* g,
1113  real sbet1, real cbet1, real dn1,
1114  real sbet2, real cbet2, real dn2,
1115  real lam12,
1116  real* psalp1, real* pcalp1,
1117  /* Only updated if return val >= 0 */
1118  real* psalp2, real* pcalp2,
1119  /* Only updated for short lines */
1120  real* pdnm,
1121  /* Scratch area of the right size */
1122  real Ca[]) {
1123  real salp1 = 0, calp1 = 0, salp2 = 0, calp2 = 0, dnm = 0;
1124 
1125  /* Return a starting point for Newton's method in salp1 and calp1 (function
1126  * value is -1). If Newton's method doesn't need to be used, return also
1127  * salp2 and calp2 and function value is sig12. */
1128  real
1129  sig12 = -1, /* Return value */
1130  /* bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0] */
1131  sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
1132  cbet12 = cbet2 * cbet1 + sbet2 * sbet1;
1133  real sbet12a;
1134  boolx shortline = cbet12 >= 0 && sbet12 < (real)(0.5) &&
1135  cbet2 * lam12 < (real)(0.5);
1136  real omg12 = lam12, somg12, comg12, ssig12, csig12;
1137 #if defined(__GNUC__) && __GNUC__ == 4 && \
1138  (__GNUC_MINOR__ < 6 || defined(__MINGW32__))
1139  /* Volatile declaration needed to fix inverse cases
1140  * 88.202499451857 0 -88.202499451857 179.981022032992859592
1141  * 89.262080389218 0 -89.262080389218 179.992207982775375662
1142  * 89.333123580033 0 -89.333123580032997687 179.99295812360148422
1143  * which otherwise fail with g++ 4.4.4 x86 -O3 (Linux)
1144  * and g++ 4.4.0 (mingw) and g++ 4.6.1 (tdm mingw). */
1145  {
1146  volatile real xx1 = sbet2 * cbet1;
1147  volatile real xx2 = cbet2 * sbet1;
1148  sbet12a = xx1 + xx2;
1149  }
1150 #else
1151  sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
1152 #endif
1153  if (shortline) {
1154  real sbetm2 = sq(sbet1 + sbet2);
1155  /* sin((bet1+bet2)/2)^2
1156  * = (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2) */
1157  sbetm2 /= sbetm2 + sq(cbet1 + cbet2);
1158  dnm = sqrt(1 + g->ep2 * sbetm2);
1159  omg12 /= g->f1 * dnm;
1160  }
1161  somg12 = sin(omg12); comg12 = cos(omg12);
1162 
1163  salp1 = cbet2 * somg12;
1164  calp1 = comg12 >= 0 ?
1165  sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12) :
1166  sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
1167 
1168  ssig12 = hypotx(salp1, calp1);
1169  csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
1170 
1171  if (shortline && ssig12 < g->etol2) {
1172  /* really short lines */
1173  salp2 = cbet1 * somg12;
1174  calp2 = sbet12 - cbet1 * sbet2 *
1175  (comg12 >= 0 ? sq(somg12) / (1 + comg12) : 1 - comg12);
1176  norm2(&salp2, &calp2);
1177  /* Set return value */
1178  sig12 = atan2(ssig12, csig12);
1179  } else if (fabs(g->n) > (real)(0.1) || /* No astroid calc if too eccentric */
1180  csig12 >= 0 ||
1181  ssig12 >= 6 * fabs(g->n) * pi * sq(cbet1)) {
1182  /* Nothing to do, zeroth order spherical approximation is OK */
1183  } else {
1184  /* Scale lam12 and bet2 to x, y coordinate system where antipodal point
1185  * is at origin and singular point is at y = 0, x = -1. */
1186  real y, lamscale, betscale;
1187  /* Volatile declaration needed to fix inverse case
1188  * 56.320923501171 0 -56.320923501171 179.664747671772880215
1189  * which otherwise fails with g++ 4.4.4 x86 -O3 */
1190  volatile real x;
1191  if (g->f >= 0) { /* In fact f == 0 does not get here */
1192  /* x = dlong, y = dlat */
1193  {
1194  real
1195  k2 = sq(sbet1) * g->ep2,
1196  eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
1197  lamscale = g->f * cbet1 * A3f(g, eps) * pi;
1198  }
1199  betscale = lamscale * cbet1;
1200 
1201  x = (lam12 - pi) / lamscale;
1202  y = sbet12a / betscale;
1203  } else { /* f < 0 */
1204  /* x = dlat, y = dlong */
1205  real
1206  cbet12a = cbet2 * cbet1 - sbet2 * sbet1,
1207  bet12a = atan2(sbet12a, cbet12a);
1208  real m12b, m0;
1209  /* In the case of lon12 = 180, this repeats a calculation made in
1210  * Inverse. */
1211  Lengths(g, g->n, pi + bet12a,
1212  sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
1213  cbet1, cbet2, 0, &m12b, &m0, 0, 0, Ca);
1214  x = -1 + m12b / (cbet1 * cbet2 * m0 * pi);
1215  betscale = x < -(real)(0.01) ? sbet12a / x :
1216  -g->f * sq(cbet1) * pi;
1217  lamscale = betscale / cbet1;
1218  y = (lam12 - pi) / lamscale;
1219  }
1220 
1221  if (y > -tol1 && x > -1 - xthresh) {
1222  /* strip near cut */
1223  if (g->f >= 0) {
1224  salp1 = minx((real)(1), -(real)(x)); calp1 = - sqrt(1 - sq(salp1));
1225  } else {
1226  calp1 = maxx((real)(x > -tol1 ? 0 : -1), (real)(x));
1227  salp1 = sqrt(1 - sq(calp1));
1228  }
1229  } else {
1230  /* Estimate alp1, by solving the astroid problem.
1231  *
1232  * Could estimate alpha1 = theta + pi/2, directly, i.e.,
1233  * calp1 = y/k; salp1 = -x/(1+k); for f >= 0
1234  * calp1 = x/(1+k); salp1 = -y/k; for f < 0 (need to check)
1235  *
1236  * However, it's better to estimate omg12 from astroid and use
1237  * spherical formula to compute alp1. This reduces the mean number of
1238  * Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
1239  * (min 0 max 5). The changes in the number of iterations are as
1240  * follows:
1241  *
1242  * change percent
1243  * 1 5
1244  * 0 78
1245  * -1 16
1246  * -2 0.6
1247  * -3 0.04
1248  * -4 0.002
1249  *
1250  * The histogram of iterations is (m = number of iterations estimating
1251  * alp1 directly, n = number of iterations estimating via omg12, total
1252  * number of trials = 148605):
1253  *
1254  * iter m n
1255  * 0 148 186
1256  * 1 13046 13845
1257  * 2 93315 102225
1258  * 3 36189 32341
1259  * 4 5396 7
1260  * 5 455 1
1261  * 6 56 0
1262  *
1263  * Because omg12 is near pi, estimate work with omg12a = pi - omg12 */
1264  real k = Astroid(x, y);
1265  real
1266  omg12a = lamscale * ( g->f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k );
1267  somg12 = sin(omg12a); comg12 = -cos(omg12a);
1268  /* Update spherical estimate of alp1 using omg12 instead of lam12 */
1269  salp1 = cbet2 * somg12;
1270  calp1 = sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
1271  }
1272  }
1273  /* Sanity check on starting guess. Backwards check allows NaN through. */
1274  if (!(salp1 <= 0))
1275  norm2(&salp1, &calp1);
1276  else {
1277  salp1 = 1; calp1 = 0;
1278  }
1279 
1280  *psalp1 = salp1;
1281  *pcalp1 = calp1;
1282  if (shortline)
1283  *pdnm = dnm;
1284  if (sig12 >= 0) {
1285  *psalp2 = salp2;
1286  *pcalp2 = calp2;
1287  }
1288  return sig12;
1289 }
1290 
1291 real Lambda12(const struct geod_geodesic* g,
1292  real sbet1, real cbet1, real dn1,
1293  real sbet2, real cbet2, real dn2,
1294  real salp1, real calp1,
1295  real* psalp2, real* pcalp2,
1296  real* psig12,
1297  real* pssig1, real* pcsig1,
1298  real* pssig2, real* pcsig2,
1299  real* peps, real* pdomg12,
1300  boolx diffp, real* pdlam12,
1301  /* Scratch area of the right size */
1302  real Ca[]) {
1303  real salp2 = 0, calp2 = 0, sig12 = 0,
1304  ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, eps = 0, domg12 = 0, dlam12 = 0;
1305  real salp0, calp0;
1306  real somg1, comg1, somg2, comg2, omg12, lam12;
1307  real B312, h0, k2;
1308 
1309  if (sbet1 == 0 && calp1 == 0)
1310  /* Break degeneracy of equatorial line. This case has already been
1311  * handled. */
1312  calp1 = -tiny;
1313 
1314  /* sin(alp1) * cos(bet1) = sin(alp0) */
1315  salp0 = salp1 * cbet1;
1316  calp0 = hypotx(calp1, salp1 * sbet1); /* calp0 > 0 */
1317 
1318  /* tan(bet1) = tan(sig1) * cos(alp1)
1319  * tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1) */
1320  ssig1 = sbet1; somg1 = salp0 * sbet1;
1321  csig1 = comg1 = calp1 * cbet1;
1322  norm2(&ssig1, &csig1);
1323  /* norm2(&somg1, &comg1); -- don't need to normalize! */
1324 
1325  /* Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful
1326  * about this case, since this can yield singularities in the Newton
1327  * iteration.
1328  * sin(alp2) * cos(bet2) = sin(alp0) */
1329  salp2 = cbet2 != cbet1 ? salp0 / cbet2 : salp1;
1330  /* calp2 = sqrt(1 - sq(salp2))
1331  * = sqrt(sq(calp0) - sq(sbet2)) / cbet2
1332  * and subst for calp0 and rearrange to give (choose positive sqrt
1333  * to give alp2 in [0, pi/2]). */
1334  calp2 = cbet2 != cbet1 || fabs(sbet2) != -sbet1 ?
1335  sqrt(sq(calp1 * cbet1) +
1336  (cbet1 < -sbet1 ?
1337  (cbet2 - cbet1) * (cbet1 + cbet2) :
1338  (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
1339  fabs(calp1);
1340  /* tan(bet2) = tan(sig2) * cos(alp2)
1341  * tan(omg2) = sin(alp0) * tan(sig2). */
1342  ssig2 = sbet2; somg2 = salp0 * sbet2;
1343  csig2 = comg2 = calp2 * cbet2;
1344  norm2(&ssig2, &csig2);
1345  /* norm2(&somg2, &comg2); -- don't need to normalize! */
1346 
1347  /* sig12 = sig2 - sig1, limit to [0, pi] */
1348  sig12 = atan2(maxx(csig1 * ssig2 - ssig1 * csig2, (real)(0)),
1349  csig1 * csig2 + ssig1 * ssig2);
1350 
1351  /* omg12 = omg2 - omg1, limit to [0, pi] */
1352  omg12 = atan2(maxx(comg1 * somg2 - somg1 * comg2, (real)(0)),
1353  comg1 * comg2 + somg1 * somg2);
1354  k2 = sq(calp0) * g->ep2;
1355  eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
1356  C3f(g, eps, Ca);
1357  B312 = (SinCosSeries(TRUE, ssig2, csig2, Ca, nC3-1) -
1358  SinCosSeries(TRUE, ssig1, csig1, Ca, nC3-1));
1359  h0 = -g->f * A3f(g, eps);
1360  domg12 = salp0 * h0 * (sig12 + B312);
1361  lam12 = omg12 + domg12;
1362 
1363  if (diffp) {
1364  if (calp2 == 0)
1365  dlam12 = - 2 * g->f1 * dn1 / sbet1;
1366  else {
1367  Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
1368  cbet1, cbet2, 0, &dlam12, 0, 0, 0, Ca);
1369  dlam12 *= g->f1 / (calp2 * cbet2);
1370  }
1371  }
1372 
1373  *psalp2 = salp2;
1374  *pcalp2 = calp2;
1375  *psig12 = sig12;
1376  *pssig1 = ssig1;
1377  *pcsig1 = csig1;
1378  *pssig2 = ssig2;
1379  *pcsig2 = csig2;
1380  *peps = eps;
1381  *pdomg12 = domg12;
1382  if (diffp)
1383  *pdlam12 = dlam12;
1384 
1385  return lam12;
1386 }
1387 
1388 real A3f(const struct geod_geodesic* g, real eps) {
1389  /* Evaluate A3 */
1390  return polyval(nA3 - 1, g->A3x, eps);
1391 }
1392 
1393 void C3f(const struct geod_geodesic* g, real eps, real c[]) {
1394  /* Evaluate C3 coeffs
1395  * Elements c[1] thru c[nC3 - 1] are set */
1396  real mult = 1;
1397  int o = 0, l;
1398  for (l = 1; l < nC3; ++l) { /* l is index of C3[l] */
1399  int m = nC3 - l - 1; /* order of polynomial in eps */
1400  mult *= eps;
1401  c[l] = mult * polyval(m, g->C3x + o, eps);
1402  o += m + 1;
1403  }
1404 }
1405 
1406 void C4f(const struct geod_geodesic* g, real eps, real c[]) {
1407  /* Evaluate C4 coeffs
1408  * Elements c[0] thru c[nC4 - 1] are set */
1409  real mult = 1;
1410  int o = 0, l;
1411  for (l = 0; l < nC4; ++l) { /* l is index of C4[l] */
1412  int m = nC4 - l - 1; /* order of polynomial in eps */
1413  c[l] = mult * polyval(m, g->C4x + o, eps);
1414  o += m + 1;
1415  mult *= eps;
1416  }
1417 }
1418 
1419 /* The scale factor A1-1 = mean value of (d/dsigma)I1 - 1 */
1420 real A1m1f(real eps) {
1421  static const real coeff[] = {
1422  /* (1-eps)*A1-1, polynomial in eps2 of order 3 */
1423  1, 4, 64, 0, 256,
1424  };
1425  int m = nA1/2;
1426  real t = polyval(m, coeff, sq(eps)) / coeff[m + 1];
1427  return (t + eps) / (1 - eps);
1428 }
1429 
1430 /* The coefficients C1[l] in the Fourier expansion of B1 */
1431 void C1f(real eps, real c[]) {
1432  static const real coeff[] = {
1433  /* C1[1]/eps^1, polynomial in eps2 of order 2 */
1434  -1, 6, -16, 32,
1435  /* C1[2]/eps^2, polynomial in eps2 of order 2 */
1436  -9, 64, -128, 2048,
1437  /* C1[3]/eps^3, polynomial in eps2 of order 1 */
1438  9, -16, 768,
1439  /* C1[4]/eps^4, polynomial in eps2 of order 1 */
1440  3, -5, 512,
1441  /* C1[5]/eps^5, polynomial in eps2 of order 0 */
1442  -7, 1280,
1443  /* C1[6]/eps^6, polynomial in eps2 of order 0 */
1444  -7, 2048,
1445  };
1446  real
1447  eps2 = sq(eps),
1448  d = eps;
1449  int o = 0, l;
1450  for (l = 1; l <= nC1; ++l) { /* l is index of C1p[l] */
1451  int m = (nC1 - l) / 2; /* order of polynomial in eps^2 */
1452  c[l] = d * polyval(m, coeff + o, eps2) / coeff[o + m + 1];
1453  o += m + 2;
1454  d *= eps;
1455  }
1456 }
1457 
1458 /* The coefficients C1p[l] in the Fourier expansion of B1p */
1459 void C1pf(real eps, real c[]) {
1460  static const real coeff[] = {
1461  /* C1p[1]/eps^1, polynomial in eps2 of order 2 */
1462  205, -432, 768, 1536,
1463  /* C1p[2]/eps^2, polynomial in eps2 of order 2 */
1464  4005, -4736, 3840, 12288,
1465  /* C1p[3]/eps^3, polynomial in eps2 of order 1 */
1466  -225, 116, 384,
1467  /* C1p[4]/eps^4, polynomial in eps2 of order 1 */
1468  -7173, 2695, 7680,
1469  /* C1p[5]/eps^5, polynomial in eps2 of order 0 */
1470  3467, 7680,
1471  /* C1p[6]/eps^6, polynomial in eps2 of order 0 */
1472  38081, 61440,
1473  };
1474  real
1475  eps2 = sq(eps),
1476  d = eps;
1477  int o = 0, l;
1478  for (l = 1; l <= nC1p; ++l) { /* l is index of C1p[l] */
1479  int m = (nC1p - l) / 2; /* order of polynomial in eps^2 */
1480  c[l] = d * polyval(m, coeff + o, eps2) / coeff[o + m + 1];
1481  o += m + 2;
1482  d *= eps;
1483  }
1484 }
1485 
1486 /* The scale factor A2-1 = mean value of (d/dsigma)I2 - 1 */
1487 real A2m1f(real eps) {
1488  static const real coeff[] = {
1489  /* (eps+1)*A2-1, polynomial in eps2 of order 3 */
1490  -11, -28, -192, 0, 256,
1491  };
1492  int m = nA2/2;
1493  real t = polyval(m, coeff, sq(eps)) / coeff[m + 1];
1494  return (t - eps) / (1 + eps);
1495 }
1496 
1497 /* The coefficients C2[l] in the Fourier expansion of B2 */
1498 void C2f(real eps, real c[]) {
1499  static const real coeff[] = {
1500  /* C2[1]/eps^1, polynomial in eps2 of order 2 */
1501  1, 2, 16, 32,
1502  /* C2[2]/eps^2, polynomial in eps2 of order 2 */
1503  35, 64, 384, 2048,
1504  /* C2[3]/eps^3, polynomial in eps2 of order 1 */
1505  15, 80, 768,
1506  /* C2[4]/eps^4, polynomial in eps2 of order 1 */
1507  7, 35, 512,
1508  /* C2[5]/eps^5, polynomial in eps2 of order 0 */
1509  63, 1280,
1510  /* C2[6]/eps^6, polynomial in eps2 of order 0 */
1511  77, 2048,
1512  };
1513  real
1514  eps2 = sq(eps),
1515  d = eps;
1516  int o = 0, l;
1517  for (l = 1; l <= nC2; ++l) { /* l is index of C2[l] */
1518  int m = (nC2 - l) / 2; /* order of polynomial in eps^2 */
1519  c[l] = d * polyval(m, coeff + o, eps2) / coeff[o + m + 1];
1520  o += m + 2;
1521  d *= eps;
1522  }
1523 }
1524 
1525 /* The scale factor A3 = mean value of (d/dsigma)I3 */
1526 void A3coeff(struct geod_geodesic* g) {
1527  static const real coeff[] = {
1528  /* A3, coeff of eps^5, polynomial in n of order 0 */
1529  -3, 128,
1530  /* A3, coeff of eps^4, polynomial in n of order 1 */
1531  -2, -3, 64,
1532  /* A3, coeff of eps^3, polynomial in n of order 2 */
1533  -1, -3, -1, 16,
1534  /* A3, coeff of eps^2, polynomial in n of order 2 */
1535  3, -1, -2, 8,
1536  /* A3, coeff of eps^1, polynomial in n of order 1 */
1537  1, -1, 2,
1538  /* A3, coeff of eps^0, polynomial in n of order 0 */
1539  1, 1,
1540  };
1541  int o = 0, k = 0, j;
1542  for (j = nA3 - 1; j >= 0; --j) { /* coeff of eps^j */
1543  int m = nA3 - j - 1 < j ? nA3 - j - 1 : j; /* order of polynomial in n */
1544  g->A3x[k++] = polyval(m, coeff + o, g->n) / coeff[o + m + 1];
1545  o += m + 2;
1546  }
1547 }
1548 
1549 /* The coefficients C3[l] in the Fourier expansion of B3 */
1550 void C3coeff(struct geod_geodesic* g) {
1551  static const real coeff[] = {
1552  /* C3[1], coeff of eps^5, polynomial in n of order 0 */
1553  3, 128,
1554  /* C3[1], coeff of eps^4, polynomial in n of order 1 */
1555  2, 5, 128,
1556  /* C3[1], coeff of eps^3, polynomial in n of order 2 */
1557  -1, 3, 3, 64,
1558  /* C3[1], coeff of eps^2, polynomial in n of order 2 */
1559  -1, 0, 1, 8,
1560  /* C3[1], coeff of eps^1, polynomial in n of order 1 */
1561  -1, 1, 4,
1562  /* C3[2], coeff of eps^5, polynomial in n of order 0 */
1563  5, 256,
1564  /* C3[2], coeff of eps^4, polynomial in n of order 1 */
1565  1, 3, 128,
1566  /* C3[2], coeff of eps^3, polynomial in n of order 2 */
1567  -3, -2, 3, 64,
1568  /* C3[2], coeff of eps^2, polynomial in n of order 2 */
1569  1, -3, 2, 32,
1570  /* C3[3], coeff of eps^5, polynomial in n of order 0 */
1571  7, 512,
1572  /* C3[3], coeff of eps^4, polynomial in n of order 1 */
1573  -10, 9, 384,
1574  /* C3[3], coeff of eps^3, polynomial in n of order 2 */
1575  5, -9, 5, 192,
1576  /* C3[4], coeff of eps^5, polynomial in n of order 0 */
1577  7, 512,
1578  /* C3[4], coeff of eps^4, polynomial in n of order 1 */
1579  -14, 7, 512,
1580  /* C3[5], coeff of eps^5, polynomial in n of order 0 */
1581  21, 2560,
1582  };
1583  int o = 0, k = 0, l, j;
1584  for (l = 1; l < nC3; ++l) { /* l is index of C3[l] */
1585  for (j = nC3 - 1; j >= l; --j) { /* coeff of eps^j */
1586  int m = nC3 - j - 1 < j ? nC3 - j - 1 : j; /* order of polynomial in n */
1587  g->C3x[k++] = polyval(m, coeff + o, g->n) / coeff[o + m + 1];
1588  o += m + 2;
1589  }
1590  }
1591 }
1592 
1593 /* The coefficients C4[l] in the Fourier expansion of I4 */
1594 void C4coeff(struct geod_geodesic* g) {
1595  static const real coeff[] = {
1596  /* C4[0], coeff of eps^5, polynomial in n of order 0 */
1597  97, 15015,
1598  /* C4[0], coeff of eps^4, polynomial in n of order 1 */
1599  1088, 156, 45045,
1600  /* C4[0], coeff of eps^3, polynomial in n of order 2 */
1601  -224, -4784, 1573, 45045,
1602  /* C4[0], coeff of eps^2, polynomial in n of order 3 */
1603  -10656, 14144, -4576, -858, 45045,
1604  /* C4[0], coeff of eps^1, polynomial in n of order 4 */
1605  64, 624, -4576, 6864, -3003, 15015,
1606  /* C4[0], coeff of eps^0, polynomial in n of order 5 */
1607  100, 208, 572, 3432, -12012, 30030, 45045,
1608  /* C4[1], coeff of eps^5, polynomial in n of order 0 */
1609  1, 9009,
1610  /* C4[1], coeff of eps^4, polynomial in n of order 1 */
1611  -2944, 468, 135135,
1612  /* C4[1], coeff of eps^3, polynomial in n of order 2 */
1613  5792, 1040, -1287, 135135,
1614  /* C4[1], coeff of eps^2, polynomial in n of order 3 */
1615  5952, -11648, 9152, -2574, 135135,
1616  /* C4[1], coeff of eps^1, polynomial in n of order 4 */
1617  -64, -624, 4576, -6864, 3003, 135135,
1618  /* C4[2], coeff of eps^5, polynomial in n of order 0 */
1619  8, 10725,
1620  /* C4[2], coeff of eps^4, polynomial in n of order 1 */
1621  1856, -936, 225225,
1622  /* C4[2], coeff of eps^3, polynomial in n of order 2 */
1623  -8448, 4992, -1144, 225225,
1624  /* C4[2], coeff of eps^2, polynomial in n of order 3 */
1625  -1440, 4160, -4576, 1716, 225225,
1626  /* C4[3], coeff of eps^5, polynomial in n of order 0 */
1627  -136, 63063,
1628  /* C4[3], coeff of eps^4, polynomial in n of order 1 */
1629  1024, -208, 105105,
1630  /* C4[3], coeff of eps^3, polynomial in n of order 2 */
1631  3584, -3328, 1144, 315315,
1632  /* C4[4], coeff of eps^5, polynomial in n of order 0 */
1633  -128, 135135,
1634  /* C4[4], coeff of eps^4, polynomial in n of order 1 */
1635  -2560, 832, 405405,
1636  /* C4[5], coeff of eps^5, polynomial in n of order 0 */
1637  128, 99099,
1638  };
1639  int o = 0, k = 0, l, j;
1640  for (l = 0; l < nC4; ++l) { /* l is index of C4[l] */
1641  for (j = nC4 - 1; j >= l; --j) { /* coeff of eps^j */
1642  int m = nC4 - j - 1; /* order of polynomial in n */
1643  g->C4x[k++] = polyval(m, coeff + o, g->n) / coeff[o + m + 1];
1644  o += m + 2;
1645  }
1646  }
1647 }
1648 
1649 int transit(real lon1, real lon2) {
1650  real lon12;
1651  /* Return 1 or -1 if crossing prime meridian in east or west direction.
1652  * Otherwise return zero. */
1653  /* Compute lon12 the same way as Geodesic::Inverse. */
1654  lon1 = AngNormalize(lon1);
1655  lon2 = AngNormalize(lon2);
1656  lon12 = AngDiff(lon1, lon2);
1657  return lon1 < 0 && lon2 >= 0 && lon12 > 0 ? 1 :
1658  (lon2 < 0 && lon1 >= 0 && lon12 < 0 ? -1 : 0);
1659 }
1660 
1661 int transitdirect(real lon1, real lon2) {
1662  lon1 = fmod(lon1, (real)(720));
1663  lon2 = fmod(lon2, (real)(720));
1664  return ( ((lon2 >= 0 && lon2 < 360) || lon2 < -360 ? 0 : 1) -
1665  ((lon1 >= 0 && lon1 < 360) || lon1 < -360 ? 0 : 1) );
1666 }
1667 
1668 void accini(real s[]) {
1669  /* Initialize an accumulator; this is an array with two elements. */
1670  s[0] = s[1] = 0;
1671 }
1672 
1673 void acccopy(const real s[], real t[]) {
1674  /* Copy an accumulator; t = s. */
1675  t[0] = s[0]; t[1] = s[1];
1676 }
1677 
1678 void accadd(real s[], real y) {
1679  /* Add y to an accumulator. */
1680  real u, z = sumx(y, s[1], &u);
1681  s[0] = sumx(z, s[0], &s[1]);
1682  if (s[0] == 0)
1683  s[0] = u;
1684  else
1685  s[1] = s[1] + u;
1686 }
1687 
1688 real accsum(const real s[], real y) {
1689  /* Return accumulator + y (but don't add to accumulator). */
1690  real t[2];
1691  acccopy(s, t);
1692  accadd(t, y);
1693  return t[0];
1694 }
1695 
1696 void accneg(real s[]) {
1697  /* Negate an accumulator. */
1698  s[0] = -s[0]; s[1] = -s[1];
1699 }
1700 
1701 void geod_polygon_init(struct geod_polygon* p, boolx polylinep) {
1702  p->lat0 = p->lon0 = p->lat = p->lon = NaN;
1703  p->polyline = (polylinep != 0);
1704  accini(p->P);
1705  accini(p->A);
1706  p->num = p->crossings = 0;
1707 }
1708 
1709 void geod_polygon_addpoint(const struct geod_geodesic* g,
1710  struct geod_polygon* p,
1711  real lat, real lon) {
1712  lon = AngNormalize(lon);
1713  if (p->num == 0) {
1714  p->lat0 = p->lat = lat;
1715  p->lon0 = p->lon = lon;
1716  } else {
1717  real s12, S12;
1718  geod_geninverse(g, p->lat, p->lon, lat, lon,
1719  &s12, 0, 0, 0, 0, 0, p->polyline ? 0 : &S12);
1720  accadd(p->P, s12);
1721  if (!p->polyline) {
1722  accadd(p->A, S12);
1723  p->crossings += transit(p->lon, lon);
1724  }
1725  p->lat = lat; p->lon = lon;
1726  }
1727  ++p->num;
1728 }
1729 
1730 void geod_polygon_addedge(const struct geod_geodesic* g,
1731  struct geod_polygon* p,
1732  real azi, real s) {
1733  if (p->num) { /* Do nothing is num is zero */
1734  real lat, lon, S12;
1735  geod_gendirect(g, p->lat, p->lon, azi, GEOD_LONG_UNROLL, s,
1736  &lat, &lon, 0,
1737  0, 0, 0, 0, p->polyline ? 0 : &S12);
1738  accadd(p->P, s);
1739  if (!p->polyline) {
1740  accadd(p->A, S12);
1741  p->crossings += transitdirect(p->lon, lon);
1742  }
1743  p->lat = lat; p->lon = lon;
1744  ++p->num;
1745  }
1746 }
1747 
1748 unsigned geod_polygon_compute(const struct geod_geodesic* g,
1749  const struct geod_polygon* p,
1750  boolx reverse, boolx sign,
1751  real* pA, real* pP) {
1752  real s12, S12, t[2], area0;
1753  int crossings;
1754  if (p->num < 2) {
1755  if (pP) *pP = 0;
1756  if (!p->polyline && pA) *pA = 0;
1757  return p->num;
1758  }
1759  if (p->polyline) {
1760  if (pP) *pP = p->P[0];
1761  return p->num;
1762  }
1763  geod_geninverse(g, p->lat, p->lon, p->lat0, p->lon0,
1764  &s12, 0, 0, 0, 0, 0, &S12);
1765  if (pP) *pP = accsum(p->P, s12);
1766  acccopy(p->A, t);
1767  accadd(t, S12);
1768  crossings = p->crossings + transit(p->lon, p->lon0);
1769  area0 = 4 * pi * g->c2;
1770  if (crossings & 1)
1771  accadd(t, (t[0] < 0 ? 1 : -1) * area0/2);
1772  /* area is with the clockwise sense. If !reverse convert to
1773  * counter-clockwise convention. */
1774  if (!reverse)
1775  accneg(t);
1776  /* If sign put area in (-area0/2, area0/2], else put area in [0, area0) */
1777  if (sign) {
1778  if (t[0] > area0/2)
1779  accadd(t, -area0);
1780  else if (t[0] <= -area0/2)
1781  accadd(t, +area0);
1782  } else {
1783  if (t[0] >= area0)
1784  accadd(t, -area0);
1785  else if (t[0] < 0)
1786  accadd(t, +area0);
1787  }
1788  if (pA) *pA = 0 + t[0];
1789  return p->num;
1790 }
1791 
1792 unsigned geod_polygon_testpoint(const struct geod_geodesic* g,
1793  const struct geod_polygon* p,
1794  real lat, real lon,
1795  boolx reverse, boolx sign,
1796  real* pA, real* pP) {
1797  real perimeter, tempsum, area0;
1798  int crossings, i;
1799  unsigned num = p->num + 1;
1800  if (num == 1) {
1801  if (pP) *pP = 0;
1802  if (!p->polyline && pA) *pA = 0;
1803  return num;
1804  }
1805  perimeter = p->P[0];
1806  tempsum = p->polyline ? 0 : p->A[0];
1807  crossings = p->crossings;
1808  for (i = 0; i < (p->polyline ? 1 : 2); ++i) {
1809  real s12, S12;
1810  geod_geninverse(g,
1811  i == 0 ? p->lat : lat, i == 0 ? p->lon : lon,
1812  i != 0 ? p->lat0 : lat, i != 0 ? p->lon0 : lon,
1813  &s12, 0, 0, 0, 0, 0, p->polyline ? 0 : &S12);
1814  perimeter += s12;
1815  if (!p->polyline) {
1816  tempsum += S12;
1817  crossings += transit(i == 0 ? p->lon : lon,
1818  i != 0 ? p->lon0 : lon);
1819  }
1820  }
1821 
1822  if (pP) *pP = perimeter;
1823  if (p->polyline)
1824  return num;
1825 
1826  area0 = 4 * pi * g->c2;
1827  if (crossings & 1)
1828  tempsum += (tempsum < 0 ? 1 : -1) * area0/2;
1829  /* area is with the clockwise sense. If !reverse convert to
1830  * counter-clockwise convention. */
1831  if (!reverse)
1832  tempsum *= -1;
1833  /* If sign put area in (-area0/2, area0/2], else put area in [0, area0) */
1834  if (sign) {
1835  if (tempsum > area0/2)
1836  tempsum -= area0;
1837  else if (tempsum <= -area0/2)
1838  tempsum += area0;
1839  } else {
1840  if (tempsum >= area0)
1841  tempsum -= area0;
1842  else if (tempsum < 0)
1843  tempsum += area0;
1844  }
1845  if (pA) *pA = 0 + tempsum;
1846  return num;
1847 }
1848 
1849 unsigned geod_polygon_testedge(const struct geod_geodesic* g,
1850  const struct geod_polygon* p,
1851  real azi, real s,
1852  boolx reverse, boolx sign,
1853  real* pA, real* pP) {
1854  real perimeter, tempsum, area0;
1855  int crossings;
1856  unsigned num = p->num + 1;
1857  if (num == 1) { /* we don't have a starting point! */
1858  if (pP) *pP = NaN;
1859  if (!p->polyline && pA) *pA = NaN;
1860  return 0;
1861  }
1862  perimeter = p->P[0] + s;
1863  if (p->polyline) {
1864  if (pP) *pP = perimeter;
1865  return num;
1866  }
1867 
1868  tempsum = p->A[0];
1869  crossings = p->crossings;
1870  {
1871  real lat, lon, s12, S12;
1872  geod_gendirect(g, p->lat, p->lon, azi, GEOD_LONG_UNROLL, s,
1873  &lat, &lon, 0,
1874  0, 0, 0, 0, &S12);
1875  tempsum += S12;
1876  crossings += transitdirect(p->lon, lon);
1877  geod_geninverse(g, lat, lon, p->lat0, p->lon0,
1878  &s12, 0, 0, 0, 0, 0, &S12);
1879  perimeter += s12;
1880  tempsum += S12;
1881  crossings += transit(lon, p->lon0);
1882  }
1883 
1884  area0 = 4 * pi * g->c2;
1885  if (crossings & 1)
1886  tempsum += (tempsum < 0 ? 1 : -1) * area0/2;
1887  /* area is with the clockwise sense. If !reverse convert to
1888  * counter-clockwise convention. */
1889  if (!reverse)
1890  tempsum *= -1;
1891  /* If sign put area in (-area0/2, area0/2], else put area in [0, area0) */
1892  if (sign) {
1893  if (tempsum > area0/2)
1894  tempsum -= area0;
1895  else if (tempsum <= -area0/2)
1896  tempsum += area0;
1897  } else {
1898  if (tempsum >= area0)
1899  tempsum -= area0;
1900  else if (tempsum < 0)
1901  tempsum += area0;
1902  }
1903  if (pP) *pP = perimeter;
1904  if (pA) *pA = 0 + tempsum;
1905  return num;
1906 }
1907 
1908 void geod_polygonarea(const struct geod_geodesic* g,
1909  real lats[], real lons[], int n,
1910  real* pA, real* pP) {
1911  int i;
1912  struct geod_polygon p;
1913  geod_polygon_init(&p, FALSE);
1914  for (i = 0; i < n; ++i)
1915  geod_polygon_addpoint(g, &p, lats[i], lons[i]);
1916  geod_polygon_compute(g, &p, FALSE, TRUE, pA, pP);
1917 }
1918 
1919 /** @endcond */
unsigned geod_polygon_testedge(const struct geod_geodesic *g, const struct geod_polygon *p, double azi, double s, int reverse, int sign, double *pA, double *pP)
double geod_genposition(const struct geod_geodesicline *l, unsigned flags, double s12_a12, double *plat2, double *plon2, double *pazi2, double *ps12, double *pm12, double *pM12, double *pM21, double *pS12)
GeographicLib::Math::real real
double lon
Definition: geodesic.h:204
void geod_polygon_addedge(const struct geod_geodesic *g, struct geod_polygon *p, double azi, double s)
unsigned num
Definition: geodesic.h:213
void geod_position(const struct geod_geodesicline *l, double s12, double *plat2, double *plon2, double *pazi2)
double f
Definition: geodesic.h:171
void geod_lineinit(struct geod_geodesicline *l, const struct geod_geodesic *g, double lat1, double lon1, double azi1, unsigned caps)
unsigned caps
Definition: geodesic.h:194
double geod_geninverse(const struct geod_geodesic *g, double lat1, double lon1, double lat2, double lon2, double *ps12, double *pazi1, double *pazi2, double *pm12, double *pM12, double *pM21, double *pS12)
void geod_polygon_addpoint(const struct geod_geodesic *g, struct geod_polygon *p, double lat, double lon)
void geod_polygon_init(struct geod_polygon *p, int polylinep)
void geod_direct(const struct geod_geodesic *g, double lat1, double lon1, double azi1, double s12, double *plat2, double *plon2, double *pazi2)
unsigned geod_polygon_compute(const struct geod_geodesic *g, const struct geod_polygon *p, int reverse, int sign, double *pA, double *pP)
void geod_polygonarea(const struct geod_geodesic *g, double lats[], double lons[], int n, double *pA, double *pP)
double a
Definition: geodesic.h:170
double geod_gendirect(const struct geod_geodesic *g, double lat1, double lon1, double azi1, unsigned flags, double s12_a12, double *plat2, double *plon2, double *pazi2, double *ps12, double *pm12, double *pM12, double *pM21, double *pS12)
unsigned geod_polygon_testpoint(const struct geod_geodesic *g, const struct geod_polygon *p, double lat, double lon, int reverse, int sign, double *pA, double *pP)
void geod_inverse(const struct geod_geodesic *g, double lat1, double lon1, double lat2, double lon2, double *ps12, double *pazi1, double *pazi2)
void geod_init(struct geod_geodesic *g, double a, double f)
double lat
Definition: geodesic.h:203
Header for the geodesic routines in C.