AStar/Graph/Utils.cs
2022-05-06 00:02:28 +02:00

35 lines
1.1 KiB
C#

namespace Graph{
public struct Utils
{
public static double DistanceBetweenNodes(Node n1, Node n2)
{
return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
}
public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
{
const int earthRadius = 6371;
double differenceLat = DegreesToRadians(lat2 - lat1);
double differenceLon = DegreesToRadians(lon2 - lon1);
double lat1Rads = DegreesToRadians(lat1);
double lat2Rads = DegreesToRadians(lat2);
double a = Math.Sin(differenceLat / 2) * Math.Sin(differenceLat / 2) + Math.Sin(differenceLon / 2) * Math.Sin(differenceLon / 2) * Math.Cos(lat1Rads) * Math.Cos(lat2Rads);
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
return earthRadius * c;
}
private static double DegreesToRadians(double deg)
{
return deg * Math.PI / 180.0;
}
private static double RadiansToDegrees(double rad)
{
return rad * 180.0 / Math.PI;
}
}
}