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