private List <PathNode> FindPath(PathNode startNode, PathNode endNode)
{
startNode.heurecticCostToTheEnd = CalculateDistance(startNode, endNode);
startNode.costFromStartToTheCell = 0;
startNode.CalculateTotalCost();
startNode.previousNode = null;
openedList = new List <PathNode>();
openedList.Add(startNode);
closedList = new List <PathNode>();
for (int x = 0; x < grid.GetCollumns(); x++)
{
for (int y = 0; y < grid.GetRows(); y++)
{
PathNode node = grid.GetGridObject(x, y);
node.costFromStartToTheCell = Int32.MaxValue;
node.previousNode = null;
node.CalculateTotalCost();
}
}
//piorityQueue = new PiorityQueueHeap<PathNode>(tempList);
while (openedList.Count > 0)
{
currentNode = GetLowestTotalCostNode();
if (currentNode == endNode)
{
return(GetPath(currentNode));
}
openedList.Remove(currentNode);
closedList.Add(currentNode);
foreach (var neighbour in GetNeighbours(currentNode))
{
if (closedList.Contains(neighbour))
{
continue;
}
int estimateGcost = currentNode.costFromStartToTheCell + CalculateDistance(currentNode, neighbour);
if (estimateGcost < neighbour.costFromStartToTheCell)
{
neighbour.previousNode = currentNode;
neighbour.costFromStartToTheCell = estimateGcost;
neighbour.heurecticCostToTheEnd = CalculateDistance(neighbour, endNode);
neighbour.CalculateTotalCost();
if (!openedList.Contains(neighbour))
{
openedList.Add(neighbour);
}
}
}
}
return(null);
}