/// <summary> /// Removes an item from the queue. The item does not need to be the head of the queue. /// If the item is not in the queue, an exception is thrown. If unsure, check Contains() first. /// If multiple copies of the item are enqueued, only the first one is removed. /// O(log n) /// </summary> public void Remove(TItem item) { lock (_queue) { SimpleNode removeMe; IList <SimpleNode> nodes; if (item == null) { if (_nullNodesCache.Count == 0) { throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item); } removeMe = _nullNodesCache[0]; nodes = _nullNodesCache; } else { if (!_itemToNodesCache.TryGetValue(item, out nodes)) { throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item); } removeMe = nodes[0]; if (nodes.Count == 1) { _itemToNodesCache.Remove(item); } } _queue.Remove(removeMe); nodes.Remove(removeMe); } }
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]); } } } }
/// <summary> /// Removes an item from the queue. The item does not need to be the head of the queue. /// If the item is not in the queue, an exception is thrown. If unsure, check Contains() first. /// If multiple copies of the item are enqueued, only the first one is removed. /// O(n) /// </summary> public void Remove(TItem item) { lock (_queue) { try { _queue.Remove(GetExistingNode(item)); } catch (InvalidOperationException ex) { throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item, ex); } } }
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]); } } }