public static double[] Dist_Ell(double glat1, double glon1, double glat2, double glon2, double[] ellipse)
{
double a = ellipse[0]; //Equitorial Radius
double f = 1 / ellipse[1]; //Flattening
double r, tu1, tu2, cu1, su1, cu2, s1, b1, f1;
double x = 0, sx = 0, cx = 0, sy = 0, cy = 0, y = 0, sa = 0, c2a = 0, cz = 0, e = 0, c = 0, d = 0;
double EPS = 0.00000000005;
double faz, baz, s;
double iter = 1;
double MAXITER = 100;
if ((glat1 + glat2 == 0.0) && (Math.Abs(glon1 - glon2) == Math.PI))
{
Debug.WriteLine("Warning: Course and distance between antipodal points is undefined");
glat1 = glat1 + 0.00001; // allow algorithm to complete
}
if (glat1 == glat2 && (glon1 == glon2 || Math.Abs(Math.Abs(glon1 - glon2) - 2 * Math.PI) < EPS))
{
Debug.WriteLine("Warning: Points 1 and 2 are identical- course undefined");
//D
//crs12
//crs21
return(new double[] { 0, 0, Math.PI });
}
r = 1 - f;
tu1 = r * Math.Tan(glat1);
tu2 = r * Math.Tan(glat2);
cu1 = 1.0 / Math.Sqrt(1.0 + tu1 * tu1);
su1 = cu1 * tu1;
cu2 = 1.0 / Math.Sqrt(1.0 + tu2 * tu2);
s1 = cu1 * cu2;
b1 = s1 * tu2;
f1 = b1 * tu1;
x = glon2 - glon1;
d = x + 1; // force one pass
while ((Math.Abs(d - x) > EPS) && (iter < MAXITER))
{
iter = iter + 1;
sx = Math.Sin(x);
cx = Math.Cos(x);
tu1 = cu2 * sx;
tu2 = b1 - su1 * cu2 * cx;
sy = Math.Sqrt(tu1 * tu1 + tu2 * tu2);
cy = s1 * cx + f1;
y = Math.Atan2(sy, cy);
sa = s1 * sx / sy;
c2a = 1 - sa * sa;
cz = f1 + f1;
if (c2a > 0.0)
{
cz = cy - cz / c2a;
}
e = cz * cz * 2.0 - 1.0;
c = ((-3.0 * c2a + 4.0) * f + 4.0) * c2a * f / 16.0;
d = x;
x = ((e * cy * c + cz) * sy * c + y) * sa;
x = (1.0 - c) * x * f + glon2 - glon1;
}
faz = ModM.ModCrs(Math.Atan2(tu1, tu2));
baz = ModM.ModCrs(Math.Atan2(cu1 * sx, b1 * cx - su1 * cu2) + Math.PI);
x = Math.Sqrt((1 / (r * r) - 1) * c2a + 1);
x += 1;
x = (x - 2.0) / x;
c = 1.0 - x;
c = (x * x / 4.0 + 1.0) / c;
d = (0.375 * x * x - 1.0) * x;
x = e * cy;
s = ((((sy * sy * 4.0 - 3.0) * (1.0 - e - e) * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * a * r;
if (Math.Abs(iter - MAXITER) < EPS)
{
Debug.WriteLine("Warning: Distance algorithm did not converge");
}
return(new double[] { s, faz, baz });
}
