public static bool Search <T>(IAstarGraph <T> graph, T start, T goal, out Dictionary <T, T> cameFrom)
{
var foundPath = false;
cameFrom = new Dictionary <T, T> {
{ start, start }
};
var costSoFar = new Dictionary <T, int>();
var frontier = new List <Tuple <int, T> > {
new Tuple <int, T>(0, start)
};
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier[0];
frontier.RemoveAt(0);
if (current.Item2.Equals(goal))
{
foundPath = true;
break;
}
foreach (var next in graph.GetNeighbors(current.Item2))
{
var newCost = costSoFar[current.Item2] + graph.Cost(current.Item2, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
var priority = newCost + graph.Heuristic(next, goal);
frontier.Add(new Tuple <int, T>(priority, next));
cameFrom[next] = current.Item2;
}
}
frontier.Sort(new TupleComparer <T>());
}
return(foundPath);
}
public static bool Search <T>(IAstarGraph <T> graph, T start, T goal, out Dictionary <T, T> cameFrom)
{
var foundPath = false;
cameFrom = new Dictionary <T, T>();
cameFrom.Add(start, start);
var costSoFar = new Dictionary <T, int>();
var frontier = new PriorityQueue <AStarNode <T> >(1000);
frontier.Enqueue(new AStarNode <T>(start), 0);
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Data.Equals(goal))
{
foundPath = true;
break;
}
foreach (var next in graph.GetNeighbors(current.Data))
{
var newCost = costSoFar[current.Data] + graph.Cost(current.Data, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
var priority = newCost + graph.Heuristic(next, goal);
frontier.Enqueue(new AStarNode <T>(next), priority);
cameFrom[next] = current.Data;
}
}
}
return(foundPath);
}