public List FindPath(GridPosition startPosition, GridPosition endPosition)
{
List openList = new();
List closedList = new();
PathNode startNode = gridSystem.GetGridObject(startPosition);
PathNode endNode = gridSystem.GetGridObject(endPosition);
openList.Add(startNode);
for (int x = 0; x < gridSystem.GetWidth(); x++)
{
for (int z = 0; z < gridSystem.GetHeight(); z++)
{
GridPosition gridPosition = new GridPosition(x, z);
PathNode pathNode = gridSystem.GetGridObject(gridPosition);
pathNode.SetGCost(int.MaxValue);
pathNode.SetHCost(0);
pathNode.CalculateFCost();
pathNode.ResetPreviousPathNode();
}
}
startNode.SetGCost(0);
startNode.SetHCost(CalculateDistance(startPosition, endPosition));
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return CalculatePath(endNode);
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode)) continue;
int tentativeGCost = currentNode.GetGCost() + CalculateDistance(currentNode.GetGridPosition(), neighbourNode.GetGridPosition());
if (tentativeGCost < neighbourNode.GetGCost())
{
neighbourNode.SetPreviousNode(currentNode);
neighbourNode.SetGCost(tentativeGCost);
neighbourNode.SetHCost(CalculateDistance(neighbourNode.GetGridPosition(), endPosition));
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// No path found
Debug.Log("You shall not pass!");
return null;
}