internal static double[] GetDistanceAndHeadingToDestination(double currentLat, double currentLon,
double destinationLat, double destinationLon)
{
try
{
var diflat = (destinationLat - currentLat).ToRadians();
currentLat = currentLat.ToRadians(); //convert current latitude to radians
destinationLat = destinationLat.ToRadians(); //convert waypoint latitude to radians
var diflon = (destinationLon - currentLon).ToRadians();
//subtract and convert longitude to radians
var distCalc = Math.Sin(diflat / 2.0) * Math.Sin(diflat / 2.0);
var distCalc2 = Math.Cos(currentLat);
distCalc2 = distCalc2 * Math.Cos(destinationLat);
distCalc2 = distCalc2 * Math.Sin(diflon / 2.0);
distCalc2 = distCalc2 * Math.Sin(diflon / 2.0); //and again, why?
distCalc += distCalc2;
distCalc = 2 * Math.Atan2(Math.Sqrt(distCalc), Math.Sqrt(1.0 - distCalc));
distCalc = distCalc * 6371000.0;
//Converting to meters. 6371000 is the magic number, 3959 is average Earth radius in miles
distCalc = Math.Round(distCalc * 39.3701, 1); // and then to inches.
currentLon = currentLon.ToRadians();
destinationLon = destinationLon.ToRadians();
var heading = Math.Atan2(Math.Sin(destinationLon - currentLon) * Math.Cos(destinationLat),
Math.Cos(currentLat) * Math.Sin(destinationLat) -
Math.Sin(currentLat) * Math.Cos(destinationLat) * Math.Cos(destinationLon - currentLon));
heading = heading.ToDegrees();
if (heading < 0)
heading += 360;
return new[] { Math.Round(distCalc, 1), Math.Round(heading, 1) };
}
catch (Exception e)
{
Debug.WriteLine(e);
return new double[] { 0, 0 };
}
}