public static int Search(List<State> states, List<Action> actions, State start, Tile target) { var found = 0; PriorityQueue<BFNode> frontier = new PriorityQueue<BFNode>(); List<State> explored = new List<State>(); frontier.Add(new BFNode(start)); while (frontier.Count > 0) { // Chooses the lowest-cost node in the frontier BFNode currentBFNode = frontier.Pop(); // Win condition if (currentBFNode.State.Type.Equals(target)) found++; explored.Add(currentBFNode.State); // Filter actions to the ones connected to the current node foreach (Action action in actions.Where(a => a.StateA.Equals(currentBFNode.State))) { // One of A or B will be the currentBFNode's action // but it won't be added to the frontier since it // is already in explored var childA = new BFNode(currentBFNode, action, action.StateA); var childB = new BFNode(currentBFNode, action, action.StateB); if (!explored.Contains(childA.State) && !frontier.Any(n => n.State == childA.State)) frontier.Add(childA); if (!explored.Contains(childB.State) && !frontier.Any(n => n.State == childB.State)) frontier.Add(childB); } } return found; }
public List<MapTile> GetPath(Point start, Point goal) { var startTile = GetTile(start); var goalTile = GetTile(goal); // check that the start and goal positions are valid, and are not the same if (!Within(start) || !Within(goal) || start == goal || startTile == null || goalTile == null) { return new List<MapTile>(); } // Check that start and goal are walkable and that a path can exist between them if (startTile.Set != goalTile.Set) { return new List<MapTile>(); } // reset costs foreach (var t in _tiles) { t.F = t.G = float.MaxValue; } var open = new PriorityQueue<MapTile>(_tiles.Length); var closed = new HashSet<MapTile>(); startTile.G = 0; startTile.F = h(start, goal); open.Enqueue(startTile, startTile.F); MapTile current = null; while (open.Any() && current != goalTile) { current = open.Dequeue(); closed.Add(current); for (var i = 0; i < 8; i++) { var edge = current.Edges[i]; if (edge == null) { continue; } var neighbor = edge.Node2; var cost = current.G + edge.Cost; if (open.Contains(neighbor) && cost < neighbor.G) { open.Remove(neighbor); } if (closed.Contains(neighbor) && cost < neighbor.G) { closed.Remove(neighbor); } if (!open.Contains(neighbor) && !closed.Contains(neighbor)) { neighbor.G = cost; var f = cost + h(neighbor.MapPosition, goal); open.Enqueue(neighbor, f); neighbor.Parent = current; } } } System.Diagnostics.Debug.Assert(current == goalTile); var path = new List<MapTile>(); while (current != startTile) { path.Add(current); current = current.Parent; } path.Reverse(); return path; }