public double GetHaversineDistance(GPSData source, GPSData destination)
{
var phi1 = GPSData.ToRadians(source.Latitude);
var phi2 = GPSData.ToRadians(destination.Latitude);
var delPhi = GPSData.ToRadians(destination.Latitude - source.Latitude);
var delLambda = GPSData.ToRadians(destination.Longitude - source.Longitude);
var a = Math.Sin(delPhi / 2) * Math.Sin(delPhi / 2) + Math.Cos(phi1) * Math.Cos(phi2) * Math.Sin(delLambda / 2) * Math.Sin(delLambda / 2);
var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
HaversineDistance = R * c;
return HaversineDistance;
}
public double GetInitBearing(GPSData source, GPSData destination)
{
GPSData sourceInRadians = new GPSData() { Latitude = GPSData.ToRadians(source.Latitude), Longitude = GPSData.ToRadians(source.Longitude), Elevation = source.Elevation };
GPSData destinationInRadians = new GPSData() { Latitude = GPSData.ToRadians(destination.Latitude), Longitude = GPSData.ToRadians(destination.Longitude), Elevation = source.Elevation };
var y = Math.Sin(destinationInRadians.Longitude - sourceInRadians.Longitude) * Math.Cos(destinationInRadians.Latitude);
var x = Math.Cos((sourceInRadians.Latitude)) * Math.Sin((destinationInRadians.Latitude)) - Math.Sin((sourceInRadians.Latitude)) * Math.Cos((destinationInRadians.Latitude)) * Math.Cos(destinationInRadians.Longitude - sourceInRadians.Longitude);
InitBearing = GPSData.ToAngle(Math.Atan2(y, x));
return InitBearing;
}
public double GetFinalBearing(GPSData source, GPSData destination)
{
InitBearing = GetInitBearing(source, destination);
FinalBearing = (InitBearing + 180) % 360;
return FinalBearing;
}
public GPSData GetIntermediatePoint(GPSData source, GPSData destination, double fraction)
{
GPSData sourceInRadians = new GPSData() { Latitude = GPSData.ToRadians(source.Latitude), Longitude = GPSData.ToRadians(source.Longitude), Elevation = source.Elevation };
GPSData destinationInRadians = new GPSData() { Latitude = GPSData.ToRadians(destination.Latitude), Longitude = GPSData.ToRadians(destination.Longitude), Elevation = source.Elevation };
var distance = GetHaversineDistance(source, destination);
var a = Math.Sin((1 - fraction) * distance / R) / Math.Sin(distance / R);
var b = Math.Sin(fraction - (distance / R)) / Math.Sin(distance / R);
var x = (a * Math.Cos(sourceInRadians.Latitude) * Math.Cos(sourceInRadians.Longitude)) + (b * Math.Cos(destinationInRadians.Latitude) * Math.Cos(destinationInRadians.Longitude));
var y = (a * Math.Cos(sourceInRadians.Latitude) * Math.Sin(sourceInRadians.Longitude)) + (b * Math.Cos(destinationInRadians.Latitude) * Math.Sin(destinationInRadians.Longitude));
var z = (a * Math.Sin(sourceInRadians.Latitude)) + (b * Math.Sin(destinationInRadians.Latitude));
var lat = Math.Atan2(z, Math.Sqrt((x * x) + (y * y)));
var longit = Math.Atan2(y, x);
GPSData interPoint = new GPSData() { Latitude = GPSData.ToAngle(lat), Longitude = GPSData.ToAngle(longit)};
return interPoint;
}