private List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
if (endNode.isWalkable == false)
{
endNode.isWalkable = true;
changedEndNodeIsWalkable = true;
}
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 pathNode = grid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
// Reached final node
if (changedEndNodeIsWalkable == true)
{
endNode.isWalkable = false;
changedEndNodeIsWalkable = false;
}
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in currentNode.neighbourList)
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList (Could not find a path)
Debug.Log("Failed to find path");
if (changedEndNodeIsWalkable == true)
{
endNode.isWalkable = false;
changedEndNodeIsWalkable = false;
}
return(null);
}