private static CrsdistResult Crsdist(double lat1, double lon1, double lat2, double lon2)
{ // radian args
/* compute course and Distance (spherical) */
double crs12, crs21;
double argacos;
if ((lat1 + lat2 == 0.0) && (MathEx.Abs(lon1 - lon2) == MathEx.PI) && (MathEx.Abs(lat1) != (MathEx.PI / 180) * 90.0))
{
throw new ArgumentException("Course between antipodal points is undefined");
}
double d = MathEx.Acos(MathEx.Sin(lat1) * MathEx.Sin(lat2) + MathEx.Cos(lat1) * MathEx.Cos(lat2) * MathEx.Cos(lon1 - lon2));
if ((d == 0.0) || (lat1 == -(MathEx.PI / 180) * 90.0))
{
crs12 = 2 * MathEx.PI;
}
else if (lat1 == (MathEx.PI / 180) * 90.0)
{
crs12 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat2) - MathEx.Sin(lat1) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat1));
if (MathEx.Sin(lon2 - lon1) < 0)
{
crs12 = Acosf(argacos);
}
else
{
crs12 = 2 * MathEx.PI - Acosf(argacos);
}
}
if ((d == 0.0) || (lat2 == -(MathEx.PI / 180) * 90.0))
{
crs21 = 0.0;
}
else if (lat2 == (MathEx.PI / 180) * 90.0)
{
crs21 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat1) - MathEx.Sin(lat2) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat2));
if (MathEx.Sin(lon1 - lon2) < 0)
{
crs21 = Acosf(argacos);
}
else
{
crs21 = 2 * MathEx.PI - Acosf(argacos);
}
}
CrsdistResult outValue = new CrsdistResult {
_d = d, _crs12 = crs12, _crs21 = crs21
};
return(outValue);
}
private CrsdistResult ComputeFormCD(GeoLatLng pt1, GeoLatLng pt2)
{
double d, crs12, crs21;
CrsdistResult outValue = new CrsdistResult();
double lat1 = pt1.LatRadians();
double lat2 = pt2.LatRadians();
double lon1 = -pt1.LngRadians();
double lon2 = -pt2.LngRadians();
double dc = _currentUnit == UNIT_NM ? 1 : 1.852;
if (_currentEarthMode == EARTH_MODEL_SPHERE)
{
// spherical code// compute crs and Distance
CrsdistResult cd = Crsdist(lat1, lon1, lat2, lon2);
crs12 = cd._crs12 * (180 / MathEx.PI);
crs21 = cd._crs21 * (180 / MathEx.PI);
d = cd._d * (180 / MathEx.PI) * 60 * dc; // go to physical units
}
else
{
// elliptic code // ellipse uses East negative
CrsdistResult cde = CrsdistEll(lat1, -lon1, lat2, -lon2, _currentEarthMode);
crs12 = cde._crs12 * (180 / MathEx.PI);
crs21 = cde._crs21 * (180 / MathEx.PI);
d = cde._d * dc; // go to physical units
}
outValue._crs12 = crs12;
outValue._crs21 = crs21;
outValue._d = d;
return(outValue);
}
////////////////////////////////////////////////////////////////////////////
//--------------------------------- REVISIONS ------------------------------
// Date Name Tracking # Description
// --------- ------------------- ------------- ----------------------
// 18JUN2009 James Shen Initial Creation
////////////////////////////////////////////////////////////////////////////
/**
* Calculate the Distance between two point in great circle.
* @param pt1 the first location.
* @param pt2 the second location.
* @return the Distance and course between the 2 point.
*/
internal GeoLatLng CalculateDistanceAndCourse(GeoLatLng pt1, GeoLatLng pt2)
{
CrsdistResult ret = ComputeFormCD(pt1, pt2);
GeoLatLng ret1 = new GeoLatLng(ret._crs12, ret._d);
return(ret1);
}
private CrsdistResult ComputeFormCD(GeoLatLng pt1, GeoLatLng pt2)
{
double d, crs12, crs21;
CrsdistResult outValue = new CrsdistResult();
double lat1 = pt1.LatRadians();
double lat2 = pt2.LatRadians();
double lon1 = -pt1.LngRadians();
double lon2 = -pt2.LngRadians();
double dc = _currentUnit == UNIT_NM ? 1 : 1.852;
if (_currentEarthMode == EARTH_MODEL_SPHERE)
{
// spherical code// compute crs and Distance
CrsdistResult cd = Crsdist(lat1, lon1, lat2, lon2);
crs12 = cd._crs12 * (180 / MathEx.PI);
crs21 = cd._crs21 * (180 / MathEx.PI);
d = cd._d * (180 / MathEx.PI) * 60 * dc; // go to physical units
}
else
{
// elliptic code // ellipse uses East negative
CrsdistResult cde = CrsdistEll(lat1, -lon1, lat2, -lon2, _currentEarthMode);
crs12 = cde._crs12 * (180 / MathEx.PI);
crs21 = cde._crs21 * (180 / MathEx.PI);
d = cde._d * dc; // go to physical units
}
outValue._crs12 = crs12;
outValue._crs21 = crs21;
outValue._d = d;
return outValue;
}
private static CrsdistResult CrsdistEll(double glat1, double glon1, double glat2, double glon2, int ellipse)
{
// glat1 initial geodetic latitude in radians N positive
// glon1 initial geodetic longitude in radians E positive
// glat2 final geodetic latitude in radians N positive
// glon2 final geodetic longitude in radians E positive
double a = EarthModel[ellipse][0];
double f = 1 / EarthModel[ellipse][1];
//alert("a="+a+" f="+f)
double sx = 0, cx = 0, sy = 0, cy = 0, y = 0;
double c2A = 0, cz = 0, e = 0, c;
const double eps = 0.00000000005;
double iter = 1;
const double maxiter = 100;
CrsdistResult outValue = new CrsdistResult();
if ((glat1 + glat2 == 0.0) && (MathEx.Abs(glon1 - glon2) == MathEx.PI))
{
glat1 = glat1 + 0.00001; // allow algorithm to complete
}
if (glat1 == glat2 && (glon1 == glon2 || MathEx.Abs(MathEx.Abs(glon1 - glon2) - 2 * MathEx.PI) < eps))
{
outValue._d = 0;
outValue._crs12 = 0;
outValue._crs21 = MathEx.PI;
return outValue;
}
double r = 1 - f;
double tu1 = r * MathEx.Tan(glat1);
double tu2 = r * MathEx.Tan(glat2);
double cu1 = 1.0 / MathEx.Sqrt(1.0 + tu1 * tu1);
double su1 = cu1 * tu1;
double cu2 = 1.0 / MathEx.Sqrt(1.0 + tu2 * tu2);
double s1 = cu1 * cu2;
double b1 = s1 * tu2;
double f1 = b1 * tu1;
double x = glon2 - glon1;
double d = x + 1;
while ((MathEx.Abs(d - x) > eps) && (iter < maxiter))
{
iter = iter + 1;
sx = MathEx.Sin(x);
cx = MathEx.Cos(x);
tu1 = cu2 * sx;
tu2 = b1 - su1 * cu2 * cx;
sy = MathEx.Sqrt(tu1 * tu1 + tu2 * tu2);
cy = s1 * cx + f1;
y = Atan2(sy, cy);
double 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;
}
double faz = Modcrs(Atan2(tu1, tu2));
double baz = Modcrs(Atan2(cu1 * sx, b1 * cx - su1 * cu2) + MathEx.PI);
x = MathEx.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;
double 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;
outValue._d = s;
outValue._crs12 = faz;
outValue._crs21 = baz;
if (MathEx.Abs(iter - maxiter) < eps)
{
throw new ArgumentException("Algorithm did not converge");
}
return outValue;
}
private static CrsdistResult Crsdist(double lat1, double lon1, double lat2, double lon2)
{
// radian args
/* compute course and Distance (spherical) */
double crs12, crs21;
double argacos;
if ((lat1 + lat2 == 0.0) && (MathEx.Abs(lon1 - lon2) == MathEx.PI) && (MathEx.Abs(lat1) != (MathEx.PI / 180) * 90.0))
{
throw new ArgumentException("Course between antipodal points is undefined");
}
double d = MathEx.Acos(MathEx.Sin(lat1) * MathEx.Sin(lat2) + MathEx.Cos(lat1) * MathEx.Cos(lat2) * MathEx.Cos(lon1 - lon2));
if ((d == 0.0) || (lat1 == -(MathEx.PI / 180) * 90.0))
{
crs12 = 2 * MathEx.PI;
}
else if (lat1 == (MathEx.PI / 180) * 90.0)
{
crs12 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat2) - MathEx.Sin(lat1) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat1));
if (MathEx.Sin(lon2 - lon1) < 0)
{
crs12 = Acosf(argacos);
}
else
{
crs12 = 2 * MathEx.PI - Acosf(argacos);
}
}
if ((d == 0.0) || (lat2 == -(MathEx.PI / 180) * 90.0))
{
crs21 = 0.0;
}
else if (lat2 == (MathEx.PI / 180) * 90.0)
{
crs21 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat1) - MathEx.Sin(lat2) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat2));
if (MathEx.Sin(lon1 - lon2) < 0)
{
crs21 = Acosf(argacos);
}
else
{
crs21 = 2 * MathEx.PI - Acosf(argacos);
}
}
CrsdistResult outValue = new CrsdistResult {_d = d, _crs12 = crs12, _crs21 = crs21};
return outValue;
}
private static CrsdistResult CrsdistEll(double glat1, double glon1, double glat2, double glon2, int ellipse)
{
// glat1 initial geodetic latitude in radians N positive
// glon1 initial geodetic longitude in radians E positive
// glat2 final geodetic latitude in radians N positive
// glon2 final geodetic longitude in radians E positive
double a = EarthModel[ellipse][0];
double f = 1 / EarthModel[ellipse][1];
//alert("a="+a+" f="+f)
double sx = 0, cx = 0, sy = 0, cy = 0, y = 0;
double c2A = 0, cz = 0, e = 0, c;
const double eps = 0.00000000005;
double iter = 1;
const double maxiter = 100;
CrsdistResult outValue = new CrsdistResult();
if ((glat1 + glat2 == 0.0) && (MathEx.Abs(glon1 - glon2) == MathEx.PI))
{
glat1 = glat1 + 0.00001; // allow algorithm to complete
}
if (glat1 == glat2 && (glon1 == glon2 || MathEx.Abs(MathEx.Abs(glon1 - glon2) - 2 * MathEx.PI) < eps))
{
outValue._d = 0;
outValue._crs12 = 0;
outValue._crs21 = MathEx.PI;
return(outValue);
}
double r = 1 - f;
double tu1 = r * MathEx.Tan(glat1);
double tu2 = r * MathEx.Tan(glat2);
double cu1 = 1.0 / MathEx.Sqrt(1.0 + tu1 * tu1);
double su1 = cu1 * tu1;
double cu2 = 1.0 / MathEx.Sqrt(1.0 + tu2 * tu2);
double s1 = cu1 * cu2;
double b1 = s1 * tu2;
double f1 = b1 * tu1;
double x = glon2 - glon1;
double d = x + 1;
while ((MathEx.Abs(d - x) > eps) && (iter < maxiter))
{
iter = iter + 1;
sx = MathEx.Sin(x);
cx = MathEx.Cos(x);
tu1 = cu2 * sx;
tu2 = b1 - su1 * cu2 * cx;
sy = MathEx.Sqrt(tu1 * tu1 + tu2 * tu2);
cy = s1 * cx + f1;
y = Atan2(sy, cy);
double 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;
}
double faz = Modcrs(Atan2(tu1, tu2));
double baz = Modcrs(Atan2(cu1 * sx, b1 * cx - su1 * cu2) + MathEx.PI);
x = MathEx.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;
double 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;
outValue._d = s;
outValue._crs12 = faz;
outValue._crs21 = baz;
if (MathEx.Abs(iter - maxiter) < eps)
{
throw new ArgumentException("Algorithm did not converge");
}
return(outValue);
}
////////////////////////////////////////////////////////////////////////////
//--------------------------------- REVISIONS ------------------------------
// Date Name Tracking # Description
// --------- ------------------- ------------- ----------------------
// 18JUN2009 James Shen Initial Creation
////////////////////////////////////////////////////////////////////////////
/**
* Calculate the Distance between two point in great circle.
* @param pt1 the first location.
* @param pt2 the second location.
* @return the Distance between the two points.
*/
public double CalculateDistance(GeoLatLng pt1, GeoLatLng pt2)
{
CrsdistResult ret = ComputeFormCD(pt1, pt2);
return(ret._d);
}
