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