private void UpdateChunkQueues() { for (int x = -LoadDistance; x <= LoadDistance; x++) { for (int z = -LoadDistance; z <= LoadDistance; z++) { for (int y = 0; y < Height; y++) { ChunkLocation l = new ChunkLocation(centerX + x, y, centerZ + z); if (GetDistanceSquared(l) <= LoadDistance * LoadDistance && !currentlyLoading.Contains(l) && !chunkMap.ContainsKey(l)) { if (chunkLoadQueue.Contains(l)) { chunkLoadQueue.TryUpdatePriority(l, GetDistanceSquared(l, true)); } else { chunkLoadQueue.Enqueue(l, GetDistanceSquared(l, true)); } } } } } foreach (Chunk chunk in chunkMap.Values.Where(c => GetDistanceSquared(c.Location) > UnloadDistance * UnloadDistance)) { chunkUnloadStack.Push(chunk); } }
public TNode Evaluate <TNode, TKey>(TNode start, TKey key) where TNode : Node <TNode, TKey> { TNode bestComplete = null; var bestNodes = new Dictionary <TKey, TNode>(); var toEvaluate = new SimplePriorityQueue <TKey, decimal>(); var evaluated = new HashSet <TKey>(); bestNodes[start.Key] = start; toEvaluate.Enqueue(start.Key, start.EstimatedCost); while (true) { if (toEvaluate.Count == 0) { //var x = bestNodes.Select(n => new { node = n.Value, next = n.Value.GetAdjacent().ToArray() }) // .ToArray(); var bestLeft = bestNodes.Values.OrderBy(i => i.CurrentCost).First(); Console.WriteLine("Best of the rest...."); return(bestLeft); } var workKey = toEvaluate.Dequeue(); var work = bestNodes[workKey]; if (bestComplete != null && bestComplete.CurrentCost <= work.EstimatedCost) { return(bestComplete); } evaluated.Add(work.Key); foreach (var next in work.GetAdjacent()) { if (!next.IsValid) { continue; } if (next.IsComplete) { if (null == bestComplete || next.CurrentCost < bestComplete.CurrentCost) { // new best - remember it bestComplete = next; } // no need to continue to evaluate complete nodes continue; } if (bestNodes.TryGetValue(next.Key, out var existing)) { // we've already seen this node - update the cost if better, but no need to process further if (next.CurrentCost < existing.CurrentCost) { bestNodes[next.Key] = next; toEvaluate.TryUpdatePriority(next.Key, next.EstimatedCost); } continue; } // never seen this node before - track it and queue it up bestNodes.Add(next.Key, next); toEvaluate.Enqueue(next.Key, next.EstimatedCost); } } }
public virtual void Search(ref HashSet <SpaceTimeNode> reservationTable) { var startNode = new SpaceTimeNode(Start.X, Start.Y, 0); _gScore[startNode] = 0; _open.Enqueue(startNode, Heuristic(Start)); SpaceTimeNode current; while (_open.Count > 0) { current = _open.Dequeue(); _closed.Add(current); // current tile is reserved, we can't be here if (!current.Equals(Start) && (reservationTable.Contains(current) || reservationTable.Contains(current.Next()))) { continue; } if (current.Equals(Target)) { Path.AddRange(ReconstructPath(current)); for (var t = 0; t < Path.Count; t++) { reservationTable.Add(new SpaceTimeNode(Path[t].X, Path[t].Y, t)); } return; } foreach (var neighbor in _graph.GetNeighbours(current)) { if (_closed.Contains(neighbor) || reservationTable.Contains(neighbor) || reservationTable.Contains(neighbor.Next())) { continue; } var tentative_gScore = _gScore.GetValueOrDefault(current, double.PositiveInfinity) + Cost(current, neighbor); if (tentative_gScore < _gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { _cameFrom[neighbor] = current; _gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + Heuristic(_graph.GetTile(neighbor)); if (!_open.TryUpdatePriority(neighbor, fScore)) { _open.Enqueue(neighbor, fScore); } } } } }
private Stacja2[] Dijkstra(int miastopocz, int miastokon) { SimplePriorityQueue <int> kolejka = new SimplePriorityQueue <int>(); bool[] odwiedzone = new bool[ilosc_miast]; bool check = false; Stacja2[] tablica_odl = new Stacja2[ilosc_miast]; for (int i = 0; i < ilosc_miast; i++) { if (lista_incydencji[i].Count != 0) { odwiedzone[i] = true; check = true; } tablica_odl[i] = new Stacja2(); tablica_odl[i].czas = int.MaxValue; } if (check == false) { return(null); } tablica_odl[miastopocz].czas = -5; kolejka.Enqueue(miastopocz, -5); odwiedzone[miastopocz] = false; while (kolejka.Count != 0) { int aktualne = kolejka.First; kolejka.Dequeue(); foreach (Stacja s in lista_incydencji[aktualne]) { if (tablica_odl[aktualne].czas + s.czas + 5 < tablica_odl[s.nr_miasta].czas) { tablica_odl[s.nr_miasta].czas = tablica_odl[aktualne].czas + s.czas + 5; tablica_odl[s.nr_miasta].poprzednik = aktualne; if (odwiedzone[s.nr_miasta] == true) { odwiedzone[s.nr_miasta] = false; kolejka.Enqueue(s.nr_miasta, tablica_odl[s.nr_miasta].czas); } else { kolejka.TryUpdatePriority(s.nr_miasta, tablica_odl[s.nr_miasta].czas); } } } } return(tablica_odl); }
public void ChangePriority(Guid taskId, HttpRequestPriority newPriority) { _taskIdToTaskMap.TryGetValue(taskId, out var task); if (task == null || task.Priority == newPriority) { return; } _tasksQueue.TryUpdatePriority(task, GetPriorityForNewItem(newPriority)); }
public List <NetworkLane> CalculateRoute(string startID, string endID) { var q = new SimplePriorityQueue <NetworkLaneConnection> (); var distances = new Dictionary <string, double> (); var previous = new Dictionary <string, NetworkLaneConnection> (); NetworkLaneConnection start; if (!connectivityGraph.TryGetValue(startID, out start)) { Debug.Log("Start node was not found!"); return(null); } distances [startID] = 0; q.Enqueue(start, distances [startID]); while (q.Count > 0) { var current = q.Dequeue(); if (current.id == endID) { return(BacktrackRoute(current, previous)); } foreach (string id in current.adjacentLanes) { double newDist = distances [current.id] + current.Weight(id); if (!distances.ContainsKey(id) || newDist < distances [id]) { previous [id] = current; if (!distances.ContainsKey(id)) { q.Enqueue(connectivityGraph [id], newDist); } else { q.TryUpdatePriority(connectivityGraph [id], newDist); } distances [id] = newDist; } } } return(null); }
public void Search(IGraph graph) { Path.RemoveRange(_posIndex, Path.Count - _posIndex); var cameFrom = new Dictionary <AbstractTile, AbstractTile>(); var gScore = new Dictionary <AbstractTile, double>(); var closed = new HashSet <AbstractTile>(); var open = new SimplePriorityQueue <AbstractTile, double>(); gScore[Position] = 0; open.Enqueue(Position, Helpers.Metrics.Octile(Position, Target)); // TODO add agitation noise (see Silver) AbstractTile current; while (open.Count > 0) { current = open.Dequeue(); closed.Add(current); if (current == Target) { Path.AddRange(ReconstructPath(cameFrom, current)); return; } foreach (var neighbor in graph.AdjacentVertices(current)) { if (closed.Contains(neighbor)) { continue; } var tentative_gScore = gScore.GetValueOrDefault(current, double.PositiveInfinity) + Helpers.Metrics.Octile(current, neighbor); // TODO add agitation noise (see Silver) if (tentative_gScore < gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { cameFrom[neighbor] = current; gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + Helpers.Metrics.Octile(neighbor, Target); if (!open.TryUpdatePriority(neighbor, fScore)) { open.Enqueue(neighbor, fScore); } } } } }
/// <summary> /// Responsible for actually persisting the word counts in memory. /// </summary> /// /// <param name="word">The <see cref="string"/> word to persist.</param> private void PersistWord(string word) { if (!wordDict.ContainsKey(word)) { wordDict[word] = 0; } wordDict[word] += 1; var priority = wordDict[word]; if (priorityQueue.Contains(word)) { if (!priorityQueue.TryUpdatePriority(word, priority)) { throw new Exception($"Could not update priority for item: {word}."); } } else { if (priorityQueue.TryFirst(out var first)) { if (priority >= wordDict[first]) { priorityQueue.Enqueue(word, priority); } } else { priorityQueue.Enqueue(word, priority); } if (priorityQueue.Count > wordLimit) { if (priorityQueue.TryDequeue(out var head)) { Log.Logger.Debug($"Successfully removed item {head} with priority {wordDict[head]} from pirority queue."); } else { Log.Logger.Debug($"Cannot remove head from empty priority queue."); } } } }
void UCSearch(City root, City destiny, List <City> cities) { SimplePriorityQueue <City> Frontier = new SimplePriorityQueue <City>(); root.PathCost = 0; Frontier.Enqueue(root, 0); float sum; while (Frontier.Count != 0) { sum = Frontier.GetPriority(Frontier.First); City parent = (City)Frontier.Dequeue(); tbResult.Text += ("\n Expando el nodo de: " + parent.CityName + " Llevando un costo actual de: " + sum); tbOrder.Text += ("\n" + parent.CityName + " Costo: " + sum + "\n ↓"); if (parent == destiny) { tbResult.Text += ("\n Llegue al destino con un costo total de: " + sum); break; } cities.Add(parent); foreach (var tempRute in parent.Rutes) { if (!cities.Contains(tempRute.DestinationCity)) { if (!Frontier.Contains(tempRute.DestinationCity)) { Frontier.Enqueue(tempRute.DestinationCity, sum + tempRute.Cost); } else { if (Frontier.GetPriority(tempRute.DestinationCity) > sum + tempRute.Cost) { Frontier.TryUpdatePriority(tempRute.DestinationCity, sum + tempRute.Cost); } } } } } }
private void Search(ref HashSet <SpaceTimeNode> reservationTable) { _cameFrom = new Dictionary <SpaceTimeNode, SpaceTimeNode>(); _closed = new HashSet <SpaceTimeNode>(); _gScore = new Dictionary <SpaceTimeNode, double>(); _open = new SimplePriorityQueue <SpaceTimeNode, double>(); var pos = new SpaceTimeNode(Position.X, Position.Y, 0); _gScore[pos] = 0; _open.Enqueue(pos, _rra.AbstractDist(Position)); SpaceTimeNode current; while (_open.Count > 0) { current = _open.Dequeue(); _closed.Add(current); // current tile is reserved, we can't be here if (!current.Equals(Position) && (reservationTable.Contains(new SpaceTimeNode(current.X, current.Y, current.T + _pathIndex)) || reservationTable.Contains(new SpaceTimeNode(current.X, current.Y, current.T + _pathIndex + 1)))) { continue; } if (current.T == _w || current.Equals(Target)) { Path.AddRange(ReconstructPath(current)); for (var t = _pathIndex; t < Path.Count; t++) { reservationTable.Add(new SpaceTimeNode(Path[t].X, Path[t].Y, t)); } return; } foreach (var neighbor in _graph.GetNeighbours(current)) { if (_closed.Contains(neighbor) || reservationTable.Contains(new SpaceTimeNode(neighbor.X, neighbor.Y, neighbor.T + _pathIndex)) || reservationTable.Contains(new SpaceTimeNode(neighbor.X, neighbor.Y, neighbor.T + _pathIndex + 1))) { continue; } var tentative_gScore = _gScore.GetValueOrDefault(current, double.PositiveInfinity) + Cost(current, neighbor); if (tentative_gScore < _gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { _cameFrom[neighbor] = current; _gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + _rra.AbstractDist(_graph.GetTile(neighbor)); if (!_open.TryUpdatePriority(neighbor, fScore)) { _open.Enqueue(neighbor, fScore); } } } } }
//There is no need to aggressively early exit for now, we are in no need for such optimization public static bool TryFindPath(Coordinate start, Coordinate goal, Map map, out IList <Coordinate> path, int maxCost = int.MaxValue, bool includeStartingTile = false) { var tileToDiscover = new SimplePriorityQueue <Coordinate, float>(); var cameFrom = new Dictionary <Coordinate, Coordinate>(); var distanceToTile = new Dictionary <Coordinate, float> { [start] = 0f }; tileToDiscover.Enqueue(start, Heuristic(start, goal)); path = null; while (tileToDiscover.Any()) { var current = tileToDiscover.Dequeue(); if (current == goal) { path = new List <Coordinate>(); while (current != start) { path.Add(current); current = cameFrom[current]; } //we ignore the starting tile when calculating the cost var pathCost = path.Select(map.Get <Tile>).Sum(t => t.Data.Weight); if (includeStartingTile) { path.Add(start); } path = path.Reverse().ToList(); return(pathCost <= maxCost); } foreach (var neighbour in map.GetNeighbours <Tile>(current).Values) { if (neighbour == null) { //ignore tiles that does not exist(e.g. when current is at the top/bottom edge of the map) continue; } var currentDistanceToNeighbour = distanceToTile[current] + neighbour.Data.Weight; var neighbourCoordinate = neighbour.Coordinate; if (!distanceToTile.TryGetValue(neighbourCoordinate, out var previousDistanceToNeighbour)) { previousDistanceToNeighbour = float.PositiveInfinity; } if (currentDistanceToNeighbour > previousDistanceToNeighbour) { continue; } var newScoreForNeighbour = currentDistanceToNeighbour + Heuristic(neighbourCoordinate, goal); cameFrom[neighbourCoordinate] = current; distanceToTile[neighbourCoordinate] = currentDistanceToNeighbour; if (!tileToDiscover.TryUpdatePriority(neighbourCoordinate, newScoreForNeighbour)) { tileToDiscover.Enqueue(neighbourCoordinate, newScoreForNeighbour); } } } return(false); }
public Tuple <List <PointF>, float, float> CalculatePathBetweenVertices(GraphVertex vertex1, GraphVertex vertex2, RouteCalculationMode mode) { //Реализация алгоритма Дейкстры на основе очереди с приоритетом (Heap) int n = vertices.Count; float[] d = new float[n]; int[] p = new int[n]; for (int i = 0; i < n; i++) { d[i] = float.MaxValue; p[i] = -1; } d[vertex1.Id] = 0; var queue = new SimplePriorityQueue <int, float>(); queue.Enqueue(vertex1.Id, 0); while (queue.Count > 0) { int v = queue.Dequeue(); for (int i = 0; i < vertices[v].Edges.Count; i++) { int to = vertices[v].Edges[i].Vert2.Id; //если нужен самый короткий путь - минимизируем длину пути. //если нужен самый быстрый - минимизируем время (длина/скорость). float len = (mode == RouteCalculationMode.ShortRoute) ? (vertices[v].Edges[i].Length) : (vertices[v].Edges[i].Time); if (d[v] + len < d[to]) { d[to] = d[v] + len; p[to] = v; if (!queue.TryUpdatePriority(to, d[to])) { queue.Enqueue(to, d[to]); } } } } if (p[vertex2.Id] == -1) { //не нашли путь throw new Exception(); } // восстанавливаем путь int currentPoint = vertex2.Id; var vertPath = new List <int>(); while (currentPoint != -1) { vertPath.Add(currentPoint); currentPoint = p[currentPoint]; } vertPath.Reverse(); var edgePath = new List <GraphEdge>(); for (int i = 0; i < vertPath.Count - 1; i++) { var vert1 = vertices[vertPath[i]]; var vert2 = vertices[vertPath[i + 1]]; var edge = vert1.Edges.First(e => e.Vert2.Id == vert2.Id); edgePath.Add(edge); } List <PointF> wktPath = edgePath.SelectMany(e => e.Shape).ToList(); float length = edgePath.Sum(edge => edge.Length); float time = edgePath.Sum(edge => edge.Time); return(new Tuple <List <PointF>, float, float>(wktPath, length, time)); }
public TNode Evaluate <TNode, TKey, TCost>(TNode[] startNodes, Func <TNode, TNode, bool> isBetter = null, Action <TNode> evaluateNode = null, Action <TNode> queuedNode = null, Action <Dictionary <TKey, TNode>, SimplePriorityQueue <TKey, TCost>, HashSet <TKey> > whenDone = null) where TNode : Node <TNode, TKey, TCost> where TCost : IComparable <TCost> { if (null == isBetter) { isBetter = (i1, i2) => false; } if (null == evaluateNode) { evaluateNode = i => { }; } if (null == queuedNode) { queuedNode = i => { }; } if (null == whenDone) { whenDone = (a, b, c) => { }; } TNode bestComplete = null; var bestNodes = new Dictionary <TKey, TNode>(); var toEvaluate = new SimplePriorityQueue <TKey, TCost>(); var evaluated = new HashSet <TKey>(); foreach (var start in startNodes) { bestNodes[start.Key] = start; toEvaluate.Enqueue(start.Key, start.EstimatedCost); } var bestEstimatedCost = bestNodes[toEvaluate.First].EstimatedCost; while (true) { if (toEvaluate.Count == 0) { //var x = bestNodes.Select(n => new { node = n.Value, next = n.Value.GetAdjacent().ToArray() }) // .ToArray(); whenDone(bestNodes, toEvaluate, evaluated); return(bestComplete); //if (null != bestComplete) //{ // return bestComplete; //} //var bestLeft = bestNodes.Values.OrderBy(i => i.CurrentCost).First(); //Console.WriteLine("Best of the rest...."); //return bestLeft; } var workKey = toEvaluate.Dequeue(); var work = bestNodes[workKey]; if (work.EstimatedCost.CompareTo(bestEstimatedCost) < 0) { bestEstimatedCost = work.EstimatedCost; Console.WriteLine($"New best estimated cost: {bestEstimatedCost} - {work.Description}"); } evaluateNode(work); evaluated.Add(work.Key); if (work.IsComplete) { if (null == bestComplete || work.CurrentCost.CompareTo(bestComplete.CurrentCost) < 0 || (work.CurrentCost.CompareTo(bestComplete.CurrentCost) == 0 && isBetter(work, bestComplete))) { // new best - remember it bestComplete = work; } // no need to continue to evaluate complete nodes continue; } if (bestComplete != null && bestComplete.CurrentCost.CompareTo(work.EstimatedCost) < 0) { whenDone(bestNodes, toEvaluate, evaluated); return(bestComplete); } foreach (var next in work.GetAdjacent()) { if (!next.IsValid) { continue; } if (bestNodes.TryGetValue(next.Key, out var existing)) { // we've already seen this node - update the cost if better, but no need to process further var compare = next.CurrentCost.CompareTo(existing.CurrentCost); if (compare < 0 || (compare == 0 && isBetter(next, existing))) { bestNodes[next.Key] = next; toEvaluate.TryUpdatePriority(next.Key, next.EstimatedCost); } //Console.WriteLine($"Already seen node with {next.CurrentCost} - {next.EstimatedCost}\n{existing.Description}\n{next.Description}\n"); continue; } // never seen this node before - track it and queue it up bestNodes.Add(next.Key, next); toEvaluate.Enqueue(next.Key, next.EstimatedCost); //Console.WriteLine($"Queued node with EC {next.EstimatedCost}"); queuedNode(next); } } }