public ActionAStar(ActionGraph graph, GoapActionNode start, Dictionary <string, object> goal)
{
var frontier = new Priority_Queue.FastPriorityQueue <GoapActionNode>(10000);
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (MeetsGoal(current, goal))
{
finalPoint = current;
break;
}
foreach (var next in graph.Neighbors(current))
{
float newCost = costSoFar[current] + graph.Cost(current, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
float priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
}
public Dictionary <Coordinates, Coordinates> SearchUnfilled(Graph graph, Coordinates start, Coordinates end)
{
var frontier = new Priority_Queue.FastPriorityQueue <Coordinates>(10000);
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current == end)
{
this.end = current;
break;
}
foreach (var next in graph.Neighbors(current))
{
float newCost = costSoFar[current] + graph.Cost(next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
float priority = newCost + Heuristic(next, end);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
return(cameFrom);
}
/// <summary>
/// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out.
/// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.
/// Duplicates are allowed.
/// O(log n)
/// </summary>
public void Enqueue(T item, double priority)
{
SimpleNode node = new SimpleNode(item);
if (_queue.Count == _queue.MaxSize)
{
_queue.Resize(_queue.MaxSize * 2 + 1);
}
_queue.Enqueue(node, priority);
}
public void Enqueue(T item, double priority)
{
lock (queue) {
Node node = new Node(item);
if (queue.Count == queue.MaxSize)
{
queue.Resize(queue.MaxSize * 2 + 1);
}
queue.Enqueue(node, priority);
}
}
