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