OSMServer/Pathfinding/Pathfinder.cs

42 lines
1.4 KiB
C#
Raw Normal View History

2023-02-03 23:35:22 +01:00
using OSMDatastructure;
using OSMImporter;
namespace Pathfinding;
public class Pathfinder
{
private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
{
RegionManager regionManager = new RegionManager(workingDir);
Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
2023-02-03 23:44:15 +01:00
PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
2023-02-03 23:35:22 +01:00
Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
2023-02-03 23:44:15 +01:00
PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
2023-02-03 23:35:22 +01:00
}
private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region);
return closestId != null ? region.GetNode((ulong)closestId) : null;
}
private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = null;
double closestDistance = double.MaxValue, distance;
foreach (KeyValuePair<ulong, Node> kv in region.GetNodes())
{
distance = Utils.DistanceBetween(kv.Value, coordinates);
if (distance < closestDistance)
{
closestDistance = distance;
closestId = kv.Key;
}
}
return closestId;
}
}