summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--geodesic.c1847
-rw-r--r--geodesic.h111
-rw-r--r--main.c41
-rw-r--r--mkfile11
4 files changed, 2010 insertions, 0 deletions
diff --git a/geodesic.c b/geodesic.c
new file mode 100644
index 0000000..c98f7c8
--- /dev/null
+++ b/geodesic.c
@@ -0,0 +1,1847 @@
+#include <u.h>
+#include <libc.h>
+#include "geodesic.h"
+
+static uint digits, maxit1, maxit2;
+static double ε, realmin, tiny,
+ tol0, tol1, tol2, tolb,
+ xthresh;
+
+static double
+max(double a, double b)
+{
+ return a > b ? a : b;
+}
+
+static double
+min(double a, double b)
+{
+ return a < b ? a : b;
+}
+
+static void
+swap(double *x, double *y)
+{
+ double t;
+
+ t = *x;
+ *x = *y;
+ *y = t;
+}
+
+static double
+sum(double u, double v, double *t)
+{
+ double s, up, vpp;
+
+ s = u + v;
+ up = s - v;
+ vpp = s - up;
+ up -= u;
+ vpp -= v;
+ if(t)
+ *t = -(up + vpp);
+ /*
+ * error-free sum:
+ * u + v = s + t
+ * = round(u + v) + t
+ */
+ return s;
+}
+
+static double
+log1p(double x)
+{
+ double y, z;
+
+ y = x + 1;
+ z = y - 1;
+ /*
+ * Here's the explanation for this magic: y = 1 + z, exactly, and z
+ * approx x, thus log(y)/z (which is nearly constant near z = 0) returns
+ * a good approximation to the true log(1 + x)/x. The multiplication x *
+ * (log(y)/z) introduces little additional error.
+ */
+ return z == 0 ? x : x * log(y)/z;
+}
+
+static double
+atanh(double x)
+{
+ double y;
+
+ y = fabs(x); /* Enforce odd parity */
+ y = log1p(2*y/(1 - y))/2;
+ return x < 0 ? -y : y;
+}
+
+static double
+atan2d(double y, double x)
+{
+ /*
+ * In order to minimize round-off errors, this function rearranges the
+ * arguments so that result of atan2 is in the range [-π/4, π/4] before
+ * converting it to degrees and mapping the result to the correct
+ * quadrant.
+ */
+ double ang;
+ int q;
+
+ q = 0;
+ if(fabs(y) > fabs(x)){
+ swap(&x, &y);
+ q = 2;
+ }
+ if(x < 0){
+ x = -x;
+ ++q;
+ }
+ /* here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4] */
+ ang = atan2(y, x)/DEG;
+ /* Note that atan2d(-0.0, 1.0) will return -0. However, we expect that
+ * atan2d will not be called with y = -0. If need be, include
+ *
+ * case 0: ang = 0 + ang; break;
+ */
+ switch(q){
+ case 1: ang = (y >= 0 ? 180 : -180) - ang; break;
+ case 2: ang = 90 - ang; break;
+ case 3: ang = -90 + ang; break;
+ }
+ return ang;
+}
+
+static double
+cbrt(double x)
+{
+ double y;
+
+ y = pow(fabs(x), 1/3.0);
+ return x < 0 ? -y : y;
+}
+
+static void
+norm2(double *sinx, double *cosx)
+{
+ double r;
+
+ r = hypot(*sinx, *cosx);
+ *sinx /= r;
+ *cosx /= r;
+}
+
+static double
+copysign(double x, double y)
+{
+ return fabs(x)*(y < 0 || (y == 0 && 1/y < 0) ? -1 : 1);
+}
+
+static double
+AngNormalize(double x)
+{
+ double y;
+
+ y = fmod(x, 360.0);
+ return y <= -180 ? y + 360 :
+ y <= 180 ? y : y - 360;
+}
+
+static double
+AngRound(double x)
+{
+ double y, z;
+
+ z = 1/16.0;
+ if(x == 0)
+ return 0;
+ y = fabs(x);
+ /* The compiler mustn't "simplify" z - (z - y) to y */
+ y = y < z ? z - (z - y) : y;
+ return x < 0 ? -y : y;
+}
+
+static double
+AngDiff(double x, double y, double *e)
+{
+ double t, d;
+
+ d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), &t));
+ /*
+ * Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
+ * abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
+ * addition of t takes the result outside the range (-180,180] is d = 180
+ * and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since
+ * sum would have returned the exact result in such a case (i.e., given t
+ * = 0).
+ */
+ return sum(d == 180 && t > 0 ? -180 : d, t, e);
+}
+
+static double
+LatFix(double x)
+{
+ return fabs(x) > 90 ? NaN() : x;
+}
+
+static void
+sincosd(double x, double *sinx, double *cosx)
+{
+ /*
+ * In order to minimize round-off errors, this function exactly reduces
+ * the argument to the range [-45, 45] before converting it to radians.
+ */
+ double r, s, c;
+ int q;
+
+ r = fmod(x, 360.0);
+ q = !isNaN(r) ? (int)floor(r/90 + 0.5) : 0;
+ r -= 90 * q;
+ /* now |r| <= 45 */
+ s = sin(r*DEG);
+ c = cos(r*DEG);
+ switch(q & 3){
+ case 0: *sinx = s; *cosx = c; break;
+ case 1: *sinx = c; *cosx = -s; break;
+ case 2: *sinx = -s; *cosx = -c; break;
+ case 3: *sinx = -c; *cosx = s; break;
+ }
+ if(x != 0){
+ *sinx += 0.0;
+ *cosx += 0.0;
+ }
+}
+
+static double
+polyval(int N, double p[], double x)
+{
+ double y;
+
+ y = N < 0 ? 0 : *p++;
+ while(--N >= 0)
+ y *= x + *p++;
+ return y;
+}
+
+/* The scale factor A3 = mean value of (d/dsigma)I3 */
+static void
+A3coeff(Geodesic *g)
+{
+ static double coeff[] = {
+ /* A3, coeff of eps^5, polynomial in n of order 0 */
+ -3, 128,
+ /* A3, coeff of eps^4, polynomial in n of order 1 */
+ -2, -3, 64,
+ /* A3, coeff of eps^3, polynomial in n of order 2 */
+ -1, -3, -1, 16,
+ /* A3, coeff of eps^2, polynomial in n of order 2 */
+ 3, -1, -2, 8,
+ /* A3, coeff of eps^1, polynomial in n of order 1 */
+ 1, -1, 2,
+ /* A3, coeff of eps^0, polynomial in n of order 0 */
+ 1, 1,
+ };
+ int o, k, j, m;
+
+ o = k = 0;
+ for(j = nA3 - 1; j >= 0; --j){ /* coeff of eps^j */
+ m = min(nA3 - j-1, j); /* order of polynomial in n */
+ g->A3x[k++] = polyval(m, coeff+o, g->n) / coeff[o+m+1];
+ o += m + 2;
+ }
+}
+
+/* The coefficients C3[l] in the Fourier expansion of B3 */
+static void
+C3coeff(Geodesic *g)
+{
+ static double coeff[] = {
+ /* C3[1], coeff of eps^5, polynomial in n of order 0 */
+ 3, 128,
+ /* C3[1], coeff of eps^4, polynomial in n of order 1 */
+ 2, 5, 128,
+ /* C3[1], coeff of eps^3, polynomial in n of order 2 */
+ -1, 3, 3, 64,
+ /* C3[1], coeff of eps^2, polynomial in n of order 2 */
+ -1, 0, 1, 8,
+ /* C3[1], coeff of eps^1, polynomial in n of order 1 */
+ -1, 1, 4,
+ /* C3[2], coeff of eps^5, polynomial in n of order 0 */
+ 5, 256,
+ /* C3[2], coeff of eps^4, polynomial in n of order 1 */
+ 1, 3, 128,
+ /* C3[2], coeff of eps^3, polynomial in n of order 2 */
+ -3, -2, 3, 64,
+ /* C3[2], coeff of eps^2, polynomial in n of order 2 */
+ 1, -3, 2, 32,
+ /* C3[3], coeff of eps^5, polynomial in n of order 0 */
+ 7, 512,
+ /* C3[3], coeff of eps^4, polynomial in n of order 1 */
+ -10, 9, 384,
+ /* C3[3], coeff of eps^3, polynomial in n of order 2 */
+ 5, -9, 5, 192,
+ /* C3[4], coeff of eps^5, polynomial in n of order 0 */
+ 7, 512,
+ /* C3[4], coeff of eps^4, polynomial in n of order 1 */
+ -14, 7, 512,
+ /* C3[5], coeff of eps^5, polynomial in n of order 0 */
+ 21, 2560,
+ };
+ int o, k, l, j, m;
+
+ o = k = 0;
+ for(l = 1; l < nC3; ++l){ /* l is index of C3[l] */
+ for(j = nC3 - 1; j >= l; --j){ /* coeff of eps^j */
+ m = min(nC3 - j-1, j); /* order of polynomial in n */
+ g->C3x[k++] = polyval(m, coeff+o, g->n) / coeff[o+m+1];
+ o += m + 2;
+ }
+ }
+}
+
+/* The coefficients C4[l] in the Fourier expansion of I4 */
+static void
+C4coeff(Geodesic *g)
+{
+ static double coeff[] = {
+ /* C4[0], coeff of eps^5, polynomial in n of order 0 */
+ 97, 15015,
+ /* C4[0], coeff of eps^4, polynomial in n of order 1 */
+ 1088, 156, 45045,
+ /* C4[0], coeff of eps^3, polynomial in n of order 2 */
+ -224, -4784, 1573, 45045,
+ /* C4[0], coeff of eps^2, polynomial in n of order 3 */
+ -10656, 14144, -4576, -858, 45045,
+ /* C4[0], coeff of eps^1, polynomial in n of order 4 */
+ 64, 624, -4576, 6864, -3003, 15015,
+ /* C4[0], coeff of eps^0, polynomial in n of order 5 */
+ 100, 208, 572, 3432, -12012, 30030, 45045,
+ /* C4[1], coeff of eps^5, polynomial in n of order 0 */
+ 1, 9009,
+ /* C4[1], coeff of eps^4, polynomial in n of order 1 */
+ -2944, 468, 135135,
+ /* C4[1], coeff of eps^3, polynomial in n of order 2 */
+ 5792, 1040, -1287, 135135,
+ /* C4[1], coeff of eps^2, polynomial in n of order 3 */
+ 5952, -11648, 9152, -2574, 135135,
+ /* C4[1], coeff of eps^1, polynomial in n of order 4 */
+ -64, -624, 4576, -6864, 3003, 135135,
+ /* C4[2], coeff of eps^5, polynomial in n of order 0 */
+ 8, 10725,
+ /* C4[2], coeff of eps^4, polynomial in n of order 1 */
+ 1856, -936, 225225,
+ /* C4[2], coeff of eps^3, polynomial in n of order 2 */
+ -8448, 4992, -1144, 225225,
+ /* C4[2], coeff of eps^2, polynomial in n of order 3 */
+ -1440, 4160, -4576, 1716, 225225,
+ /* C4[3], coeff of eps^5, polynomial in n of order 0 */
+ -136, 63063,
+ /* C4[3], coeff of eps^4, polynomial in n of order 1 */
+ 1024, -208, 105105,
+ /* C4[3], coeff of eps^3, polynomial in n of order 2 */
+ 3584, -3328, 1144, 315315,
+ /* C4[4], coeff of eps^5, polynomial in n of order 0 */
+ -128, 135135,
+ /* C4[4], coeff of eps^4, polynomial in n of order 1 */
+ -2560, 832, 405405,
+ /* C4[5], coeff of eps^5, polynomial in n of order 0 */
+ 128, 99099,
+ };
+ int o, k, l, j, m;
+
+ o = k = 0;
+ for(l = 0; l < nC4; ++l){ /* l is index of C4[l] */
+ for(j = nC4 - 1; j >= l; --j){ /* coeff of eps^j */
+ m = nC4 - j-1; /* order of polynomial in n */
+ g->C4x[k++] = polyval(m, coeff+o, g->n) / coeff[o+m+1];
+ o += m + 2;
+ }
+ }
+}
+
+/* The scale factor A1-1 = mean value of (d/dsigma)I1 - 1 */
+static double
+A1m1f(double eps)
+{
+ static double coeff[] = {
+ /* (1-eps)*A1-1, polynomial in eps2 of order 3 */
+ 1, 4, 64, 0, 256,
+ };
+ double t;
+ int m;
+
+ m = nA1/2;
+ t = polyval(m, coeff, eps*eps)/coeff[m+1];
+ return (t + eps)/(1 - eps);
+}
+
+/* The coefficients C1[l] in the Fourier expansion of B1 */
+static void
+C1f(double eps, double c[])
+{
+ static double coeff[] = {
+ /* C1[1]/eps^1, polynomial in eps2 of order 2 */
+ -1, 6, -16, 32,
+ /* C1[2]/eps^2, polynomial in eps2 of order 2 */
+ -9, 64, -128, 2048,
+ /* C1[3]/eps^3, polynomial in eps2 of order 1 */
+ 9, -16, 768,
+ /* C1[4]/eps^4, polynomial in eps2 of order 1 */
+ 3, -5, 512,
+ /* C1[5]/eps^5, polynomial in eps2 of order 0 */
+ -7, 1280,
+ /* C1[6]/eps^6, polynomial in eps2 of order 0 */
+ -7, 2048,
+ };
+ double eps², d;
+ int o, l, m;
+
+ eps² = eps*eps;
+ d = eps;
+ o = 0;
+ for(l = 1; l <= nC1; ++l){ /* l is index of C1p[l] */
+ m = (nC1 - l)/2; /* order of polynomial in eps^2 */
+ c[l] = d * polyval(m, coeff+o, eps²)/coeff[o+m+1];
+ o += m + 2;
+ d *= eps;
+ }
+}
+
+/* The coefficients C1p[l] in the Fourier expansion of B1p */
+static void
+C1pf(double eps, double c[])
+{
+ static double coeff[] = {
+ /* C1p[1]/eps^1, polynomial in eps2 of order 2 */
+ 205, -432, 768, 1536,
+ /* C1p[2]/eps^2, polynomial in eps2 of order 2 */
+ 4005, -4736, 3840, 12288,
+ /* C1p[3]/eps^3, polynomial in eps2 of order 1 */
+ -225, 116, 384,
+ /* C1p[4]/eps^4, polynomial in eps2 of order 1 */
+ -7173, 2695, 7680,
+ /* C1p[5]/eps^5, polynomial in eps2 of order 0 */
+ 3467, 7680,
+ /* C1p[6]/eps^6, polynomial in eps2 of order 0 */
+ 38081, 61440,
+ };
+ double eps², d;
+ int o, l, m;
+
+ eps² = eps*eps,
+ d = eps;
+ o = 0;
+ for(l = 1; l <= nC1p; ++l){ /* l is index of C1p[l] */
+ m = (nC1p - l)/2; /* order of polynomial in eps^2 */
+ c[l] = d * polyval(m, coeff+o, eps²)/coeff[o+m+1];
+ o += m + 2;
+ d *= eps;
+ }
+}
+
+/* The scale factor A2-1 = mean value of (d/dsigma)I2 - 1 */
+static double
+A2m1f(double eps)
+{
+ static double coeff[] = {
+ /* (eps+1)*A2-1, polynomial in eps2 of order 3 */
+ -11, -28, -192, 0, 256,
+ };
+ double t;
+ int m;
+
+ m = nA2/2;
+ t = polyval(m, coeff, eps*eps)/coeff[m+1];
+ return (t - eps)/(1 + eps);
+}
+
+/* The coefficients C2[l] in the Fourier expansion of B2 */
+static void
+C2f(double eps, double c[])
+{
+ static double coeff[] = {
+ /* C2[1]/eps^1, polynomial in eps2 of order 2 */
+ 1, 2, 16, 32,
+ /* C2[2]/eps^2, polynomial in eps2 of order 2 */
+ 35, 64, 384, 2048,
+ /* C2[3]/eps^3, polynomial in eps2 of order 1 */
+ 15, 80, 768,
+ /* C2[4]/eps^4, polynomial in eps2 of order 1 */
+ 7, 35, 512,
+ /* C2[5]/eps^5, polynomial in eps2 of order 0 */
+ 63, 1280,
+ /* C2[6]/eps^6, polynomial in eps2 of order 0 */
+ 77, 2048,
+ };
+ double eps², d;
+ int o, l, m;
+
+ eps² = eps*eps,
+ d = eps;
+ o = 0;
+ for(l = 1; l <= nC2; ++l){ /* l is index of C2[l] */
+ m = (nC2 - l)/2; /* order of polynomial in eps^2 */
+ c[l] = d * polyval(m, coeff+o, eps²)/coeff[o+m+1];
+ o += m + 2;
+ d *= eps;
+ }
+}
+
+static double
+A3f(Geodesic *g, double eps)
+{
+ /* Evaluate A3 */
+ return polyval(nA3 - 1, g->A3x, eps);
+}
+
+static void
+C3f(Geodesic *g, double eps, double c[])
+{
+ /*
+ * Evaluate C3 coeffs
+ * Elements c[1] through c[nC3 - 1] are set
+ */
+ double mult;
+ int o, l, m;
+
+ mult = 1;
+ o = 0;
+ for(l = 1; l < nC3; ++l){ /* l is index of C3[l] */
+ m = nC3 - l-1; /* order of polynomial in eps */
+ mult *= eps;
+ c[l] = mult*polyval(m, g->C3x + o, eps);
+ o += m + 1;
+ }
+}
+
+static void
+C4f(Geodesic *g, double eps, double c[])
+{
+ /*
+ * Evaluate C4 coeffs
+ * Elements c[0] through c[nC4 - 1] are set
+ */
+ double mult;
+ int o, l, m;
+
+ mult = 1;
+ o = 0;
+ for(l = 0; l < nC4; ++l){ /* l is index of C4[l] */
+ m = nC4 - l-1; /* order of polynomial in eps */
+ c[l] = mult*polyval(m, g->C4x + o, eps);
+ o += m + 1;
+ mult *= eps;
+ }
+}
+
+static double
+SinCosSeries(int sinp, double sinx, double cosx, double c[], int n)
+{
+ /*
+ * Evaluate
+ * y = sinp ? sum(c[i] * sin( 2*i * x), i, 1, n) :
+ * sum(c[i] * cos((2*i+1) * x), i, 0, n-1)
+ * using Clenshaw summation. N.B. c[0] is unused for sin series
+ * Approx operation count = (n + 5) mult and (2*n + 2) add
+ */
+ double ar, y0, y1;
+
+ c += (n + sinp); /* Point to one beyond last element */
+ ar = 2*(cosx - sinx)*(cosx + sinx); /* 2 * cos(2 * x) */
+ /* accumulators for sum */
+ y0 = (n & 1) ? *--c : 0;
+ y1 = 0;
+ /* Now n is even */
+ n /= 2;
+ while(n--){
+ /* Unroll loop x 2, so accumulators return to their original role */
+ y1 = ar * y0 - y1 + *--c;
+ y0 = ar * y1 - y0 + *--c;
+ }
+ return sinp ?
+ 2 * sinx * cosx * y0 : /* sin(2 * x) * y0 */
+ cosx * (y0 - y1); /* cos(x) * (y0 - y1) */
+}
+
+static void
+Lengths(Geodesic *g,
+ double eps, double sig12,
+ double ssig1, double csig1, double dn1,
+ double ssig2, double csig2, double dn2,
+ double cbet1, double cbet2,
+ double *ps12b, double *pm12b, double *pm0,
+ double *pM12, double *pM21,
+ /* Scratch area of the right size */
+ double Ca[])
+{
+ double m0, J12, A1, A2;
+ double Cb[nC];
+ int redlp;
+
+ m0 = J12 = A1 = A2 = 0;
+
+ /* Return m12b = (reduced length)/b; also calculate s12b = distance/b,
+ * and m0 = coefficient of secular term in expression for reduced length. */
+ redlp = pm12b || pm0 || pM12 || pM21;
+ if(ps12b || redlp){
+ A1 = A1m1f(eps);
+ C1f(eps, Ca);
+ if(redlp){
+ A2 = A2m1f(eps);
+ C2f(eps, Cb);
+ m0 = A1 - A2;
+ A2 = 1 + A2;
+ }
+ A1 = 1 + A1;
+ }
+ if(ps12b){
+ double B1;
+
+ B1 = SinCosSeries(1, ssig2, csig2, Ca, nC1) -
+ SinCosSeries(1, ssig1, csig1, Ca, nC1);
+ /* Missing a factor of b */
+ *ps12b = A1*(sig12 + B1);
+ if(redlp){
+ double B2;
+
+ B2 = SinCosSeries(1, ssig2, csig2, Cb, nC2) -
+ SinCosSeries(1, ssig1, csig1, Cb, nC2);
+ J12 = m0 * sig12 + (A1*B1 - A2*B2);
+ }
+ }else if(redlp){
+ /* Assume here that nC1 >= nC2 */
+ int l;
+
+ for(l = 1; l <= nC2; ++l)
+ Cb[l] = A1*Ca[l] - A2*Cb[l];
+ J12 = m0 * sig12 + (SinCosSeries(1, ssig2, csig2, Cb, nC2) -
+ SinCosSeries(1, ssig1, csig1, Cb, nC2));
+ }
+ if(pm0)
+ *pm0 = m0;
+ if(pm12b)
+ /*
+ * Missing a factor of b.
+ * Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
+ * accurate cancellation in the case of coincident points.
+ */
+ *pm12b = dn2 * (csig1*ssig2) - dn1 * (ssig1*csig2) - csig1*csig2 * J12;
+ if(pM12 || pM21){
+ double csig12, t;
+
+ csig12 = csig1*csig2 + ssig1*ssig2;
+ t = g->ep2*(cbet1 - cbet2)*(cbet1 + cbet2)/(dn1 + dn2);
+ if(pM12)
+ *pM12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1;
+ if(pM21)
+ *pM21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2;
+ }
+}
+
+static double
+Astroid(double x, double y)
+{
+ /* Solve k⁴+2*k³-(x²+y²-1)*k²-2*y²*k-y² = 0 for positive root k. */
+ double k, p, q, r;
+
+ p = x*x;
+ q = y*y;
+ r = (p + q - 1)/6;
+ if(!(q == 0 && r <= 0)){
+ double S, r2, r3, disc;
+ double u, v, uv, w;
+
+ /*
+ * Avoid possible division by zero when r = 0 by multiplying equations
+ * for s and t by r^3 and r, resp.
+ */
+ S = p*q / 4; /* S = r^3 * s */
+ r2 = r*r;
+ r3 = r*r2;
+ /* The discriminant of the quadratic equation for T3. This is zero on
+ * the evolute curve p^(1/3)+q^(1/3) = 1 */
+ disc = S*(S + 2*r3);
+ u = r;
+ if(disc >= 0){
+ double T3, T;
+
+ T3 = S + r3;
+ /*
+ * Pick the sign on the sqrt to maximize abs(T3). This minimizes loss
+ * of precision due to cancellation. The result is unchanged because
+ * of the way the T is used in definition of u.
+ */
+ T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); /* T3 = (r * t)^3 */
+ /* N.B. cbrtx always returns the real root. cbrtx(-8) = -2. */
+ T = cbrt(T3); /* T = r * t */
+ /* T can be zero; but then r2 / T -> 0. */
+ u += T + (T != 0 ? r2/T : 0);
+ }else{
+ double ang;
+
+ /* T is complex, but the way u is defined the result is real. */
+ ang = atan2(sqrt(-disc), -(S + r3));
+ /*
+ * There are three possible cube roots. We choose the root which
+ * avoids cancellation. Note that disc < 0 implies that r < 0.
+ */
+ u += 2*r*cos(ang/3);
+ }
+ v = sqrt(u*u + q); /* guaranteed positive */
+ /* Avoid loss of accuracy when u < 0. */
+ uv = u < 0 ? q/(v - u) : u + v; /* u+v, guaranteed positive */
+
+ w = (uv - q)/(2*v); /* positive? */
+ /*
+ * Rearrange expression for k to avoid loss of accuracy due to
+ * subtraction. Division by 0 not possible because uv > 0, w >= 0.
+ */
+ k = uv/(sqrt(uv + w*w) + w); /* guaranteed positive */
+ }else{ /* q == 0 && r <= 0 */
+ /*
+ * y = 0 with |x| <= 1. Handle this case directly.
+ * for y small, positive root is k = abs(y)/sqrt(1-x^2)
+ */
+ k = 0;
+ }
+ return k;
+}
+
+static double
+InverseStart(Geodesic *g,
+ double sbet1, double cbet1, double dn1,
+ double sbet2, double cbet2, double dn2,
+ double lam12, double slam12, double clam12,
+ double *psalp1, double *pcalp1,
+ /* Only updated if return val >= 0 */
+ double *psalp2, double *pcalp2,
+ /* Only updated for short lines */
+ double *pdnm,
+ /* Scratch area of the right size */
+ double Ca[])
+{
+ double salp1, calp1, salp2, calp2, dnm;
+ double sig12, sbet12, cbet12, sbet12a;
+ double somg12, comg12, ssig12, csig12;
+ int shortline;
+
+ salp1 = calp1 = salp2 = calp2 = dnm = 0;
+ /*
+ * Return a starting point for Newton's method in salp1 and calp1 (function
+ * value is -1). If Newton's method doesn't need to be used, return also
+ * salp2 and calp2 and function value is sig12.
+ */
+ sig12 = -1; /* Return value */
+ /* bet12 = bet2 - bet1 in [0, π); bet12a = bet2 + bet1 in (-π, 0] */
+ sbet12 = sbet2*cbet1 - cbet2*sbet1;
+ cbet12 = cbet2*cbet1 + sbet2*sbet1;
+ shortline = cbet12 >= 0 && sbet12 < 0.5 &&
+ cbet2*lam12 < 0.5;
+ sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
+ if(shortline){
+ double sbetm2, omg12;
+
+ sbetm2 = (sbet1 + sbet2)*(sbet1 + sbet2);
+ /*
+ * sin((bet1+bet2)/2)^2
+ * = (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2)
+ */
+ sbetm2 /= sbetm2 + (cbet1 + cbet2)*(cbet1 + cbet2);
+ dnm = sqrt(1 + g->ep2 * sbetm2);
+ omg12 = lam12 / (g->f1 * dnm);
+ somg12 = sin(omg12);
+ comg12 = cos(omg12);
+ }else{
+ somg12 = slam12;
+ comg12 = clam12;
+ }
+
+ salp1 = cbet2*somg12;
+ calp1 = comg12 >= 0 ?
+ sbet12 + cbet2 * sbet1 * somg12*somg12 / (1 + comg12) :
+ sbet12a - cbet2 * sbet1 * somg12*somg12 / (1 - comg12);
+
+ ssig12 = hypot(salp1, calp1);
+ csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
+
+ if(shortline && ssig12 < g->etol2){
+ /* really short lines */
+ salp2 = cbet1 * somg12;
+ calp2 = sbet12 - cbet1 * sbet2 *
+ (comg12 >= 0 ? somg12*somg12 / (1 + comg12) : 1 - comg12);
+ norm2(&salp2, &calp2);
+ /* Set return value */
+ sig12 = atan2(ssig12, csig12);
+ }else if(fabs(g->n) > 0.1 || /* No astroid calc if too eccentric */
+ csig12 >= 0 ||
+ ssig12 >= 6 * fabs(g->n) * PI * cbet1*cbet1){
+ /* Nothing to do, zeroth order spherical approximation is OK */
+ }else{
+ /*
+ * Scale lam12 and bet2 to x, y coordinate system where antipodal point
+ * is at origin and singular point is at y = 0, x = -1.
+ */
+ double x, y, lamscale, betscale;
+ double lam12x;
+
+ lam12x = atan2(-slam12, -clam12); /* lam12 - π */
+ if(g->f >= 0){ /* In fact f == 0 does not get here */
+ /* x = dlong, y = dlat */
+ {
+ double k2, eps;
+
+ k2 = sbet1*sbet1 * g->ep2;
+ eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
+ lamscale = g->f * cbet1 * A3f(g, eps) * PI;
+ }
+ betscale = lamscale * cbet1;
+ x = lam12x / lamscale;
+ y = sbet12a / betscale;
+ }else{ /* f < 0 */
+
+ /* x = dlat, y = dlong */
+ double cbet12a, bet12a;
+ double m12b, m0;
+
+ cbet12a = cbet2*cbet1 - sbet2*sbet1;
+ bet12a = atan2(sbet12a, cbet12a);
+ /*
+ * In the case of lon12 = 180, this repeats a calculation made in
+ * Inverse.
+ */
+ Lengths(g, g->n, PI + bet12a,
+ sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
+ cbet1, cbet2, nil, &m12b, &m0, nil, nil, Ca);
+ x = -1 + m12b / (cbet1*cbet2*m0 * PI);
+ betscale = x < -0.01 ? sbet12a/x :
+ -g->f * cbet1*cbet1 * PI;
+ lamscale = betscale / cbet1;
+ y = lam12x / lamscale;
+ }
+
+ if(y > -tol1 && x > -1 - xthresh){
+ /* strip near cut */
+ if(g->f >= 0){
+ salp1 = min(1, -x);
+ calp1 = -sqrt(1 - salp1*salp1);
+ }else{
+ calp1 = max(x > -tol1 ? 0 : -1, x);
+ salp1 = sqrt(1 - calp1*calp1);
+ }
+ }else{
+ /*
+ * Estimate alp1, by solving the astroid problem.
+ *
+ * Could estimate alpha1 = theta + pi/2, directly, i.e.,
+ * calp1 = y/k; salp1 = -x/(1+k); for f >= 0
+ * calp1 = x/(1+k); salp1 = -y/k; for f < 0 (need to check)
+ *
+ * However, it's better to estimate omg12 from astroid and use
+ * spherical formula to compute alp1. This reduces the mean number of
+ * Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
+ * (min 0 max 5). The changes in the number of iterations are as
+ * follows:
+ *
+ * change percent
+ * 1 5
+ * 0 78
+ * -1 16
+ * -2 0.6
+ * -3 0.04
+ * -4 0.002
+ *
+ * The histogram of iterations is (m = number of iterations estimating
+ * alp1 directly, n = number of iterations estimating via omg12, total
+ * number of trials = 148605):
+ *
+ * iter m n
+ * 0 148 186
+ * 1 13046 13845
+ * 2 93315 102225
+
+ * 3 36189 32341
+ * 4 5396 7
+ * 5 455 1
+ * 6 56 0
+ *
+ * Because omg12 is near π, estimate work with omg12a = π - omg12
+ */
+ double k, omg12a;
+
+ k = Astroid(x, y);
+ omg12a = lamscale*(g->f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k);
+ somg12 = sin(omg12a);
+ comg12 = -cos(omg12a);
+ /* Update spherical estimate of alp1 using omg12 instead of lam12 */
+ salp1 = cbet2*somg12;
+ calp1 = sbet12a - cbet2 * sbet1 * somg12*somg12/(1 - comg12);
+ }
+ }
+ /* Sanity check on starting guess. Backwards check allows NaN through. */
+ if(!(salp1 <= 0))
+ norm2(&salp1, &calp1);
+ else{
+ salp1 = 1;
+ calp1 = 0;
+ }
+
+ *psalp1 = salp1;
+ *pcalp1 = calp1;
+ if(shortline)
+ *pdnm = dnm;
+ if(sig12 >= 0){
+ *psalp2 = salp2;
+ *pcalp2 = calp2;
+ }
+ return sig12;
+}
+
+static double
+Lambda12(Geodesic *g,
+ double sbet1, double cbet1, double dn1,
+ double sbet2, double cbet2, double dn2,
+ double salp1, double calp1,
+ double slam120, double clam120,
+ double *psalp2, double *pcalp2,
+ double *psig12,
+ double *pssig1, double *pcsig1,
+ double *pssig2, double *pcsig2,
+ double *peps,
+ double *pdomg12,
+ int diffp, double *pdlam12,
+ /* Scratch area of the right size */
+ double Ca[])
+{
+ double salp2, calp2, sig12, ssig1, csig1, ssig2, csig2, eps, domg12, dlam12;
+ double salp0, calp0;
+ double somg1, comg1, somg2, comg2, somg12, comg12, lam12;
+ double B312, eta, k2;
+
+ ssig1 = csig1 = ssig2 = csig2 = dlam12 = 0;
+ if(sbet1 == 0 && calp1 == 0)
+ /*
+ * Break degeneracy of equatorial line. This case has already been
+ * handled.
+ */
+ calp1 = -tiny;
+
+ /* sin(alp1) * cos(bet1) = sin(alp0) */
+ salp0 = salp1 * cbet1;
+ calp0 = hypot(calp1, salp1*sbet1); /* calp0 > 0 */
+
+ /*
+ * tan(bet1) = tan(sig1) * cos(alp1)
+ * tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1)
+ */
+ ssig1 = sbet1;
+ somg1 = salp0 * sbet1;
+ csig1 = comg1 = calp1 * cbet1;
+ norm2(&ssig1, &csig1);
+ /* norm2(&somg1, &comg1); -- don't need to normalize! */
+
+ /* Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful
+ * about this case, since this can yield singularities in the Newton
+ * iteration.
+ * sin(alp2) * cos(bet2) = sin(alp0) */
+ salp2 = cbet2 != cbet1 ? salp0/cbet2 : salp1;
+ /*
+ * calp2 = sqrt(1 - sq(salp2))
+ * = sqrt(sq(calp0) - sq(sbet2)) / cbet2
+ * and subst for calp0 and rearrange to give (choose positive sqrt
+ * to give alp2 in [0, pi/2]).
+ */
+ calp2 = cbet2 != cbet1 || fabs(sbet2) != -sbet1 ?
+ sqrt((calp1 * cbet1)*(calp1 * cbet1) +
+ (cbet1 < -sbet1 ? (cbet2 - cbet1) * (cbet1 + cbet2) :
+ (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
+ fabs(calp1);
+ /*
+ * tan(bet2) = tan(sig2) * cos(alp2)
+ * tan(omg2) = sin(alp0) * tan(sig2).
+ */
+ ssig2 = sbet2;
+ somg2 = salp0 * sbet2;
+ csig2 = comg2 = calp2 * cbet2;
+ norm2(&ssig2, &csig2);
+ /* norm2(&somg2, &comg2); -- don't need to normalize! */
+
+ /* sig12 = sig2 - sig1, limit to [0, pi] */
+ sig12 = atan2(max(0, csig1*ssig2 - ssig1*csig2), csig1*csig2 + ssig1*ssig2);
+
+ /* omg12 = omg2 - omg1, limit to [0, pi] */
+ somg12 = max(0, comg1*somg2 - somg1*comg2);
+ comg12 = comg1*comg2 + somg1*somg2;
+ /* eta = omg12 - lam120 */
+ eta = atan2(somg12*clam120 - comg12*slam120, comg12*clam120 + somg12*slam120);
+ k2 = calp0*calp0 * g->ep2;
+ eps = k2/(2*(1 + sqrt(1 + k2)) + k2);
+ C3f(g, eps, Ca);
+ B312 = SinCosSeries(1, ssig2, csig2, Ca, nC3-1) -
+ SinCosSeries(1, ssig1, csig1, Ca, nC3-1);
+ domg12 = -g->f * A3f(g, eps) * salp0 * (sig12 + B312);
+ lam12 = eta + domg12;
+
+ if(diffp){
+ if(calp2 == 0)
+ dlam12 = -2*g->f1*dn1 / sbet1;
+ else{
+ Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
+ cbet1, cbet2, nil, &dlam12, nil, nil, nil, Ca);
+ dlam12 *= g->f1/(calp2*cbet2);
+ }
+ }
+
+ *psalp2 = salp2;
+ *pcalp2 = calp2;
+ *psig12 = sig12;
+ *pssig1 = ssig1;
+ *pcsig1 = csig1;
+ *pssig2 = ssig2;
+ *pcsig2 = csig2;
+ *peps = eps;
+ *pdomg12 = domg12;
+ if(diffp)
+ *pdlam12 = dlam12;
+ return lam12;
+}
+
+
+static void
+init(void)
+{
+ digits = 53;
+ ε = pow(0.5, digits - 1);
+ realmin = pow(0.5, 1022);
+ maxit1 = 20;
+ maxit2 = maxit1 + digits + 10;
+ tiny = sqrt(realmin);
+ tol0 = ε;
+ /*
+ * Increase multiplier in defn of tol1 from 100 to 200 to fix inverse case
+ * 52.784459512564 0 -52.784459512563990912 179.634407464943777557
+ * which otherwise failed for Visual Studio 10 (Release and Debug)
+ */
+ tol1 = 200 * tol0;
+ tol2 = sqrt(tol0);
+ /* Check on bisection interval */
+ tolb = tol0 * tol2;
+ xthresh = 1000 * tol2;
+}
+
+static void
+initgeodline_int(Geodline *l, Geodesic *g,
+ double lat1, double lon1,
+ double azi1, double salp1, double calp1,
+ uint caps)
+{
+ double cbet1, sbet1, eps;
+
+ l->a = g->a;
+ l->f = g->f;
+ l->b = g->b;
+ l->c2 = g->c2;
+ l->f1 = g->f1;
+ /* If caps is 0 assume the standard direct calculation */
+ l->caps = (caps ? caps : GDistanceIn | GLongitude) |
+ /* always allow latitude and azimuth and unrolling of longitude */
+ GLatitude | GAzimuth | GUnrollLon;
+ l->lat1 = LatFix(lat1);
+ l->lon1 = lon1;
+ l->azi1 = azi1;
+ l->salp1 = salp1;
+ l->calp1 = calp1;
+
+ sincosd(AngRound(l->lat1), &sbet1, &cbet1);
+ sbet1 *= l->f1;
+ /* Ensure cbet1 = +epsilon at poles */
+ norm2(&sbet1, &cbet1);
+ cbet1 = max(tiny, cbet1);
+ l->dn1 = sqrt(1 + g->ep2 * sbet1*sbet1);
+
+ /* Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0), */
+ l->salp0 = l->salp1 * cbet1; /* alp0 in [0, π/2 - |bet1|] */
+ /*
+ * Alt: calp0 = hypot(sbet1, calp1 * cbet1). The following
+ * is slightly better (consider the case salp1 = 0).
+ */
+ l->calp0 = hypot(l->calp1, l->salp1 * sbet1);
+ /*
+ * Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
+ * sig = 0 is nearest northward crossing of equator.
+ * With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
+ * With bet1 = pi/2, alp1 = -pi, sig1 = pi/2
+ * With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2
+ * Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
+ * With alp0 in (0, pi/2], quadrants for sig and omg coincide.
+ * No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
+ * With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi.
+ */
+ l->ssig1 = sbet1;
+ l->somg1 = l->salp0 * sbet1;
+ l->csig1 = l->comg1 = sbet1 != 0 || l->calp1 != 0 ? cbet1 * l->calp1 : 1;
+ norm2(&l->ssig1, &l->csig1); /* sig1 in (-π, π] */
+ /* norm2(somg1, comg1); -- don't need to normalize! */
+
+ l->k2 = l->calp0*l->calp0 * g->ep2;
+ eps = l->k2 / (2*(1 + sqrt(1 + l->k2)) + l->k2);
+
+ if(l->caps & CapC1){
+ double s, c;
+
+ l->A1m1 = A1m1f(eps);
+ C1f(eps, l->C1a);
+ l->B11 = SinCosSeries(1, l->ssig1, l->csig1, l->C1a, nC1);
+ s = sin(l->B11);
+ c = cos(l->B11);
+ /* tau1 = sig1 + B11 */
+ l->stau1 = l->ssig1*c + l->csig1*s;
+ l->ctau1 = l->csig1*c - l->ssig1*s;
+ /*
+ * Not necessary because C1pa reverts C1a
+ * B11 = -SinCosSeries(1, stau1, ctau1, C1pa, nC1p);
+ */
+ }
+ if(l->caps & CapC1p)
+ C1pf(eps, l->C1pa);
+ if(l->caps & CapC2){
+ l->A2m1 = A2m1f(eps);
+ C2f(eps, l->C2a);
+ l->B21 = SinCosSeries(1, l->ssig1, l->csig1, l->C2a, nC2);
+ }
+ if(l->caps & CapC3){
+ C3f(g, eps, l->C3a);
+ l->A3c = -l->f * l->salp0 * A3f(g, eps);
+ l->B31 = SinCosSeries(1, l->ssig1, l->csig1, l->C3a, nC3-1);
+ }
+ if(l->caps & CapC4){
+ C4f(g, eps, l->C4a);
+ /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0) */
+ l->A4 = l->a*l->a * l->calp0 * l->salp0 * g->e2;
+ l->B41 = SinCosSeries(0, l->ssig1, l->csig1, l->C4a, nC4);
+ }
+
+ l->a13 = l->s13 = NaN();
+}
+
+void
+initgeodline(Geodline *l, Geodesic *g, double lat1, double lon1, double azi1, uint caps)
+{
+ double salp1, calp1;
+
+ azi1 = AngNormalize(azi1);
+ /* Guard against underflow in salp0 */
+ sincosd(AngRound(azi1), &salp1, &calp1);
+ initgeodline_int(l, g, lat1, lon1, azi1, salp1, calp1, caps);
+}
+
+double
+geod_genposition(Geodline *l,
+ uint flags, double s12_a12,
+ double *plat2, double *plon2, double *pazi2,
+ double *ps12, double *pm12,
+ double *pM12, double *pM21,
+ double *pS12)
+{
+ double lat2, lon2, azi2, s12, m12, M12, M21, S12;
+ double sig12, ssig12, csig12, B12, AB1;
+ double omg12, lam12, lon12;
+ double ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2;
+ int outmask;
+
+ lat2 = lon2 = azi2 = s12 = m12 = M12 = M21 = S12 = 0;
+ /* Avoid warning about uninitialized B12. */
+ B12 = AB1 = 0;
+ outmask = 0;
+ outmask |= plat2 ? GLatitude : GNone;
+ outmask |= plon2 ? GLongitude : GNone;
+ outmask |= pazi2 ? GAzimuth : GNone;
+ outmask |= ps12 ? GDistance : GNone;
+ outmask |= pm12 ? GReducedLength : GNone;
+ outmask |= pM12 || pM21 ? GGeodesicScale : GNone;
+ outmask |= pS12 ? GArea : GNone;
+
+ outmask &= l->caps & OUT_ALL;
+ if(!(flags & GArcMode || (l->caps & (GDistanceIn & OUT_ALL))))
+ /* Impossible distance calculation requested */
+ return NaN();
+
+ if(flags & GArcMode){
+ /* Interpret s12_a12 as spherical arc length */
+ sig12 = s12_a12*DEG;
+ sincosd(s12_a12, &ssig12, &csig12);
+ }else{
+ double tau12, s, c;
+
+ /* Interpret s12_a12 as distance */
+ tau12 = s12_a12 / (l->b * (1 + l->A1m1));
+ s = sin(tau12);
+ c = cos(tau12);
+ /* tau2 = tau1 + tau12 */
+ B12 = - SinCosSeries(1,
+ l->stau1 * c + l->ctau1 * s,
+ l->ctau1 * c - l->stau1 * s,
+ l->C1pa, nC1p);
+ sig12 = tau12 - (B12 - l->B11);
+ ssig12 = sin(sig12);
+ csig12 = cos(sig12);
+ if(fabs(l->f) > 0.01){
+ /* Reverted distance series is inaccurate for |f| > 1/100, so correct
+ * sig12 with 1 Newton iteration. The following table shows the
+ * approximate maximum error for a = WGS_a() and various f relative to
+ * GeodesicExact.
+ * erri = the error in the inverse solution (nm)
+ * errd = the error in the direct solution (series only) (nm)
+ * errda = the error in the direct solution (series + 1 Newton) (nm)
+ *
+ * f erri errd errda
+ * -1/5 12e6 1.2e9 69e6
+ * -1/10 123e3 12e6 765e3
+ * -1/20 1110 108e3 7155
+ * -1/50 18.63 200.9 27.12
+ * -1/100 18.63 23.78 23.37
+ * -1/150 18.63 21.05 20.26
+ * 1/150 22.35 24.73 25.83
+ * 1/100 22.35 25.03 25.31
+ * 1/50 29.80 231.9 30.44
+ * 1/20 5376 146e3 10e3
+ * 1/10 829e3 22e6 1.5e6
+ * 1/5 157e6 3.8e9 280e6 */
+ double serr;
+
+ ssig2 = l->ssig1*csig12 + l->csig1*ssig12;
+ csig2 = l->csig1*csig12 - l->ssig1*ssig12;
+ B12 = SinCosSeries(1, ssig2, csig2, l->C1a, nC1);
+ serr = (1 + l->A1m1)*(sig12 + (B12 - l->B11)) - s12_a12/l->b;
+ sig12 = sig12 - serr/sqrt(1 + l->k2 * ssig2*ssig2);
+ ssig12 = sin(sig12);
+ csig12 = cos(sig12);
+ /* Update B12 below */
+ }
+ }
+
+ /* sig2 = sig1 + sig12 */
+ ssig2 = l->ssig1*csig12 + l->csig1*ssig12;
+ csig2 = l->csig1*csig12 - l->ssig1*ssig12;
+ dn2 = sqrt(1 + l->k2 * ssig2*ssig2);
+ if(outmask & (GDistance | GReducedLength | GGeodesicScale)){
+ if(flags & GArcMode || fabs(l->f) > 0.01)
+ B12 = SinCosSeries(1, ssig2, csig2, l->C1a, nC1);
+ AB1 = (1 + l->A1m1)*(B12 - l->B11);
+ }
+ /* sin(bet2) = cos(alp0) * sin(sig2) */
+ sbet2 = l->calp0 * ssig2;
+ /* Alt: cbet2 = hypot(csig2, salp0 * ssig2); */
+ cbet2 = hypot(l->salp0, l->calp0 * csig2);
+ if(cbet2 == 0) /* I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case */
+ cbet2 = csig2 = tiny;
+ /* tan(alp0) = cos(sig2)*tan(alp2) */
+ salp2 = l->salp0; calp2 = l->calp0 * csig2; /* No need to normalize */
+
+ if(outmask & GDistance)
+ s12 = (flags & GArcMode) ?
+ l->b*((1 + l->A1m1)*sig12 + AB1) :
+ s12_a12;
+
+ if(outmask & GLongitude){
+ double E;
+
+ E = copysign(1, l->salp0); /* east or west going? */
+ /* tan(omg2) = sin(alp0) * tan(sig2) */
+ somg2 = l->salp0 * ssig2;
+ comg2 = csig2; /* No need to normalize */
+ /* omg12 = omg2 - omg1 */
+ omg12 = (flags & GUnrollLon) ?
+ E*(sig12
+ - (atan2( ssig2, csig2) - atan2( l->ssig1, l->csig1))
+ + (atan2(E * somg2, comg2) - atan2(E * l->somg1, l->comg1)))
+ : atan2(somg2 * l->comg1 - comg2 * l->somg1,
+ comg2 * l->comg1 + somg2 * l->somg1);
+ lam12 = omg12 + l->A3c *
+ (sig12 + (SinCosSeries(1, ssig2, csig2, l->C3a, nC3-1)
+ - l->B31));
+ lon12 = lam12/DEG;
+ lon2 = (flags & GUnrollLon) ? l->lon1 + lon12 :
+ AngNormalize(AngNormalize(l->lon1) + AngNormalize(lon12));
+ }
+
+ if(outmask & GLatitude)
+ lat2 = atan2d(sbet2, l->f1*cbet2);
+
+ if(outmask & GAzimuth)
+ azi2 = atan2d(salp2, calp2);
+
+ if(outmask & (GReducedLength | GGeodesicScale)){
+ double B22, AB2, J12;
+
+ B22 = SinCosSeries(1, ssig2, csig2, l->C2a, nC2);
+ AB2 = (1 + l->A2m1)*(B22 - l->B21);
+ J12 = (l->A1m1 - l->A2m1)*sig12 + (AB1 - AB2);
+ if(outmask & GReducedLength)
+ /* Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
+ * accurate cancellation in the case of coincident points. */
+ m12 = l->b * ((dn2 * (l->csig1 * ssig2) - l->dn1 * (l->ssig1 * csig2))
+ - l->csig1 * csig2 * J12);
+ if(outmask & GGeodesicScale){
+ double t;
+
+ t = l->k2*(ssig2 - l->ssig1)*(ssig2 + l->ssig1)/(l->dn1 + dn2);
+ M12 = csig12 + (t*ssig2 - csig2*J12) * l->ssig1/l->dn1;
+ M21 = csig12 - (t*l->ssig1 - l->csig1*J12) * ssig2/dn2;
+ }
+ }
+
+ if(outmask & GArea){
+ double B42;
+ double salp12, calp12;
+
+ B42 = SinCosSeries(0, ssig2, csig2, l->C4a, nC4);
+ if(l->calp0 == 0 || l->salp0 == 0){
+ /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
+ salp12 = salp2*l->calp1 - calp2*l->salp1;
+ calp12 = calp2*l->calp1 + salp2*l->salp1;
+ }else{
+ /*
+ * tan(alp) = tan(alp0) * sec(sig)
+ * tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
+ * = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
+ * If csig12 > 0, write
+ * csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
+ * else
+ * csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
+ * No need to normalize
+ */
+ salp12 = l->calp0*l->salp0 *
+ (csig12 <= 0 ? l->csig1*(1 - csig12) + ssig12*l->ssig1 :
+ ssig12*(l->csig1*ssig12/(1 + csig12) + l->ssig1));
+ calp12 = l->salp0*l->salp0 + l->calp0*l->calp0 * l->csig1*csig2;
+ }
+ S12 = l->c2*atan2(salp12, calp12) + l->A4*(B42 - l->B41);
+ }
+
+ if((outmask & GLatitude) && plat2)
+ *plat2 = lat2;
+ if((outmask & GLongitude) && plon2)
+ *plon2 = lon2;
+ if((outmask & GAzimuth) && pazi2)
+ *pazi2 = azi2;
+ if((outmask & GDistance) && ps12)
+ *ps12 = s12;
+ if((outmask & GReducedLength) && pm12)
+ *pm12 = m12;
+ if(outmask & GGeodesicScale){
+ if(pM12)
+ *pM12 = M12;
+ if(pM21)
+ *pM21 = M21;
+ }
+ if((outmask & GArea) && pS12)
+ *pS12 = S12;
+
+ return (flags & GArcMode) ? s12_a12 : sig12/DEG;
+}
+
+double
+gendirectgeod(Geodesic *g,
+ double lat1, double lon1, double azi1,
+ uint flags, double s12_a12,
+ double *lat2, double *lon2, double *azi2,
+ double *s12, double *m12, double *M12, double *M21,
+ double *S12)
+{
+ Geodline l;
+ uint outmask;
+
+ outmask = 0;
+ outmask |= lat2 ? GLatitude : GNone;
+ outmask |= lon2 ? GLongitude : GNone;
+ outmask |= azi2 ? GAzimuth : GNone;
+ outmask |= s12 ? GDistance : GNone;
+ outmask |= m12 ? GReducedLength : GNone;
+ outmask |= M12 ? GGeodesicScale : GNone;
+ outmask |= S12 ? GArea : GNone;
+ outmask |= flags&GArcMode ? GNone : GDistanceIn;
+ initgeodline(&l, g, lat1, lon1, azi1, outmask);
+ return geod_genposition(&l, flags, s12_a12, lat2, lon2, azi2, s12, m12, M12, M21, S12);
+}
+
+void
+directgeod(Geodesic *g,
+ double lat1, double lon1, double azi1,
+ double s12,
+ double *lat2, double *lon2, double *azi2)
+{
+ gendirectgeod(g, lat1, lon1, azi1, GNoflags, s12, lat2, lon2, azi2,
+ nil, nil, nil, nil, nil);
+}
+
+static double
+geninversegeod_int(Geodesic *g,
+ double lat1, double lon1, double lat2, double lon2,
+ double *ps12,
+ double *psalp1, double *pcalp1,
+ double *psalp2, double *pcalp2,
+ double *pm12, double *pM12, double *pM21,
+ double *pS12)
+{
+ double s12, m12, M12, M21, S12;
+ double lon12, lon12s;
+ double sbet1, cbet1, sbet2, cbet2, s12x, m12x;
+ double dn1, dn2, lam12, slam12, clam12;
+ double a12, sig12, calp1, salp1, calp2, salp2;
+ double Ca[nC];
+ double omg12, somg12, comg12;
+ int latsign, lonsign, swapp;
+ int meridian;
+ uint outmask;
+
+ s12 = m12 = M12 = M21 = S12 = 0;
+ s12x = m12x = 0;
+ a12 = sig12 = calp1 = salp1 = calp2 = salp2 = 0;
+ /* somg12 > 1 marks that it needs to be calculated */
+ omg12 = comg12 = 0;
+ somg12 = 2;
+
+ outmask = 0;
+ outmask |= ps12 ? GDistance : GNone;
+ outmask |= pm12 ? GReducedLength : GNone;
+ outmask |= pM12 || pM21 ? GGeodesicScale : GNone;
+ outmask |= pS12 ? GArea : GNone;
+
+ outmask &= OUT_ALL;
+ /*
+ * Compute longitude difference (AngDiff does this carefully). Result is
+ * in [-180, 180] but -180 is only for west-going geodesics. 180 is for
+ * east-going and meridional geodesics.
+ */
+ lon12 = AngDiff(lon1, lon2, &lon12s);
+ /* Make longitude difference positive. */
+ lonsign = lon12 >= 0 ? 1 : -1;
+ /* If very close to being on the same half-meridian, then make it so. */
+ lon12 = lonsign * AngRound(lon12);
+ lon12s = AngRound((180 - lon12) - lonsign * lon12s);
+ lam12 = lon12*DEG;
+ if(lon12 > 90){
+ sincosd(lon12s, &slam12, &clam12);
+ clam12 = -clam12;
+ }else
+ sincosd(lon12, &slam12, &clam12);
+
+ /* If really close to the equator, treat as on equator. */
+ lat1 = AngRound(LatFix(lat1));
+ lat2 = AngRound(LatFix(lat2));
+ /*
+ * Swap points so that point with higher (abs) latitude is point 1
+ * If one latitude is a nan, then it becomes lat1.
+ */
+ swapp = fabs(lat1) < fabs(lat2) ? -1 : 1;
+ if(swapp < 0){
+ lonsign *= -1;
+ swap(&lat1, &lat2);
+ }
+ /* Make lat1 <= 0 */
+ latsign = lat1 < 0 ? 1 : -1;
+ lat1 *= latsign;
+ lat2 *= latsign;
+ /* Now we have
+ *
+ * 0 <= lon12 <= 180
+ * -90 <= lat1 <= 0
+ * lat1 <= lat2 <= -lat1
+ *
+ * longsign, swapp, latsign register the transformation to bring the
+ * coordinates to this canonical form. In all cases, 1 means no change was
+ * made. We make these transformations so that there are few cases to
+ * check, e.g., on verifying quadrants in atan2. In addition, this
+ * enforces some symmetries in the results returned. */
+
+ sincosd(lat1, &sbet1, &cbet1);
+ sbet1 *= g->f1;
+ /* Ensure cbet1 = +epsilon at poles */
+ norm2(&sbet1, &cbet1);
+ cbet1 = max(tiny, cbet1);
+
+ sincosd(lat2, &sbet2, &cbet2);
+ sbet2 *= g->f1;
+ /* Ensure cbet2 = +epsilon at poles */
+ norm2(&sbet2, &cbet2);
+ cbet2 = max(tiny, cbet2);
+
+ /*
+ * If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the
+ * |bet1| - |bet2|. Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is
+ * a better measure. This logic is used in assigning calp2 in Lambda12.
+ * Sometimes these quantities vanish and in that case we force bet2 = +/-
+ * bet1 exactly. An example where is is necessary is the inverse problem
+ * 48.522876735459 0 -48.52287673545898293 179.599720456223079643
+ * which failed with Visual Studio 10 (Release and Debug)
+ */
+ if(cbet1 < -sbet1){
+ if(cbet2 == cbet1)
+ sbet2 = sbet2 < 0 ? sbet1 : -sbet1;
+ }else{
+ if (fabs(sbet2) == -sbet1)
+ cbet2 = cbet1;
+ }
+
+ dn1 = sqrt(1 + g->ep2 * sbet1*sbet1);
+ dn2 = sqrt(1 + g->ep2 * sbet2*sbet2);
+
+ meridian = lat1 == -90 || slam12 == 0;
+
+ if(meridian){
+ /*
+ * Endpoints are on a single full meridian, so the geodesic might lie on
+ * a meridian.
+ */
+
+ double ssig1, csig1, ssig2, csig2;
+ /* Head to the target longitude */
+ calp1 = clam12;
+ salp1 = slam12;
+ /* At the target we're heading north */
+ calp2 = 1;
+ salp2 = 0;
+
+ /* tan(bet) = tan(sig) * cos(alp) */
+ ssig1 = sbet1;
+ csig1 = calp1 * cbet1;
+ ssig2 = sbet2;
+ csig2 = calp2 * cbet2;
+
+ /* sig12 = sig2 - sig1 */
+ sig12 = atan2(max(0, csig1 * ssig2 - ssig1 * csig2), csig1*csig2 + ssig1*ssig2);
+ Lengths(g, g->n, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
+ cbet1, cbet2, &s12x, &m12x, nil,
+ (outmask & GGeodesicScale) ? &M12 : nil,
+ (outmask & GGeodesicScale) ? &M21 : nil, Ca);
+ /*
+ * Add the check for sig12 since zero length geodesics might yield m12 <
+ * 0. Test case was
+ *
+ * echo 20.001 0 20.001 0 | GeodSolve -i
+ *
+ * In fact, we will have sig12 > pi/2 for meridional geodesic which is
+ * not a shortest path.
+ */
+ if(sig12 < 1 || m12x >= 0){
+ /* Need at least 2, to handle 90 0 90 180 */
+ if(sig12 < 3*tiny)
+ sig12 = m12x = s12x = 0;
+ m12x *= g->b;
+ s12x *= g->b;
+ a12 = sig12/DEG;
+ }else
+ /* m12 < 0, i.e., prolate and too close to anti-podal */
+ meridian = 0;
+ }
+
+ if(!meridian && sbet1 == 0 && (g->f <= 0 || lon12s >= g->f*180)){
+ /* Geodesic runs along equator */
+ calp1 = calp2 = 0; salp1 = salp2 = 1;
+ s12x = g->a * lam12;
+ sig12 = omg12 = lam12 / g->f1;
+ m12x = g->b*sin(sig12);
+ if (outmask & GGeodesicScale)
+ M12 = M21 = cos(sig12);
+ a12 = lon12 / g->f1;
+ }else if(!meridian){
+ /*
+ * Now point1 and point2 belong within a hemisphere bounded by a
+ * meridian and geodesic is neither meridional or equatorial.
+ */
+ /* Figure a starting point for Newton's method */
+ double dnm;
+
+ dnm = 0;
+ sig12 = InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
+ lam12, slam12, clam12,
+ &salp1, &calp1, &salp2, &calp2, &dnm, Ca);
+
+ if(sig12 >= 0){
+ /* Short lines (InverseStart sets salp2, calp2, dnm) */
+ s12x = sig12 * g->b * dnm;
+ m12x = dnm*dnm * g->b * sin(sig12/dnm);
+ if(outmask & GGeodesicScale)
+ M12 = M21 = cos(sig12/dnm);
+ a12 = sig12/DEG;
+ omg12 = lam12/(g->f1*dnm);
+ }else{
+ /*
+ * Newton's method. This is a straightforward solution of f(alp1) =
+ * lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one
+ * root in the interval (0, pi) and its derivative is positive at the
+ * root. Thus f(alp) is positive for alp > alp1 and negative for alp <
+ * alp1. During the course of the iteration, a range (alp1a, alp1b) is
+ * maintained which brackets the root and with each evaluation of
+ * f(alp) the range is shrunk, if possible. Newton's method is
+ * restarted whenever the derivative of f is negative (because the new
+ * value of alp1 is then further from the solution) or if the new
+ * estimate of alp1 lies outside (0,π); in this case, the new starting
+ * guess is taken to be (alp1a + alp1b)/2.
+ */
+ double ssig1, csig1, ssig2, csig2, eps, domg12;
+ double salp1a, calp1a, salp1b, calp1b;
+ uint numit;
+ int tripn, tripb;
+
+ ssig1 = csig1 = ssig2 = csig2 = eps = domg12 = 0;
+ numit = 0;
+ /* Bracketing range */
+ salp1a = tiny;
+ calp1a = 1;
+ salp1b = tiny;
+ calp1b = -1;
+ tripn = 0;
+ tripb = 0;
+ for(; numit < maxit2; ++numit){
+ /*
+ * the WGS84 test set: mean = 1.47, sd = 1.25, max = 16
+ * WGS84 and random input: mean = 2.85, sd = 0.60
+ */
+ double dv, v;
+
+ dv = 0;
+ v = Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
+ slam12, clam12,
+ &salp2, &calp2, &sig12, &ssig1, &csig1, &ssig2, &csig2,
+ &eps, &domg12, numit < maxit1, &dv, Ca);
+ /* 2 * tol0 is approximately 1 ulp for a number in [0, pi]. */
+ /* Reversed test to allow escape with NaNs */
+ if(tripb || !(fabs(v) >= (tripn ? 8 : 1) * tol0))
+ break;
+ /* Update bracketing values */
+ if(v > 0 && (numit > maxit1 || calp1/salp1 > calp1b/salp1b)){
+ salp1b = salp1;
+ calp1b = calp1;
+ }else if(v < 0 && (numit > maxit1 || calp1/salp1 < calp1a/salp1a)){
+ salp1a = salp1;
+ calp1a = calp1;
+ }
+ if(numit < maxit1 && dv > 0){
+ double dalp1, sdalp1, cdalp1, nsalp1;
+
+ dalp1 = -v/dv;
+ sdalp1 = sin(dalp1);
+ cdalp1 = cos(dalp1);
+ nsalp1 = salp1*cdalp1 + calp1*sdalp1;
+ if(nsalp1 > 0 && fabs(dalp1) < PI){
+ calp1 = calp1 * cdalp1 - salp1 * sdalp1;
+ salp1 = nsalp1;
+ norm2(&salp1, &calp1);
+ /*
+ * In some regimes we don't get quadratic convergence because
+ * slope -> 0. So use convergence conditions based on epsilon
+ * instead of sqrt(epsilon).
+ */
+ tripn = fabs(v) <= 16 * tol0;
+ continue;
+ }
+ }
+ /*
+ * Either dv was not positive or updated value was outside legal
+ * range. Use the midpoint of the bracket as the next estimate.
+ * This mechanism is not needed for the WGS84 ellipsoid, but it does
+ * catch problems with more eccentric ellipsoids. Its efficacy is
+ * such for the WGS84 test set with the starting guess set to alp1 =
+ * 90deg:
+ * the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
+ * WGS84 and random input: mean = 4.74, sd = 0.99
+ */
+ salp1 = (salp1a + salp1b)/2;
+ calp1 = (calp1a + calp1b)/2;
+ norm2(&salp1, &calp1);
+ tripn = 0;
+ tripb = (fabs(salp1a - salp1) + (calp1a - calp1) < tolb ||
+ fabs(salp1 - salp1b) + (calp1 - calp1b) < tolb);
+
+ }
+ Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
+ cbet1, cbet2, &s12x, &m12x, nil,
+ (outmask & GGeodesicScale) ? &M12 : nil,
+ (outmask & GGeodesicScale) ? &M21 : nil, Ca);
+ m12x *= g->b;
+ s12x *= g->b;
+ a12 = sig12/DEG;
+ if(outmask & GArea){
+ double sdomg12, cdomg12;
+
+ /* omg12 = lam12 - domg12 */
+ sdomg12 = sin(domg12);
+ cdomg12 = cos(domg12);
+ somg12 = slam12*cdomg12 - clam12*sdomg12;
+ comg12 = clam12*cdomg12 + slam12*sdomg12;
+ }
+ }
+ }
+
+ if(outmask & GDistance)
+ s12 = 0 + s12x; /* Convert -0 to 0 */
+
+ if(outmask & GReducedLength)
+ m12 = 0 + m12x; /* Convert -0 to 0 */
+
+ if(outmask & GArea){
+ double salp0, calp0, alp12;
+
+ /* From Lambda12: sin(alp1) * cos(bet1) = sin(alp0) */
+ salp0 = salp1 * cbet1;
+ calp0 = hypot(calp1, salp1 * sbet1); /* calp0 > 0 */
+ if(calp0 != 0 && salp0 != 0){
+ double ssig1, csig1, ssig2, csig2, k2, eps, A4;
+ double B41, B42;
+
+ /* From Lambda12: tan(bet) = tan(sig) * cos(alp) */
+ ssig1 = sbet1;
+ csig1 = calp1 * cbet1;
+ ssig2 = sbet2;
+ csig2 = calp2 * cbet2;
+ k2 = calp0*calp0 * g->ep2;
+ eps = k2/(2*(1 + sqrt(1 + k2)) + k2);
+ /* Multiplier = a² * e² * cos(alpha0) * sin(alpha0). */
+ A4 = g->a*g->a * calp0 * salp0 * g->e2;
+ norm2(&ssig1, &csig1);
+ norm2(&ssig2, &csig2);
+ C4f(g, eps, Ca);
+ B41 = SinCosSeries(0, ssig1, csig1, Ca, nC4);
+ B42 = SinCosSeries(0, ssig2, csig2, Ca, nC4);
+ S12 = A4*(B42 - B41);
+ }else
+ /* Avoid problems with indeterminate sig1, sig2 on equator */
+ S12 = 0;
+
+ if(!meridian && somg12 > 1){
+ somg12 = sin(omg12);
+ comg12 = cos(omg12);
+ }
+
+ if(!meridian && comg12 > -0.7071 && /* Lon difference not too big */
+ sbet2 - sbet1 < 1.75){ /* Lat difference not too big */
+ /*
+ * Use tan(Gamma/2) = tan(omg12/2) *
+ * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
+ * with tan(x/2) = sin(x)/(1+cos(x))
+ */
+ double domg12, dbet1, dbet2;
+
+ domg12 = 1 + comg12;
+ dbet1 = 1 + cbet1;
+ dbet2 = 1 + cbet2;
+ alp12 = 2*atan2(somg12 * (sbet1 * dbet2 + sbet2 * dbet1),
+ domg12*(sbet1 * sbet2 + dbet1 * dbet2));
+ }else{
+ /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
+ double salp12, calp12;
+
+ salp12 = salp2*calp1 - calp2*salp1;
+ calp12 = calp2*calp1 + salp2*salp1;
+ /*
+ * The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
+ * salp12 = -0 and alp12 = -180. However this depends on the sign
+ * being attached to 0 correctly. The following ensures the correct
+ * behavior.
+ */
+ if(salp12 == 0 && calp12 < 0){
+ salp12 = tiny * calp1;
+ calp12 = -1;
+ }
+ alp12 = atan2(salp12, calp12);
+ }
+ S12 += g->c2 * alp12;
+ S12 *= swapp * lonsign*latsign;
+ /* Convert -0 to 0 */
+ S12 += 0;
+ }
+
+ /* Convert calp, salp to azimuth accounting for lonsign, swapp, latsign. */
+ if(swapp < 0){
+ swap(&salp1, &salp2);
+ swap(&calp1, &calp2);
+ if(outmask & GGeodesicScale)
+ swap(&M12, &M21);
+ }
+
+ salp1 *= swapp*lonsign;
+ calp1 *= swapp*latsign;
+ salp2 *= swapp*lonsign;
+ calp2 *= swapp*latsign;
+
+ if(psalp1)
+ *psalp1 = salp1;
+ if(pcalp1)
+ *pcalp1 = calp1;
+ if(psalp2)
+ *psalp2 = salp2;
+ if(pcalp2)
+ *pcalp2 = calp2;
+
+ if (outmask & GDistance)
+ *ps12 = s12;
+ if (outmask & GReducedLength)
+ *pm12 = m12;
+ if(outmask & GGeodesicScale){
+ if(pM12)
+ *pM12 = M12;
+ if(pM21)
+ *pM21 = M21;
+ }
+ if(outmask & GArea)
+ *pS12 = S12;
+ /* Returned value in [0, 180] */
+ return a12;
+}
+
+
+double
+geninversegeod(Geodesic *g,
+ double lat1, double lon1, double lat2, double lon2,
+ double *ps12, double *pazi1, double *pazi2,
+ double *pm12, double *pM12, double *pM21, double *pS12)
+{
+ double a12, salp1, calp1, salp2, calp2;
+
+ a12 = geninversegeod_int(g, lat1, lon1, lat2, lon2, ps12,
+ &salp1, &calp1, &salp2, &calp2,
+ pm12, pM12, pM21, pS12);
+ if(pazi1)
+ *pazi1 = atan2d(salp1, calp1);
+ if(pazi2)
+ *pazi2 = atan2d(salp2, calp2);
+ return a12;
+}
+
+void
+inversegeod(Geodesic *g,
+ double lat1, double lon1, double lat2, double lon2,
+ double *s12,
+ double *azi1, double *azi2)
+{
+ geninversegeod(g, lat1, lon1, lat2, lon2, s12, azi1, azi2, nil, nil, nil, nil);
+}
+
+void
+initgeod(Geodesic *g, double a, double f)
+{
+ init();
+ g->a = a;
+ g->f = f;
+ g->f1 = 1 - f;
+ g->e2 = f*(2 - f);
+ g->ep2 = g->e2 / g->f1*g->f1; /* e2/(1 - e2) */
+ g->n = f/(2 - f);
+ g->b = a*g->f1;
+ g->c2 = (a*a + g->b*g->b * (g->e2 == 0 ? 1 : (g->e2 > 0 ? atanh(sqrt(g->e2)) : atan(sqrt(-g->e2))) / sqrt(fabs(g->e2))))/2; /* authalic radius squared */
+ /*
+ * The sig12 threshold for "really short". Using the auxiliary sphere
+ * solution with dnm computed at (bet1 + bet2) / 2, the relative error in the
+ * azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2. (Error
+ * measured for 1/100 < b/a < 100 and abs(f) >= 1/1000. For a given f and
+ * sig12, the max error occurs for lines near the pole. If the old rule for
+ * computing dnm = (dn1 + dn2)/2 is used, then the error increases by a
+ * factor of 2.) Setting this equal to epsilon gives sig12 = etol2. Here
+ * 0.1 is a safety factor (error decreased by 100) and max(0.001, abs(f))
+ * stops etol2 getting too large in the nearly spherical case.
+ */
+ g->etol2 = 0.1*tol2 / sqrt(max(0.001, fabs(f))*min(1, 1 - f/2)/2);
+ A3coeff(g);
+ C3coeff(g);
+ C4coeff(g);
+}
diff --git a/geodesic.h b/geodesic.h
new file mode 100644
index 0000000..85dacab
--- /dev/null
+++ b/geodesic.h
@@ -0,0 +1,111 @@
+/*
+ * This an implementation in C of the geodesic algorithms described in
+ * C. F. F. Karney, Algorithms for geodesics, J. Geodesy 87, pp. 43-55 (2013).
+ *
+ * The principal advantages of these algorithms over previous ones (e.g.,
+ * Vincenty, 1975) are
+ * - accurate to round off for |f| < 1/50
+ * - the solution of the inverse problem is always found
+ * - differential and integral properties of geodesics are computed
+ *
+ * The shortest path between two points on the ellipsoid at lat1, lon1
+ * and lat2, lon2 is called the geodesic. Its length is
+ * s12 and the geodesic from point 1 to point 2 has forward azimuths
+ * azi1 and azi2 at the two end points.
+ *
+ * Ported from PROJ v6.2.1 to Plan 9 on 22dec2019.
+ */
+#define DEG 0.0174532925199
+
+enum {
+ GeodesicOrder = 6,
+ nA1 = GeodesicOrder,
+ nC1 = GeodesicOrder,
+ nC1p = GeodesicOrder,
+ nA2 = GeodesicOrder,
+ nC2 = GeodesicOrder,
+ nA3 = GeodesicOrder,
+ nA3x = nA3,
+ nC3 = GeodesicOrder,
+ nC3x = nC3*(nC3 - 1) / 2,
+ nC4 = GeodesicOrder,
+ nC4x = nC4*(nC4 + 1) / 2,
+ nC = GeodesicOrder+1,
+};
+
+enum {
+ GNone = 0,
+ GLatitude = 1<<0,
+ GLongitude = 1<<1,
+ GAzimuth = 1<<2,
+ GDistance = 1<<3,
+ GDistanceIn = 1<<4, /* Allow distance as input */
+ GReducedLength = 1<<5,
+ GGeodesicScale = 1<<6,
+ GArea = 1<<7,
+ GAll = 0xFF,
+
+ GNoflags = 0,
+ GArcMode = 1<<0,
+ GUnrollLon = 1<<1
+};
+
+enum {
+ CapNone = 0,
+ CapC1 = 1<<0,
+ CapC1p = 1<<1,
+ CapC2 = 1<<2,
+ CapC3 = 1<<3,
+ CapC4 = 1<<4,
+ CapAll = 0x1F,
+ OUT_ALL = 0xFF
+};
+
+typedef struct Geodesic Geodesic;
+typedef struct Geodline Geodline;
+
+struct Geodesic
+{
+ double a; /* the equatorial radius */
+ double f; /* the flattening */
+ double f1;
+ double e2, ep2;
+ double n, b, c2, etol2;
+ double A3x[6];
+ double C3x[15];
+ double C4x[21];
+};
+
+struct Geodline
+{
+ double lat1; /* the starting latitude */
+ double lon1; /* the starting longitude */
+ double azi1; /* the starting azimuth */
+ double a; /* the equatorial radius */
+ double f; /* the flattening */
+ double salp1; /* sine of azi1 */
+ double calp1; /* cosine of azi1 */
+ double a13; /* arc length to reference point */
+ double s13; /* distance to reference point */
+ double b, c2, f1, salp0, calp0, k2,
+ ssig1, csig1, dn1, stau1, ctau1, somg1, comg1,
+ A1m1, A2m1, A3c, B11, B21, B31, A4, B41;
+ double C1a[6+1];
+ double C1pa[6+1];
+ double C2a[6+1];
+ double C3a[6];
+ double C4a[6];
+ uint caps; /* the capabilities */
+};
+
+
+void initgeod(Geodesic*, double, double);
+void directgeod(Geodesic*, double, double, double, double, double*, double*, double*);
+double gendirectgeod(Geodesic*, double, double, double, uint, double, double*,
+ double*, double*, double*, double*, double*, double*, double*);
+void inversegeod(Geodesic*, double, double, double, double, double*, double*, double*);
+double geninversegeod(Geodesic*, double, double, double, double, double*,
+ double*, double*, double*, double*, double*, double*);
+void initgeodline(Geodline*, Geodesic*, double, double, double, uint);
+double geod_genposition(Geodline*, uint, double, double*, double*, double*,
+ double*, double*, double*, double*, double*);
diff --git a/main.c b/main.c
new file mode 100644
index 0000000..487171b
--- /dev/null
+++ b/main.c
@@ -0,0 +1,41 @@
+#include <u.h>
+#include <libc.h>
+#include "geodesic.h"
+
+typedef struct Geomodel Geomodel;
+
+struct Geomodel
+{
+ double a; /* equatorial radius */
+ double f; /* flattening */
+};
+
+Geomodel WGS84 = { 6378137, 1/298.257223563 };
+
+void
+usage(void)
+{
+ fprint(2, "usage: %s lat0 lon0 lat1 lon1\n", argv0);
+ exits("usage");
+}
+
+void
+main(int argc, char *argv[])
+{
+ Geodesic g;
+ double lat0, lon0, lat1, lon1;
+ double s12;
+
+ ARGBEGIN{
+ default: usage();
+ }ARGEND;
+ if(argc != 4)
+ usage();
+ lat0 = strtod(argv[0], nil);
+ lon0 = strtod(argv[1], nil);
+ lat1 = strtod(argv[2], nil);
+ lon1 = strtod(argv[3], nil);
+ initgeod(&g, WGS84.a, WGS84.f);
+ inversegeod(&g, lat0, lon0, lat1, lon1, &s12, nil, nil);
+ print("%gm\n", s12);
+}
diff --git a/mkfile b/mkfile
new file mode 100644
index 0000000..298eaa4
--- /dev/null
+++ b/mkfile
@@ -0,0 +1,11 @@
+</$objtype/mkfile
+
+BIN=/$objtype/bin
+TARG=geod
+OFILES=\
+ main.$O\
+ geodesic.$O\
+
+HFILES=geodesic.h\
+
+</sys/src/cmd/mkone