public static List <ITile> GetPath(int sizeX, int sizeY, List <ITile> startNodes, IsNodeValid matchesGoal, IsNodeValid validNode, NodeHuersticValue heuristic = null)
{
if (heuristic == null)
{
heuristic = delegate(ITile n, ITile cameFrom, IsNodeValid m) {
return(0);
};
}
HashSet <ITile> closedset = new HashSet <ITile>(); // The set of nodes already evaluated
HashSet <ITile> openset = new HashSet <ITile>(); // The set of tentative nodes to be evaluated, initially containing the start node
foreach (var node in startNodes)
{
openset.Add(node);
}
Dictionary <ITile, ITile> came_from = new Dictionary <ITile, ITile>(); // The map of navigated nodes.
Dictionary <ITile, int> g_score = new Dictionary <ITile, int>();
foreach (ITile node in startNodes)
{
g_score.Add(node, 0);
}
HeapPriorityQueue <PriorityQueueCoordinate <ITile> > f_score = new HeapPriorityQueue <PriorityQueueCoordinate <ITile> >(sizeX * sizeY);
foreach (ITile node in g_score.Keys)
{
f_score.Enqueue(new PriorityQueueCoordinate <ITile>(node), g_score[node] + heuristic(node, (came_from.ContainsKey(node) ? came_from[node] : null), matchesGoal));
}
while (openset.Count > 0)
{
// Find queued index with lowest score
ITile current = f_score.Dequeue().GetCoord();
if (matchesGoal(current))
{
// Walk backwards through the list and build the path
List <ITile> path = new List <ITile>();
while (true)
{
path.Insert(0, current);
if (startNodes.Contains(current))
{
return(path);
}
current = came_from[current];
}
}
openset.Remove(current);
closedset.Add(current);
foreach (ITile neighbor in current.NeighborCoordinates())
{
if (closedset.Contains(neighbor))
{
continue;
}
if (validNode != null)
{
if (!validNode(neighbor))
{
continue;
}
}
int bestTileScore = (g_score.ContainsKey(neighbor) ? g_score[neighbor] : 0);
int tentative_g_score = bestTileScore + 1;
if (!openset.Contains(neighbor) || tentative_g_score < g_score[neighbor])
{
came_from[neighbor] = current;
g_score[neighbor] = tentative_g_score;
f_score.Enqueue(new PriorityQueueCoordinate <ITile>(neighbor), g_score[neighbor] + heuristic(neighbor, current, matchesGoal));
if (!openset.Contains(neighbor))
{
openset.Add(neighbor);
}
}
}
}
//No path found
return(new List <ITile>());
}
public static List <ITile> GetPath(int sizeX, int sizeY, ITile startNode, IsNodeValid matchesGoal, IsNodeValid validNode, NodeHuersticValue heuristic = null)
{
return(GetPath(sizeX, sizeY, new List <ITile> {
startNode
}, matchesGoal, validNode, heuristic));
}
