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