99 lines
3.4 KiB
C#
99 lines
3.4 KiB
C#
using OSMDatastructure;
|
|
using OSMImporter;
|
|
|
|
namespace Pathfinding;
|
|
|
|
public class Pathfinder
|
|
{
|
|
|
|
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
|
|
{
|
|
RegionManager regionManager = new RegionManager(workingDir);
|
|
Region startRegion, goalRegion;
|
|
PathNode startNode, goalNode;
|
|
try
|
|
{
|
|
startRegion = regionManager.GetRegion(start);
|
|
startNode = PathNode.FromNode(ClosestNodeToCoordinates(start, startRegion)!); //TODO null handling
|
|
}
|
|
catch (FileNotFoundException e)
|
|
{
|
|
throw new Exception(string.Format("No region at coordinates {0}", start), e);
|
|
}
|
|
try
|
|
{
|
|
goalRegion = regionManager.GetRegion(goal);
|
|
goalNode = PathNode.FromNode(ClosestNodeToCoordinates(goal, goalRegion)!); //TODO null handling
|
|
}
|
|
catch (FileNotFoundException e)
|
|
{
|
|
throw new Exception(string.Format("No region at coordinates {0}", start), e);
|
|
}
|
|
|
|
List<PathNode> toVisit = new() { startNode };
|
|
startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
|
|
bool stop = false;
|
|
|
|
while (toVisit.Count > 0 && !stop)
|
|
{
|
|
PathNode closestNodeToGoal = toVisit.First();
|
|
foreach (PathNode node in toVisit)
|
|
{
|
|
if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
|
|
{
|
|
closestNodeToGoal = node;
|
|
}
|
|
}
|
|
|
|
foreach (Connection connection in closestNodeToGoal.GetConnections())
|
|
{
|
|
PathNode? neighbor = (PathNode)regionManager.GetNode(connection.endNodeCoordinates);
|
|
if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
|
|
{
|
|
neighbor.previousPathNode = closestNodeToGoal;
|
|
neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
|
|
Utils.DistanceBetween(closestNodeToGoal, neighbor);
|
|
|
|
if (neighbor.Equals(goalNode))
|
|
stop = true;
|
|
else
|
|
toVisit.Add(neighbor);
|
|
}
|
|
}
|
|
}
|
|
|
|
List<PathNode> path = new();
|
|
PathNode currentNode = goalNode;
|
|
while (!currentNode.Equals(startNode))
|
|
{
|
|
path.Add(currentNode);
|
|
currentNode = currentNode.previousPathNode!;
|
|
}
|
|
path.Add(startNode);
|
|
|
|
return path;
|
|
}
|
|
|
|
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;
|
|
}
|
|
} |