AStar/Graph/Utils.cs

35 lines
1.2 KiB
C#
Raw Normal View History

2022-11-13 14:02:27 +01:00
namespace Graph.Utils
2022-11-13 13:21:24 +01:00
{
2022-05-06 00:02:28 +02:00
public struct Utils
{
2022-11-13 13:21:24 +01:00
public static double DistanceBetweenNodes(Node n1, Node n2)
2022-05-06 00:02:28 +02:00
{
return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
}
2022-11-13 13:21:24 +01:00
public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
2022-05-06 00:02:28 +02:00
{
2022-11-03 19:13:53 +01:00
const int earthRadius = 6371000;
2022-05-06 00:02:28 +02:00
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));
2022-11-13 13:21:24 +01:00
return earthRadius * c;
2022-05-06 00:02:28 +02:00
}
private static double DegreesToRadians(double deg)
{
return deg * Math.PI / 180.0;
}
private static double RadiansToDegrees(double rad)
{
return rad * 180.0 / Math.PI;
}
}
}