private double GetMetricDistance(Position a, Position b, Position p)
{
Position ab = b - a;
Position ap = p - a;
double abLength = ab.SquaredDistanceFromOrigin();
Position closest;
// Handle a zero length segment; where a and b coincide:
if (abLength == 0.0)
{
closest = a;
}
else
{
double projAbAp = Position.DotProduct(ab, ap) / abLength;
if (projAbAp <= 0.0)
{
closest = a;
}
else if (projAbAp >= 1.0)
{
closest = b;
}
else
{
closest = new Position(a.X + ab.X * projAbAp, a.Y + ab.Y * projAbAp);
}
}
return GetMetricDistance(closest, p);
}
public void TestDistance()
{
Assert.AreEqual(1 / 298.257223563, Ellipsoid.WGS84.F, 0.0000001);
Position a = new Position(150, -30);
Position b = new Position(150, -31);
Position c = new Position(151, -31);
Position d = new Position(151, -32);
Position e = new Position(151, -33);
Position f = new Position(151, -34);
Position g = new Position(151, -35);
Position h = new Position(151, -40);
Position i = new Position(151, -50);
Position j = new Position(151, -60);
Position k = new Position(151, -70);
Position l = new Position(151, -80);
Position lh = new Position(-90, 0);
Position zero = new Position(0, 0);
Position rh = new Position(90, 0);
Assert.AreEqual(Ellipsoid.WGS84.A * Math.PI / 2,
Position.CalculateDistance(zero, rh, Ellipsoid.WGS84), 1);
Assert.AreEqual(Double.NaN,
Position.CalculateDistance(lh, rh, Ellipsoid.WGS84));
Assert.AreEqual(0.0, Position.CalculateDistance(a, a, Ellipsoid.WGS84), 1);
Assert.AreEqual(110861, Position.CalculateDistance(a, b, Ellipsoid.WGS84), 1);
Assert.AreEqual(146647, Position.CalculateDistance(a, c, Ellipsoid.WGS84), 1);
Assert.AreEqual(241428, Position.CalculateDistance(a, d, Ellipsoid.WGS84), 1);
Assert.AreEqual(345929, Position.CalculateDistance(a, e, Ellipsoid.WGS84), 1);
Assert.AreEqual(453493, Position.CalculateDistance(a, f, Ellipsoid.WGS84), 1);
Assert.AreEqual(562376, Position.CalculateDistance(a, g, Ellipsoid.WGS84), 1);
Assert.AreEqual(1113142, Position.CalculateDistance(a, h, Ellipsoid.WGS84), 1);
Assert.AreEqual(2222323, Position.CalculateDistance(a, i, Ellipsoid.WGS84), 1);
Assert.AreEqual(3334804, Position.CalculateDistance(a, j, Ellipsoid.WGS84), 1);
Assert.AreEqual(4449317, Position.CalculateDistance(a, k, Ellipsoid.WGS84), 1);
Assert.AreEqual(5565218, Position.CalculateDistance(a, l, Ellipsoid.WGS84), 1);
Assert.AreEqual(5565218, Position.CalculateDistance(l, a, Ellipsoid.WGS84), 1);
Assert.AreEqual(110861, Position.CalculateDistance(b, a, Ellipsoid.WGS84), 1);
}
public Position GeoToGrid(Position geo)
{
double m = MeridianDistance(geo.Latitude);
double rc, v, p;
RadiusOfCurvature(geo.Latitude, out rc, out v, out p);
double t = Math.Tan(geo.Latitude);
double t_2 = Math.Pow(t, 2);
double t_4 = Math.Pow(t, 4);
double w = geo.Longitude - lon_zero;
double w_2 = Math.Pow(w, 2);
double w_4 = Math.Pow(w, 4);
double w_6 = Math.Pow(w, 6);
double cos_lat = Math.Cos(geo.Latitude);
double sin_lat = Math.Sin(geo.Latitude);
double rc_2 = Math.Pow(rc, 2);
double t_6 = Math.Pow(t, 2);
double cos_lat_3 = Math.Pow(cos_lat, 3);
double term1 = (w_2 / 6.0) * Math.Pow(cos_lat, 2) * (rc - t_2);
double term2 = (w_4 / 120.0) * Math.Pow(cos_lat, 4) * (4.0 * Math.Pow(rc, 3) * (1.0 - 6.0 * t_2) +
rc_2 * (1.0 + 8.0 * t_2) - rc * 2 * t_2 + t_4);
double term3 = (w_6 / 5040.0) * Math.Pow(cos_lat, 6) * (61 - 479 * t_2 + 179 * t_4 - t_6);
double eastings = (projection.k0 * v * w * cos_lat) * (1.0 + term1 + term2 + term3) + projection.false_easting;
term1 = (w_2 / 2.0) * v * sin_lat * cos_lat;
term2 = (w_4 / 24.0) * v * sin_lat * cos_lat_3 * (4.0 * rc_2 + rc - t_2);
term3 = (w_6 / 720.0) * v * sin_lat * Math.Pow(cos_lat, 5) * (8 * Math.Pow(rc, 4) *
(11.0 - 24.0 * t_2) - 28.0 * Math.Pow(rc, 3) * (1.0 - 6.0 * t_2) + rc_2 * (1.0 - 32.0 *
t_2) - rc * (2 * t_2) + t_4);
double term4 = (Math.Pow(w, 8) / 40320.0) * v * sin_lat * Math.Pow(cos_lat, 7) * (1385.0 - 3111.0 *
t_2 + 543.0 * t_4 - Math.Pow(t, 6));
double northings = projection.k0 * (m + term1 + term2 + term3 + term4) + projection.false_northing;
return new Position(eastings, northings);
}
private static double GetSegmentSquaredDistance(Position a, Position b, Position p)
{
Position ab = b - a;
Position ap = p - a;
double abLength = ab.SquaredDistanceFromOrigin();
// Handle a zero length segment; where a and b coincide:
if (abLength == 0.0) {
return Position.SquaredDistance(a, p);
}
double projAbAp = Position.DotProduct(ab, ap) / abLength;
if (projAbAp <= 0.0) {
return Position.SquaredDistance(a, p);
}
if (projAbAp >= 1.0) {
return Position.SquaredDistance(b, p);
}
Position closest = new Position(a.X + ab.X * projAbAp, a.Y + ab.Y * projAbAp);
return Position.SquaredDistance(closest, p);
}
public Position GetPointFromTargetAndDistance(Position target, double distance)
{
Position delta = target - this;
double delta_length = Math.Sqrt(delta.SquaredDistanceFromOrigin());
double udx = delta.X / delta_length;
double udy = delta.Y / delta_length;
return new Position(this.X + udx * distance, this.Y + udy * distance);
}
public Position Transform(Position p)
{
return new Position(_matrix[0] * p.X + _matrix[2] * p.Y + _matrix[4],
_matrix[1] * p.X + _matrix[3] * p.Y + _matrix[5]);
}
public static bool PointsEqual(Position a, Position b, double tolerance)
{
return Math.Abs(a.Latitude - b.Latitude) <= tolerance &&
Math.Abs(a.Longitude - b.Longitude) <= tolerance;
}
public static double SquaredDistance(Position lhs, Position rhs)
{
return (lhs.X - rhs.X) * (lhs.X - rhs.X) +
(lhs.Y - rhs.Y) * (lhs.Y - rhs.Y);
}
public static double CartesianDistance(Position a, Position b)
{
return Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y));
}
public static double DotProduct(Position lhs, Position rhs)
{
return lhs.X * rhs.X + lhs.Y * rhs.Y;
}
public bool Contains(Position position)
{
return GetWindingNumber(position) != 0;
}
public static void CalculateDistanceAndBearing(Position a, Position b, Ellipsoid e, out double distance, out Angle bearing)
{
if (PointsEqual(a, b, 1e-12))
{
distance = 0.0;
bearing = Angle.FromAngleInDegrees(0.0);
}
double lambda = Radians(b.Longitude - a.Longitude);
double last_lambda = 2 * Math.PI;
double U1 = Math.Atan((1 - e.F) * Math.Tan(Radians(a.Latitude)));
double U2 = Math.Atan((1 - e.F) * Math.Tan(Radians(b.Latitude)));
double sin_U1 = Math.Sin(U1);
double sin_U2 = Math.Sin(U2);
double cos_U1 = Math.Cos(U1);
double cos_U2 = Math.Cos(U2);
double alpha, sin_sigma, cos_sigma, sigma, cos_2sigma_m, sqr_cos_2sigma_m;
int loop_limit = 30;
const double threshold = 1e-12;
do {
double sin_lambda = Math.Sin(lambda);
double cos_lambda = Math.Cos(lambda);
sin_sigma =
Math.Sqrt(
Math.Pow(cos_U2 * sin_lambda, 2) +
Math.Pow(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda, 2)
);
cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
sigma = Math.Atan2(sin_sigma, cos_sigma);
alpha = Math.Asin(cos_U1 * cos_U2 * sin_lambda / sin_sigma);
double sqr_cos_alpha = Math.Pow(Math.Cos(alpha), 2);
cos_2sigma_m = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
sqr_cos_2sigma_m = Math.Pow(cos_2sigma_m, 2);
double C = e.F / 16 * sqr_cos_alpha * (4 + e.F * (4 - 3 * sqr_cos_alpha));
last_lambda = lambda;
lambda =
Radians(b.Longitude - a.Longitude) + (1 - C) * e.F * Math.Sin(alpha) *
(sigma + C * sin_sigma *
(cos_2sigma_m + C * cos_sigma * (-1 + 2 * sqr_cos_2sigma_m))
);
loop_limit -= 1;
} while (Math.Abs(lambda - last_lambda) > threshold && loop_limit > 0);
// As in Vincenty 1975, "The inverse formula may give no
// solution over a line between two nearly antipodal points":
if (loop_limit == 0)
{
distance = double.NaN;
bearing = Angle.FromAngleInDegrees(0.0);
return;
}
double sqr_u = Math.Pow(Math.Cos(alpha), 2) *
(e.A * e.A - e.B * e.B) / (e.B * e.B);
double A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
double B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
double delta_sigma =
B * sin_sigma *
(cos_2sigma_m + B / 4 *
(cos_sigma * (-1 + 2 * sqr_cos_2sigma_m) -
B / 6 * cos_2sigma_m * (-3 + 4 * Math.Pow(sin_sigma, 2)) *
(-3 + 4 * sqr_cos_2sigma_m)
));
distance = e.B * A * (sigma - delta_sigma);
bearing = Angle.FromHeadingInDegrees(180.0 / Math.PI *
Math.Atan2(cos_U2 * Math.Sin(lambda), cos_U1 * sin_U2 - sin_U1 * cos_U2 * Math.Cos(lambda)));
}
protected abstract double GetMetricDistance(Position a, Position b);
public Position GridToGeo(Position grid)
{
double northings = grid.Y - projection.false_northing;
double eastings = grid.X - projection.false_easting;
double lat_fp = FootPointLatitude(northings);
double rc_fp, v_fp, p_fp;
RadiusOfCurvature(lat_fp, out rc_fp, out v_fp, out p_fp);
double rc_fp_2 = Math.Pow(rc_fp, 2);
double rc_fp_3 = Math.Pow(rc_fp, 3);
double t_fp = Math.Tan(lat_fp);
double t_fp_2 = Math.Pow(t_fp, 2);
double t_fp_4 = Math.Pow(t_fp, 4);
double t_fp_6 = Math.Pow(t_fp, 6);
double x = eastings / (projection.k0 * v_fp);
double x_3 = Math.Pow(x, 3);
double x_5 = Math.Pow(x, 5);
double x_7 = Math.Pow(x, 7);
double termp = t_fp / (projection.k0 * p_fp);
double term1 = termp * (x * eastings / 2.0);
double term2 = termp * (eastings * x_3 / 24.0) * (-4.0 * rc_fp_2 + 9.0 * rc_fp *
(1.0 - t_fp_2) + 12.0 * t_fp_2);
double term3 = termp * ((eastings * x_5) / 720.0) * (8.0 * Math.Pow(rc_fp, 4) *
(11.0 - 24.0 * t_fp_2) - 12.0 * rc_fp_3 * (21.0 - 71.0 * t_fp_2) + 15.0 *
rc_fp_2 * (15.0 - 98.0 * t_fp_2 + 15.0 * t_fp_4) + 180.0 * rc_fp *
(5.0 * t_fp_2 - 3.0 * t_fp_4) + 360.0 * t_fp_4);
double term4 = termp * (eastings * x_7 / 40320.0) * (1385.0 + 3633.0 * t_fp_2 +
4095.0 * t_fp_4 + 1575.0 * t_fp_6);
double latitude = lat_fp - term1 + term2 - term3 + term4;
double sec_lat_fp = Secant(lat_fp);
term1 = x * sec_lat_fp;
term2 = (x_3 / 6.0) * sec_lat_fp * (rc_fp + 2 * t_fp_2);
term3 = (x_5 / 120.0) * sec_lat_fp * (-4.0 * rc_fp_3 * (1.0 - 6.0 * t_fp_2) +
rc_fp_2 * (9.0 - 68.0 * t_fp_2) + 72.0 * rc_fp * t_fp_2 + 24.0 * t_fp_4);
term4 = (x_7 / 5040.0) * sec_lat_fp * (61.0 + 662.0 * t_fp_2 + 1320.0 * t_fp_4 +
720.0 * t_fp_6);
double longitude = lon_zero + term1 - term2 + term3 - term4;
return new Position(longitude, latitude);
}
public void TestWalk()
{
Position a = new Position(10, 100);
Position b = new Position(10 + 2, 100 + 1);
Position c = a.GetPointFromTargetAndDistance(b, Math.Sqrt(5) * 2);
Assert.AreEqual(10 + 4, c.X, 0.0001);
Assert.AreEqual(100 + 2, c.Y, 0.0001);
}
public void TestSpecificDistance()
{
Position a = new Position(121.71597, -33.516224000000001);
Position b = new Position(121.716843, -33.517730999999998);
double distance = Position.CalculateDistance(a, b, Ellipsoid.WGS84);
}
/// <summary>
/// Returns the winding number for a point in a polygon.
/// </summary>
/// <returns>The winding number, which is non-zero if the point is within the polygon.</returns>
public int GetWindingNumber(Position p)
{
int windingNumber = 0;
int vLength = _points.Count;
for (int i = 0; i < vLength; i++)
{
Position v1 = _points[i];
Position v2 = (i < vLength - 1) ? _points[i + 1] : _points[0];
// Handle the edge from _points[i] to _points[i + 1]:
if (v1.Y <= p.Y)
{
// Check for an upward crossing with the point to the left:
if (v2.Y > p.Y)
if (IsLeft(v1, v2, p) > 0)
windingNumber++;
}
else
{
// Check for an downward crossing with the point to the right:
if (v2.Y <= p.Y)
if (IsLeft(v1, v2, p) < 0)
windingNumber--;
}
}
return windingNumber;
}
public bool Contains(Position rhs)
{
return
_x1 <= rhs.X && rhs.X <= _x2 &&
_y1 <= rhs.Y && rhs.Y <= _y2;
}
// Containment routines based on:
//
// http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm
//
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
/// <summary>
/// Tests if a point is left, on, or right of an infinite line.
/// </summary>
/// <param name="a">The first point of the line.</param>
/// <param name="b">The second point of the line.</param>
/// <param name="p">The test point.</param>
/// <returns>Less than zero for a point left of the line, greater than zero for a
/// point right of the line, and zero for a point on the line.</returns>
static double IsLeft(Position a, Position b, Position p)
{
return (b.X - a.X) * (p.Y - a.Y) - (p.X - a.X) * (b.Y - a.Y);
}
// Calculates the distance between two points, using the method
// described in:
//
// Vincenty, T. Direct and Inverse Solutions of Geodesics of the
// Ellipsoid with Applications of Nested Equations, Survey Review,
// No. 176, 1975.
//
// Points that are nearly antipodal yield no solution; a
// Double.NaN is returned.
public static double CalculateDistance(Position a, Position b, Ellipsoid e)
{
double distance;
Angle bearing;
CalculateDistanceAndBearing(a, b, e, out distance, out bearing);
return distance;
}
public void Add(Position p)
{
_points.Add(p);
}
