private static double DistanceBetweenPoints(TDrivePosition point1, TDrivePosition point2)
{
double dLat = DegToRad(point2.Latitude - point1.Latitude);
double dLon = DegToRad(point2.Longtitude - point1.Longtitude);
double lat1 = DegToRad(point1.Latitude);
double lat2 = DegToRad(point2.Latitude);
double a = Math.Pow(Math.Sin(dLat / 2), 2) + Math.Pow(Math.Sin(dLon / 2), 2) * Math.Cos(lat1) * Math.Cos(lat2);
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
double d = Math.Round(EARTH_RADIUS * c * 10) / 10;
return(d);
}
private const double EARTH_RADIUS = 6371004; //km
static public double DistanceTo(this TDrivePosition currentPosition, TDrivePosition position)
{
return(DistanceBetweenPoints(currentPosition, position));
}