42 lines
1.4 KiB
C#
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;
|
|
}
|
|
} |