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); }