public override Path solve(Maze maze) { LinkedList <ScoredCoordinates> closedSet = new LinkedList <ScoredCoordinates>(); LinkedList <ScoredCoordinates> openSet = new LinkedList <ScoredCoordinates>(); ScoredCoordinates start = new ScoredCoordinates(maze.startPosition); start.f = start.heuristic_cost(maze.endPosition); openSet.AddFirst(start); while (openSet.Count >= 1) { ScoredCoordinates current = openSet.First(); foreach (ScoredCoordinates node in openSet) { if (node.f < current.f) { current = node; } } if (current.Equals(maze.endPosition)) { // Return Reconstructed path Path reconstructedPath = new Path(maze.startPosition); while (current != null) { reconstructedPath.AddAfter(reconstructedPath.First, current); current = current.cameFrom; } return(reconstructedPath); } openSet.Remove(current); closedSet.AddLast(current); foreach (ScoredCoordinates neighbor in getNeighbors(current, maze)) { if (closedSet.Contains(neighbor)) { continue; } // Selecting the node in the openSet if it was already added bool isNew = true; ScoredCoordinates exploredNeighbor = neighbor; foreach (ScoredCoordinates openSetMember in openSet) { if (neighbor.Equals(openSetMember)) { exploredNeighbor = openSetMember; isNew = false; break; } } if (isNew) { openSet.AddLast(neighbor); } double tentative_g = current.g + maze.valueAt(exploredNeighbor); if (tentative_g >= exploredNeighbor.g) { continue; } exploredNeighbor.cameFrom = current; exploredNeighbor.g = tentative_g; exploredNeighbor.f = exploredNeighbor.g + exploredNeighbor.heuristic_cost(maze.endPosition); } } return(null); }