public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
if (startNode == null || endNode == null)
{
return(null);
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < grid.GetWidth(); x++)
{
for (int y = 0; y < grid.GetHeight(); y++)
{
PathNode node = grid.GetGridObject(x, y);
node.g = int.MaxValue;
node.CalculateF();
node.parent = null;
}
}
startNode.g = 0;
startNode.h = Heuristic(startNode, endNode);
startNode.CalculateF();
while (openList.Count > 0)
{
PathNode current = GetBestNode(openList);
if (current == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(current);
closedList.Add(current);
foreach (PathNode neighbour in GetNeighbours(current))
{
if (closedList.Contains(neighbour))
{
continue;
}
if (!neighbour.isWalkable)
{
closedList.Add(neighbour);
continue;
}
int tentativeG = current.g + Heuristic(current, neighbour);
if (tentativeG < neighbour.g)
{
neighbour.parent = current;
neighbour.g = tentativeG;
neighbour.h = Heuristic(neighbour, endNode);
neighbour.CalculateF();
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
}
// openList is empty
return(null);
}