public List <GraphNode> Search(List <List <GraphNode> > grid, GraphNode start, GraphNode end, bool diagonal = false, Func <List <Vector2>, int> heuristic = null, bool avoidOthers = false) { if (start.IsWall()) { return(null); } Init(grid); heuristic = (heuristic == null)?Manhattan:heuristic; diagonal = !!diagonal; var openHeap = Heap(); openHeap.Push(start); while (openHeap.Size() > 0) { // Grab the lowest f(x) to process next. Heap keeps this sorted for us. var currentNode = openHeap.Pop(); // End case -- result has been found, return the traced path. if (currentNode == end) { var curr = currentNode; var ret = new List <GraphNode>(); while (curr.parent != null) { ret.Add(curr); curr = curr.parent; } ret.Reverse(); return(ret); } // Normal case -- move currentNode from open to closed, process each of its neighbors. currentNode.closed = true; // Find all neighbors for the current node. Optionally find diagonal neighbors as well (false by default). var neighbors = Neighbors(grid, currentNode, diagonal, avoidOthers); var il = neighbors.Count; for (var i = 0; i < il; i++) { var neighbor = neighbors[i]; if (neighbor.closed || neighbor.IsWall()) { // Not a valid node to process, skip to next neighbor. continue; } // The g score is the shortest distance from start to current node. // We need to check if the path we have arrived at this neighbor is the shortest one we have seen yet. var gScore = currentNode.g + neighbor.cost; var beenVisited = neighbor.visited; if (!beenVisited || gScore < neighbor.g) { // Found an optimal (so far) path to this node. Take score for node to see how good it is. neighbor.visited = true; neighbor.parent = currentNode; var pos = new List <Vector2>(); pos.Add(neighbor.pos); pos.Add(end.pos); // todo - make sure this is really what the original js code does if (neighbor.h == 0) { neighbor.h = heuristic(pos); } // neighbor.g = gScore; neighbor.f = neighbor.g + neighbor.h; if (!beenVisited) { // Pushing to heap will put it in proper place based on the 'f' value. openHeap.Push(neighbor); } else { // Already seen the node, but since it has been rescored we need to reorder it in the heap openHeap.RescoreElement(neighbor); } } } } // No result was found - empty array signifies failure to find path. return(null); }