public static double HaversineDistance(CoordinatePoint pa, CoordinatePoint pb)
{
double R = 6371;//kilometers, otherwise 6371 for miles
double dLat = toRadian(pa.Latitude.Value - pb.Latitude.Value);
double dLon = toRadian(pa.Longitude.Value - pb.Longitude.Value);
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(toRadian(pa.Latitude.Value)) * Math.Cos(toRadian(pb.Latitude.Value)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
double c = 2 * Math.Asin(Math.Min(1, Math.Sqrt(a)));
double d = R * c;
return(d * 1000); //convert to meters
}
public static void IntersectTest()
{
double deviation = -5.90659270237146;
var a = new CoordinatePoint(new Coordinate(39.302384), new Coordinate(-84.3143165), 0);
var aBearing = 146.51 + deviation;
var b = new CoordinatePoint(new Coordinate(39.2972996666667), new Coordinate(-84.3152046666667), 0);
var bBearing = 233.10 + deviation;
//var a = new CoordinatePoint(new Coordinate(39,15,26.84), new Coordinate(-84,-17,-15.93), 0);
//var aBearing = 90;
//var b = new CoordinatePoint(new Coordinate( 39,15,35.25), new Coordinate( -84,-17,-11.84), 0);
//var bBearing = 180;
var intersection = CoordinatePointUtilities.FindIntersection(a, aBearing, b, bBearing);
Console.WriteLine(intersection.Latitude.Value + "," + intersection.Longitude.Value);
}
/// <summary>
/// find difference between current heading and course heading
/// </summary>
/// <param name="targetMark"></param>
/// <param name="previousMark"></param>
/// <param name="current"></param>
/// <param name="previous"></param>
/// <returns></returns>
private double RelativeAngleToCourse(Mark targetMark, Mark previousMark, CoordinatePoint current, CoordinatePoint previous)
{
if (previousMark != null && targetMark != null)
{
float courseAngle = (float)AngleUtilities.FindAngle(targetMark.Location.Project(), previousMark.Location.Project());
float boatAngle = (float)AngleUtilities.FindAngle(previous.Project(), current.Project()); ;
return AngleUtilities.AngleDifference(courseAngle, boatAngle);
}
else
{
return 0;
}
}
/// <summary>
/// calculate vmg from marks
/// </summary>
/// <param name="targetMark"></param>
/// <param name="previousMark"></param>
/// <param name="current"></param>
/// <param name="previous"></param>
/// <param name="speed"></param>
/// <returns></returns>
private double VelocityMadeGood(Mark targetMark, Mark previousMark, CoordinatePoint current, CoordinatePoint previous,double speed)
{
return Math.Cos(Math.Abs(RelativeAngleToCourse(targetMark,previousMark,current,previous))) * speed;
}
/// <summary>
/// finds "great circle" intersection of 2 lines
/// http://www.movable-type.co.uk/scripts/latlong.html
/// http://www.movable-type.co.uk/scripts/js/geodesy/latlon-spherical.js
/// </summary>
/// <param name="p1"></param>
/// <param name="brng1"></param>
/// <param name="p2"></param>
/// <param name="brng2"></param>
/// <returns></returns>
public static CoordinatePoint FindIntersection(CoordinatePoint p1, double brng1, CoordinatePoint p2,
double brng2)
{
double a = p1.Latitude.Value.ToRadians();
double c = p1.Longitude.Value.ToRadians();
double b = p2.Latitude.Value.ToRadians();
double d = p2.Longitude.Value.ToRadians();
var e = brng1.ToRadians();
var f = brng2.ToRadians();
var g = b - a;
var h = d - c;
var i = 2 * Math.Asin(Math.Sqrt(Math.Sin(g / 2) * Math.Sin(g / 2) +
Math.Cos(a) * Math.Cos(b) * Math.Sin(h / 2) * Math.Sin(h / 2)));
if (i == 0)
{
return(null);
}
// initial/final bearings between points
var j = Math.Acos((Math.Sin(b) - Math.Sin(a) * Math.Cos(i)) /
(Math.Sin(i) * Math.Cos(a)));
//if (isNaN(j)) j = 0; // protect against rounding
var k = Math.Acos((Math.Sin(a) - Math.Sin(b) * Math.Cos(i)) /
(Math.Sin(i) * Math.Cos(b)));
double j2;
double k1;
if (Math.Sin(d - c) > 0)
{
j2 = j;
k1 = 2 * Math.PI - k;
}
else
{
j2 = 2 * Math.PI - j;
k1 = k;
}
var l = (e - j2 + Math.PI) % (2 * Math.PI) - Math.PI; // angle 2-1-3
var m = (k1 - f + Math.PI) % (2 * Math.PI) - Math.PI; // angle 1-2-3
if (Math.Sin(l) == 0 && Math.Sin(m) == 0)
{
return(null); // infinite intersections
}
if (Math.Sin(l) * Math.Sin(m) < 0)
{
return(null); // ambiguous intersection
}
//l = Math.abs(l);
//m = Math.abs(m);
// ... Ed Williams takes abs of l/m, but seems to break calculation?
var n = Math.Acos(-Math.Cos(l) * Math.Cos(m) +
Math.Sin(l) * Math.Sin(m) * Math.Cos(i));
var o = Math.Atan2(Math.Sin(i) * Math.Sin(l) * Math.Sin(m),
Math.Cos(m) + Math.Cos(l) * Math.Cos(n));
var p = Math.Asin(Math.Sin(a) * Math.Cos(o) +
Math.Cos(a) * Math.Sin(o) * Math.Cos(e));
var q = Math.Atan2(Math.Sin(e) * Math.Sin(o) * Math.Cos(a),
Math.Cos(o) - Math.Sin(a) * Math.Sin(p));
var r = c + q;
r = (r + 3 * Math.PI) % (2 * Math.PI) - Math.PI; // normalise to -180..+180°
return(new CoordinatePoint(new Coordinate(p.ToDegrees()), new Coordinate(r.ToDegrees()), p1.HeightAboveGeoID));
}
public static double HaversineDistance(CoordinatePoint pa, CoordinatePoint pb)
{
double R = 6371;//kilometers, otherwise 6371 for miles
double dLat = toRadian(pa.Latitude.Value - pb.Latitude.Value);
double dLon = toRadian(pa.Longitude.Value - pb.Longitude.Value);
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(toRadian(pa.Latitude.Value)) * Math.Cos(toRadian(pb.Latitude.Value)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
double c = 2 * Math.Asin(Math.Min(1, Math.Sqrt(a)));
double d = R * c;
return d * 1000; //convert to meters
}
/// <summary>
/// finds "great circle" intersection of 2 lines
/// http://www.movable-type.co.uk/scripts/latlong.html
/// http://www.movable-type.co.uk/scripts/js/geodesy/latlon-spherical.js
/// </summary>
/// <param name="p1"></param>
/// <param name="brng1"></param>
/// <param name="p2"></param>
/// <param name="brng2"></param>
/// <returns></returns>
public static CoordinatePoint FindIntersection(CoordinatePoint p1, double brng1, CoordinatePoint p2,
double brng2)
{
double a = p1.Latitude.Value.ToRadians();
double c = p1.Longitude.Value.ToRadians();
double b = p2.Latitude.Value.ToRadians();
double d = p2.Longitude.Value.ToRadians();
var e = brng1.ToRadians();
var f = brng2.ToRadians();
var g = b - a;
var h = d - c;
var i = 2*Math.Asin(Math.Sqrt(Math.Sin(g/2)*Math.Sin(g/2) +
Math.Cos(a)*Math.Cos(b)*Math.Sin(h/2)*Math.Sin(h/2)));
if (i == 0)
return null;
// initial/final bearings between points
var j = Math.Acos((Math.Sin(b) - Math.Sin(a)*Math.Cos(i))/
(Math.Sin(i)*Math.Cos(a)));
//if (isNaN(j)) j = 0; // protect against rounding
var k = Math.Acos((Math.Sin(a) - Math.Sin(b)*Math.Cos(i))/
(Math.Sin(i)*Math.Cos(b)));
double j2;
double k1;
if (Math.Sin(d - c) > 0)
{
j2 = j;
k1 = 2*Math.PI - k;
}
else
{
j2 = 2*Math.PI - j;
k1 = k;
}
var l = (e - j2 + Math.PI)%(2*Math.PI) - Math.PI; // angle 2-1-3
var m = (k1 - f + Math.PI)%(2*Math.PI) - Math.PI; // angle 1-2-3
if (Math.Sin(l) == 0 && Math.Sin(m) == 0)
return null; // infinite intersections
if (Math.Sin(l)*Math.Sin(m) < 0)
return null; // ambiguous intersection
//l = Math.abs(l);
//m = Math.abs(m);
// ... Ed Williams takes abs of l/m, but seems to break calculation?
var n = Math.Acos(-Math.Cos(l)*Math.Cos(m) +
Math.Sin(l)*Math.Sin(m)*Math.Cos(i));
var o = Math.Atan2(Math.Sin(i)*Math.Sin(l)*Math.Sin(m),
Math.Cos(m) + Math.Cos(l)*Math.Cos(n));
var p = Math.Asin(Math.Sin(a)*Math.Cos(o) +
Math.Cos(a)*Math.Sin(o)*Math.Cos(e));
var q = Math.Atan2(Math.Sin(e)*Math.Sin(o)*Math.Cos(a),
Math.Cos(o) - Math.Sin(a)*Math.Sin(p));
var r = c + q;
r = (r + 3*Math.PI)%(2*Math.PI) - Math.PI; // normalise to -180..+180°
return new CoordinatePoint(new Coordinate(p.ToDegrees()), new Coordinate(r.ToDegrees()), p1.HeightAboveGeoID);
}
