private double getHeadingToWaypoint(Coordinate curPos, APIWaypoint nextWpt)
{
Coordinate next = new Coordinate();
next.Latitude = nextWpt.Latitude;
next.Longitude = nextWpt.Longitude;
return(getHeadingToPoint(curPos, next));
}
private double getHeadingToWaypoint(Coordinate curPos, APIWaypoint nextWpt)
{
double longitude1 = curPos.Longitude;
double longitude2 = nextWpt.Longitude;
double latitude1 = deg2rad(curPos.Latitude);
double latitude2 = deg2rad(nextWpt.Latitude);
double longDiff = deg2rad(longitude2 - longitude1);
double y = Math.Sin(longDiff) * Math.Cos(latitude2);
double x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longDiff);
return((rad2deg(Math.Atan2(y, x)) + 360) % 360);
}
private double getDistToWaypoint(Coordinate curPos, APIWaypoint nextWpt)
{
var R = 3440; // Radius of the earth in nm
var dLat = deg2rad(nextWpt.Latitude - curPos.Latitude); // deg2rad below
var dLon = deg2rad(nextWpt.Longitude - curPos.Longitude);
var a =
Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(deg2rad(curPos.Latitude)) * Math.Cos(deg2rad(nextWpt.Latitude)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2)
;
var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
var d = R * c; // Distance in nm
return(d);
}