//Algorithm itself
private List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = _grid.GetGridObject(startX, startY);
PathNode endNode = _grid.GetGridObject(endX, endY);
_openList = new PriorityQueue <PathNode>();
_closedList = new List <PathNode>();
for (int x = 0; x < _grid.Width; x++)
{
for (int y = 0; y < _grid.Height; y++)
{
PathNode pathNode = _grid.GetGridObject(x, y);
pathNode.GCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.PrevNode = null;
}
}
startNode.GCost = 0;
startNode.HCost = CalculateDistance(startNode, endNode);
startNode.CalculateFCost();
_openList.Enqueue(startNode, 0);
while (_openList.Count > 0)
{
PathNode curNode = _openList.Dequeue();
if (curNode == endNode)
{
return(ReconstructPath(endNode));
}
_closedList.Add(curNode);
foreach (PathNode neighbour in GetNeighbourList(curNode))
{
if (_closedList.Contains(neighbour))
{
continue;
}
if (!neighbour.IsWalkable)
{
_closedList.Add(neighbour);
continue;
}
int tentativeGCost = curNode.GCost + CalculateDistance(curNode, neighbour);
if (tentativeGCost < neighbour.GCost)
{
neighbour.PrevNode = curNode;
neighbour.GCost = tentativeGCost;
neighbour.HCost = CalculateDistance(neighbour, endNode);
neighbour.CalculateFCost();
_openList.Enqueue(neighbour, neighbour.FCost);
}
}
}
return(null);
}
