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