public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); if (startNode == null || endNode == null) { return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode node = grid.GetGridObject(x, y); node.g = int.MaxValue; node.CalculateF(); node.parent = null; } } startNode.g = 0; startNode.h = Heuristic(startNode, endNode); startNode.CalculateF(); while (openList.Count > 0) { PathNode current = GetBestNode(openList); if (current == endNode) { return(CalculatePath(endNode)); } openList.Remove(current); closedList.Add(current); foreach (PathNode neighbour in GetNeighbours(current)) { if (closedList.Contains(neighbour)) { continue; } if (!neighbour.isWalkable) { closedList.Add(neighbour); continue; } int tentativeG = current.g + Heuristic(current, neighbour); if (tentativeG < neighbour.g) { neighbour.parent = current; neighbour.g = tentativeG; neighbour.h = Heuristic(neighbour, endNode); neighbour.CalculateF(); if (!openList.Contains(neighbour)) { openList.Add(neighbour); } } } } // openList is empty return(null); }