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