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