public List <Node> FindPath(Vector2 start, Vector2 end)
{
Node startNode = grid.WorldPositionToNode(start);
Node endNode = grid.WorldPositionToNode(end);
openList = new List <Node>();
closedList = new List <Node>();
openList.Add(startNode);
for (int x = 0; x < grid._gridSizeX; x++)
{
for (int y = 0; y < grid._gridSizeY; y++)
{
Node node = grid.GetNode(x, y);
node.GCost = int.MaxValue;
node.cameFromNode = null;
}
}
startNode.GCost = 0;
startNode.HCost = CalculateDistanceCost(startNode, endNode);
while (openList.Count > 0)
{
Node currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(currentNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (Node neighbour in GetNeighbours(currentNode))
{
if (closedList.Contains(neighbour))
{
continue;
}
int tentativeGCost = currentNode.GCost + CalculateDistanceCost(currentNode, neighbour);
if (tentativeGCost < neighbour.GCost)
{
neighbour.cameFromNode = currentNode;
neighbour.GCost = tentativeGCost;
neighbour.HCost = CalculateDistanceCost(neighbour, endNode);
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
}
return(null);
}