Added first Pathfinding
This commit is contained in:
@ -6,15 +6,72 @@ namespace Pathfinding;
|
||||
public class Pathfinder
|
||||
{
|
||||
|
||||
private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
|
||||
public static List<PathNode> 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
|
||||
|
||||
Region startRegion, goalRegion;
|
||||
PathNode startNode, goalNode;
|
||||
try
|
||||
{
|
||||
startRegion = regionManager.GetRegion(start);
|
||||
startNode = (PathNode)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)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)
|
||||
|
Reference in New Issue
Block a user