public List <T> FindPath(T start, T goal)
{
if (start == null)
{
return(null);
}
if (goal == null)
{
return(null);
}
this.Goal = goal;
gScore = new Dictionary <T, float>();
fScore = new Dictionary <T, float>();
//var openset = new HashSet<T>();
var openset = new FibonacciQueue <T, float>(d => fScore[d]);
var closedset = new HashSet <T>();
cameFrom = new Dictionary <T, T>();
//came_from := the empty map // The map of navigated nodes.
gScore[start] = 0; // Cost from start along best known path.
// Estimated total cost from start to goal through y.
fScore[start] = gScore[start] + ConnectionProvider.GetHeuristicCostEstimate(start, goal);
openset.Enqueue(start);
onBegin();
while (openset.Count > 0)
{
var current = openset.Dequeue();
onSelectNode(current);
if (StopCondition(current))
{
return(reconstruct_reverse_path(cameFrom, current).Reverse().ToList());
}
if (current.Equals(goal))
{
return(reconstruct_reverse_path(cameFrom, goal).Reverse().ToList());
}
closedset.Add(current); //add current to closedset
foreach (var neighbor in ConnectionProvider.GetConnectedNodes(this, current))
{
if (closedset.Contains(neighbor))
{
continue;
}
var tentative_g_score = gScore[current] + ConnectionProvider.GetCost(current, neighbor);
if (openset.Contains(neighbor) && !(tentative_g_score <= gScore[neighbor]))
{
continue;
}
cameFrom[neighbor] = current;
gScore[neighbor] = tentative_g_score;
fScore[neighbor] = gScore[neighbor] + ConnectionProvider.GetHeuristicCostEstimate(neighbor, goal);
if (openset.Contains(neighbor))
{
continue;
}
openset.Enqueue(neighbor);
onEnqueueNode(neighbor);
}
}
return(null);
}
