public static List <Node> FindPath(PathGrid grid, Vector2 startPos, Vector2 targetPos)
{
Node startNode = NodeFromCoordinates(grid, startPos);
Node targetNode = NodeFromCoordinates(grid, targetPos);
List <Node> openSet = new List <Node>();
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node current = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (openSet[i].FCost < current.FCost || openSet[i].FCost == current.FCost)
{
if (openSet[i].HCost < current.HCost)
{
current = openSet[i];
}
}
}
openSet.Remove(current);
closedSet.Add(current);
if (current == targetNode)
{
List <Node> path = RetracePath(startNode, targetNode);
return(path);
}
foreach (Node neighbour in grid.GetNeighbours(current))
{
if (!neighbour.Walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCost = current.GCost + GetDistance(current, neighbour);
if (newMovementCost < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newMovementCost;
neighbour.HCost = GetDistance(neighbour, targetNode);
neighbour.Parent = current;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(new List <Node>());
}
public static Node NodeFromCoordinates(PathGrid grid, Vector2 position)
{
TileCoordinates coords = position.ToTileCoordinates();
coords.X = Math.Clamp(coords.X, 0, GameWorld.WORLD_WIDTH);
coords.Y = Math.Clamp(coords.Y, 0, GameWorld.WORLD_HEIGHT);
return(grid.Grid[coords.X, coords.Y]);
}
