/// <summary> /// Call this method to change the priority of an item. /// Calling this method on a item not in the queue will throw an exception. /// If the item is enqueued multiple times, only the first one will be updated. /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able /// to update all of them, please wrap your items in a wrapper class so they can be distinguished). /// O(log n) /// </summary> public void UpdatePriority(TItem item, TPriority priority) { lock (_queue) { SimpleNode updateMe = GetExistingNode(item); if (updateMe == null) { throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + item); } _queue.UpdatePriority(updateMe, priority); } }
private void Update() { var adjacencyRule = (AdjacencyRule)_baseMap.DistanceMeasurement; var openSet = new GenericPriorityQueue <PositionNode, double>(Width * Height); foreach (var point in _baseMap.Walkable) { var newPoint = _baseMap[point] * -Magnitude; _goalMap[point] = newPoint; openSet.Enqueue(_nodes[point], newPoint.Value); } var edgeSet = new HashSet <Coord>(); var closedSet = new HashSet <Coord>(); while (openSet.Count > 0) //multiple runs are needed to deal with islands { var minNode = openSet.Dequeue(); closedSet.Add(minNode.Position); foreach (var openPoint in adjacencyRule.Neighbors(minNode.Position)) { if ((!closedSet.Contains(openPoint)) && _baseMap.BaseMap[openPoint] != GoalState.Obstacle) { edgeSet.Add(openPoint); } } while (edgeSet.Count > 0) { foreach (var coord in edgeSet.ToArray()) { var current = _goalMap[coord].Value; foreach (var openPoint in adjacencyRule.Neighbors(coord)) { if (closedSet.Contains(openPoint) || _baseMap.BaseMap[openPoint] == GoalState.Obstacle) { continue; } var neighborValue = _goalMap[openPoint].Value; var newValue = current + _baseMap.DistanceMeasurement.Calculate(coord, openPoint); if (newValue < neighborValue) { _goalMap[openPoint] = newValue; openSet.UpdatePriority(_nodes[openPoint], newValue); edgeSet.Add(openPoint); } } edgeSet.Remove(coord); closedSet.Add(coord); openSet.Remove(_nodes[coord]); } } } }
void AddToFrontier(InternalPathNode pos) { var f = movementCosts.Get(pos.coord) + Heuristic(pos.coord); if (frontier.Contains(pos)) { frontier.UpdatePriority(pos, f); } else { frontier.Enqueue(pos, f); } }
private static Path shortestPath(Tile a, Tile b, Map m) { //If runtime is too high, use a hash table to store indices dijkstraNode[] nodes = new dijkstraNode[moveList.Count]; index[] indices = new index[moveList.Count]; for (int i = 0; i < moveList.Count; ++i) { nodes[i].found = false; nodes[i].dist = 999; nodes[i].previous = -1; indices[i] = new index(i); } int start = moveList.IndexOf(a); int end = moveList.IndexOf(b); //<index,weight> GenericPriorityQueue <index, int> queue = new GenericPriorityQueue <index, int>(moveList.Count); nodes[start].dist = 0; for (int i = 0; i < moveList.Count; i++) { if (i == start) { queue.Enqueue(indices[i], 0); } else { queue.Enqueue(indices[i], 999); } } index tempi; int x, y, r; while (queue.Count != 0) { //for(int k = 0; k<queue.Count; k++) { tempi = queue.Dequeue(); nodes[tempi.i].found = true; if (tempi.i == end) { break; } x = moveList[tempi.i].getX(); y = moveList[tempi.i].getY(); //ideally we shorten this; maybe do some sin/cos stuff in UserMath //east r = moveList.IndexOf(m.findTile(x + 1, y)); if (x < m.getMapWidth() - 1 && r != -1 && !nodes[r].found) { if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist) { nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist; nodes[r].previous = tempi.i; queue.UpdatePriority(indices[r], nodes[r].dist); } } //north r = moveList.IndexOf(m.findTile(x, y + 1)); if (r != -1 && !nodes[r].found && y < m.getMapHeight() - 1) { if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist) { nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist; nodes[r].previous = tempi.i; queue.UpdatePriority(indices[r], nodes[r].dist); } } //west r = moveList.IndexOf(m.findTile(x - 1, y)); if (r != -1 && !nodes[r].found && x > 0) { if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist) { nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist; nodes[r].previous = tempi.i; queue.UpdatePriority(indices[r], nodes[r].dist); } } //south r = moveList.IndexOf(m.findTile(x, y - 1)); if (r != -1 && !nodes[r].found && y > 0) { if (!nodes[r].found && nodes[r].dist > movementCosts[moveList[r].getType()] + nodes[tempi.i].dist) { nodes[r].dist = movementCosts[moveList[r].getType()] + nodes[tempi.i].dist; nodes[r].previous = tempi.i; queue.UpdatePriority(indices[r], nodes[r].dist); } } } //end of while-loop Path ret = new Path(); if (nodes[end].previous == -1) { return(null); } for (int i = end; i != start;) { ret.insertHead(moveList[i], movementCosts[moveList[i].getType()]); i = nodes[i].previous; } //To impelement; high runtimes might result in reworking this algorithm return(ret); }
public static List <IPathNode> FindPath(IPathNode start, IPathNode finish) { if (start == finish) { return(new List <IPathNode>()); } var frontier = new GenericPriorityQueue <IPathNode, int>(1000); frontier.Enqueue(start, 0); var cameFrom = new Dictionary <IPathNode, IPathNode>(); var costSoFar = new Dictionary <IPathNode, int>(); IPathNode current; IPathNode next; List <IPathNode> neighbours; int newCost; int priority; cameFrom[start] = null; costSoFar[start] = 0; while (frontier.Count > 0) { current = frontier.Dequeue(); if (current == finish) { break; } neighbours = current.GetNeighbours(); for (int i = 0; i < neighbours.Count; i++) { next = neighbours[i]; newCost = costSoFar[current] + (next == finish || next.IsWalkable ? 1 : 10000); if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { if (costSoFar.ContainsKey(next)) { costSoFar[next] = newCost; } else { costSoFar.Add(next, newCost); } priority = newCost + finish.GetHeuristic(next); if (frontier.Contains(next)) { frontier.UpdatePriority(next, priority); } else { frontier.Enqueue(next, priority); } cameFrom[next] = current; } } } var path = new List <IPathNode>(); if (cameFrom.ContainsKey(finish)) { IPathNode last = finish; path.Add(finish); while (cameFrom[last] != start) { path.Add(cameFrom[last]); last = cameFrom[last]; } path.Reverse(); } return(path); }
private void Update() { int width = Width; AdjacencyRule adjacencyRule = _baseMap.DistanceMeasurement; var mapBounds = _goalMap.Bounds(); var walkable = _baseMap.Walkable; for (int i = 0; i < walkable.Count; i++) { var point = walkable[i]; // Value won't be null as null only happens for non-walkable squares var newPoint = _baseMap[point] !.Value * -Magnitude; _goalMap[point] = newPoint; _openSet.Enqueue(_nodes[point], newPoint); } _edgeSet.Clear(); _closedSet.SetAll(false); while (_openSet.Count > 0) // Multiple runs are needed to deal with islands { var minNode = _openSet.Dequeue(); _closedSet[minNode.Position.ToIndex(width)] = true; for (int i = 0; i < adjacencyRule.DirectionsOfNeighborsCache.Length; i++) { var openPoint = minNode.Position + adjacencyRule.DirectionsOfNeighborsCache[i]; if (!mapBounds.Contains(openPoint)) { continue; } if (!_closedSet[openPoint.ToIndex(width)] && _baseMap.BaseMap[openPoint] != GoalState.Obstacle) { _edgeSet.Enqueue(openPoint); } } while (_edgeSet.Count > 0) { var point = _edgeSet.Dequeue(); var pointIndex = point.ToIndex(width); if (!mapBounds.Contains(point) || _closedSet[pointIndex]) { continue; } var current = _goalMap[point] !.Value; // Never added non-nulls so this is fine for (int j = 0; j < adjacencyRule.DirectionsOfNeighborsCache.Length; j++) { var openPoint = point + adjacencyRule.DirectionsOfNeighborsCache[j]; if (!mapBounds.Contains(openPoint)) { continue; } if (_closedSet[openPoint.ToIndex(width)] || _baseMap.BaseMap[openPoint] == GoalState.Obstacle) { continue; } var neighborValue = _goalMap[openPoint] !.Value; // Never added non-nulls so this is fine var newValue = current + _baseMap.DistanceMeasurement.Calculate(point, openPoint); if (newValue < neighborValue) { _goalMap[openPoint] = newValue; _openSet.UpdatePriority(_nodes[openPoint], newValue); _edgeSet.Enqueue(openPoint); } } _closedSet[pointIndex] = true; _openSet.Remove(_nodes[point]); } } }