OSMServer/Pathfinding/Pathfinder.cs
2023-02-03 23:44:15 +01:00

42 lines
1.4 KiB
C#

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
PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
}
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;
}
}