public IEnumerable <T> FindPath <T>(ISearchProblem <T> problem)
{
var frontier = new CustomPriorityQueue <SearchNode <T> >();
var predecessors = new Dictionary <T, T>();
var costs = new Dictionary <T, float>();
var initialNode = new SearchNode <T>(problem.InitialState, 0);
frontier.Enqueue(initialNode);
predecessors[problem.InitialState] = default;
costs[problem.InitialState] = 0;
while (frontier.Any())
{
var node = frontier.Dequeue();
if (problem.IsGoalState(node.State))
{
return(Path.Build(problem.InitialState, node.State, predecessors));
}
var successors = problem.GetSuccessors(node.State);
foreach (var successor in successors)
{
var newCost = costs[node.State] + successor.Cost;
if (costs.TryGetValue(successor.State, out var cost) && cost < newCost)
{
continue;
}
costs[successor.State] = newCost;
predecessors[successor.State] = node.State;
var heuristic = problem.CalculateHeuristic(successor.State);
var priority = newCost + heuristic;
var successorNode = new SearchNode <T>(successor.State, priority);
frontier.Enqueue(successorNode);
}
}
return(null);
}
public IEnumerable <T> FindPath <T>(ISearchProblem <T> problem)
{
var frontier = new CustomPriorityQueue <SearchNode <T> >();
var cameFrom = new Dictionary <T, T>();
var initialNode = new SearchNode <T>(problem.InitialState, 0);
frontier.Enqueue(initialNode);
cameFrom[problem.InitialState] = default;
while (frontier.Any())
{
var node = frontier.Dequeue();
if (problem.IsGoalState(node.State))
{
return(Path.Build(problem.InitialState, node.State, cameFrom));
}
var successors = problem.GetSuccessors(node.State);
foreach (var successor in successors)
{
if (cameFrom.ContainsKey(successor.State))
{
continue;
}
var priority = problem.CalculateHeuristic(successor.State);
var successorNode = new SearchNode <T>(successor.State, priority);
frontier.Enqueue(successorNode);
cameFrom[successor.State] = node.State;
}
}
return(null);
}