public void TestDebugContainsOutOfBoundsVeryLarge()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
HeapPriorityQueue <Node> queue = new HeapPriorityQueue <Node>(5);
Enqueue(queue, node1);
node1.QueueIndex = node2.QueueIndex = int.MaxValue;
Assert.Throws <InvalidOperationException>(() => queue.Contains(node1));
Assert.Throws <InvalidOperationException>(() => queue.Contains(node2));
}
void SetVals(HeapPriorityQueue <MapNode> openQ, int x, int y, int g, int g_inc, int count, coord prev, int end_x, int end_y)
{
if (!m_Map[x, y].closed && m_Map[x, y].g > g + g_inc)
{
m_Map[x, y].g = g + g_inc;
m_Map[x, y].h = Heuristic(x, y, end_x, end_y);
m_Map[x, y].f = m_Map[x, y].g + m_Map[x, y].h;
m_Map[x, y].count = count + 1;
m_Map[x, y].previous = prev;
/*if(Heuristic(prev.x, prev.y, x, y) > 1)
* {
* Debug.Log("Err: H: " + Heuristic(prev.x, prev.y, x, y).ToString() + ": prev (" + prev.x.ToString() +", " + prev.y.ToString() + ") to current (" + x.ToString() + ", " + y.ToString() + ")");
* }*/
if (openQ.Contains(m_Map[x, y]))
{
openQ.UpdatePriority(m_Map[x, y], m_Map[x, y].f);
}
else
{
openQ.Enqueue(m_Map[x, y], m_Map[x, y].f);
}
}
}
private void AddOperation(OperationContext operationContext)
{
lock (queuedOperations)
if (RunningOperations != null)
{
lock (RunningOperations)
{
if (queuedOperations.Contains(operationContext))
{
throw new ArgumentOutOfRangeException(nameof(operationContext), "This operation has already been queued to run.");
}
if (RunningOperations.Contains(operationContext))
{
throw new ArgumentOutOfRangeException(nameof(operationContext), "This operation is already running.");
}
}
}
queuedOperations.Enqueue(operationContext);
}
public List <Coordinate> GetShortestPath(Coordinate start, Coordinate goal)
{
HashSet <Coordinate> visited = new HashSet <Coordinate>();
Dictionary <Coordinate, Coordinate> parents = new Dictionary <Coordinate, Coordinate>();
Dictionary <Coordinate, double> gScore = new Dictionary <Coordinate, double>();
HeapPriorityQueue <Coordinate> fScoreQueue = new HeapPriorityQueue <Coordinate>(rows * cols);
parents[start] = start;
gScore.Add(start, 0);
fScoreQueue.Enqueue(start, gScore[start] + Heuristic(start, goal));
while (fScoreQueue.Count() != 0)
{
Coordinate current = fScoreQueue.Dequeue();
Console.Out.WriteLine("");
Console.Out.WriteLine("Current = " + current.ToString());
Console.Out.WriteLine("Visited = " + visited.ToString());
Console.Out.WriteLine("Parents = " + parents.ToString());
Console.Out.WriteLine("gScore = " + gScore.ToString());
Console.Out.WriteLine("fScoreQueue = " + fScoreQueue.ToString());
if (current == goal)
{
return(ReconstructPath(parents, goal));
}
visited.Add(start);
foreach (Coordinate neighbor in board[current.row, current.col].GetNeighborCoordinates())
{
if (visited.Contains(neighbor))
{
continue;
}
double newGScore = gScore[current] + Distance(current, neighbor);
if (!fScoreQueue.Contains(neighbor))
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.Enqueue(neighbor, newGScore + Heuristic(neighbor, goal));
}
else if (newGScore < gScore[neighbor])
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.UpdatePriority(neighbor, newGScore + Heuristic(neighbor, goal));
}
}
}
return(null);
}
public static List<Coordinate> GetShortestPath(Board board, Coordinate start, Coordinate goal)
{
HashSet<Coordinate> visited = new HashSet<Coordinate>();
Dictionary<Coordinate, Coordinate> parents = new Dictionary<Coordinate, Coordinate>();
Dictionary<Coordinate, double> gScore = new Dictionary<Coordinate, double>();
HeapPriorityQueue<Coordinate> fScoreQueue = new HeapPriorityQueue<Coordinate>(board.MaxSize());
parents[start] = start;
gScore.Add(start, 0);
fScoreQueue.Enqueue(start, gScore[start] + Heuristic(start, goal));
while (fScoreQueue.Count() != 0)
{
Coordinate current = fScoreQueue.Dequeue();
//Console.WriteLine("Current = " + current.ToString());
if (current.Equals(goal))
{
Console.WriteLine("FOUND GOAL!!!");
return ReconstructPath(parents, goal);
}
visited.Add(current);
List<Coordinate> neighbors = board.GetNeighbors(current);
foreach (Coordinate neighbor in neighbors)
{
if (visited.Contains(neighbor)) continue;
if (!board.GetSquare(neighbor).IsTraversable()) continue;
double newGScore = gScore[current] + Distance(current, neighbor);
if (!fScoreQueue.Contains(neighbor))
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.Enqueue(neighbor, newGScore + Heuristic(neighbor, goal));
}
else if (newGScore < gScore[neighbor])
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.UpdatePriority(neighbor, newGScore + Heuristic(neighbor, goal));
}
}
}
return null;
}
public List<Node> findPath(Point start, Point end)
{
foreach (KeyValuePair<Point, Node> k in dict)
{
k.Value.gvalue = 0;
k.Value.parent = null;
}
HeapPriorityQueue<Node> open_set = new HeapPriorityQueue<Node>(49152);
Dictionary<Point, Node> closed_set = new Dictionary<Point, Node>();
dict[start].gvalue = 0;
open_set.Enqueue(dict[start], 0);
Node curr = null;
while (open_set.Count > 0)
{
// lowest rank
curr = open_set.Dequeue();
if (curr.getPoint().Equals(end))
{
break;
}
closed_set[curr.getPoint()] = curr;
foreach (Node n in curr.getLinks())
{
int cost = curr.gvalue + (int)(Math.Abs(end.X - curr.getPoint().X) + Math.Abs(end.Y - curr.getPoint().Y));
if (open_set.Contains(n) && cost < n.gvalue)
{
open_set.Remove(n);
}
if (!open_set.Contains(n) && !closed_set.ContainsKey(n.getPoint()))
{
n.gvalue = cost;
open_set.Enqueue(n, cost);
n.parent = curr;
}
}
}
return followPath(curr, new List<Node>());
}
// Get rid of all of the news, since they are creating garbage. Reuse the objects.
public static List<Node> calculatePath(Int2 start, Int2 end)
{
Node startNode = new Node(null, start, calculatePointIndex(start));
Node targetNode = new Node(null, end, calculatePointIndex(end));
Node[] visited = new Node[world.GetLength(0) * world.GetLength(1)];
HeapPriorityQueue<Node> frontier = new HeapPriorityQueue<Node>(100);
List<Node> result = new List<Node>();
frontier.Enqueue(startNode, 0); // dummy value for priority since it will be popped immediately.
// Continue algorithm until there are no more open nodes.
while (frontier.Count > 0)
{
Node current = frontier.Dequeue();
// If the popped node is the target node, then you are done.
if (current.index == targetNode.index)
{
result.Clear();
result.Add(current);
Node nodeInShortestPath = current.parent;
while (nodeInShortestPath != null)
{
result.Add(nodeInShortestPath);
nodeInShortestPath = nodeInShortestPath.parent;
}
result.Reverse();
}
else
{
List<Int2> neighbors = findNeighbors(current.point);
foreach (Int2 neighbor in neighbors) { // foreach has a bug that creates garbage via wrappers
int pointIndex = calculatePointIndex(neighbor);
Node neighborNode = visited[pointIndex] != null ?
visited[pointIndex] : new Node(current, neighbor, pointIndex);
int newNeighborCost = current.g + manhattanDistance(neighbor, current.point);
if (visited[neighborNode.index] == null || neighborNode.g > newNeighborCost)
{
neighborNode.g = newNeighborCost;
neighborNode.f = neighborNode.g + manhattanDistance(neighbor, targetNode.point);
neighborNode.parent = current;
if (!frontier.Contains(neighborNode))
{
frontier.Enqueue(neighborNode, neighborNode.f);
}
else
{
frontier.UpdatePriority(neighborNode, neighborNode.f);
}
visited[neighborNode.index] = neighborNode;
}
}
}
}
// If frontier is emptied out and the target hasn't been reached, then the path is blocked and no shortest path exists.
return result;
}
void SetVals(HeapPriorityQueue<MapNode> openQ, int x, int y, int g, int g_inc, int count, coord prev, int end_x, int end_y)
{
if(!m_Map[x,y].closed && m_Map[x,y].g > g+g_inc)
{
m_Map[x,y].g = g+g_inc;
m_Map[x,y].h = Heuristic(x, y, end_x, end_y);
m_Map[x,y].f = m_Map[x,y].g + m_Map[x,y].h;
m_Map[x,y].count = count+1;
m_Map[x,y].previous = prev;
/*if(Heuristic(prev.x, prev.y, x, y) > 1)
{
Debug.Log("Err: H: " + Heuristic(prev.x, prev.y, x, y).ToString() + ": prev (" + prev.x.ToString() +", " + prev.y.ToString() + ") to current (" + x.ToString() + ", " + y.ToString() + ")");
}*/
if(openQ.Contains(m_Map[x,y]))
{
openQ.UpdatePriority(m_Map[x,y], m_Map[x,y].f);
}
else
{
openQ.Enqueue(m_Map[x,y], m_Map[x,y].f);
}
}
}
public List<Coordinate> GetShortestPath(Coordinate start, Coordinate goal)
{
HashSet<Coordinate> visited = new HashSet<Coordinate>();
Dictionary<Coordinate, Coordinate> parents = new Dictionary<Coordinate, Coordinate>();
Dictionary<Coordinate, double> gScore = new Dictionary<Coordinate, double>();
HeapPriorityQueue<Coordinate> fScoreQueue = new HeapPriorityQueue<Coordinate>(rows * cols);
parents[start] = start;
gScore.Add(start, 0);
fScoreQueue.Enqueue(start, gScore[start] + Heuristic(start, goal));
while (fScoreQueue.Count() != 0)
{
Coordinate current = fScoreQueue.Dequeue();
Console.Out.WriteLine("");
Console.Out.WriteLine("Current = " + current.ToString());
Console.Out.WriteLine("Visited = " + visited.ToString());
Console.Out.WriteLine("Parents = " + parents.ToString());
Console.Out.WriteLine("gScore = " + gScore.ToString());
Console.Out.WriteLine("fScoreQueue = " + fScoreQueue.ToString());
if (current == goal)
{
return ReconstructPath(parents, goal);
}
visited.Add(start);
foreach (Coordinate neighbor in board[current.row,current.col].GetNeighborCoordinates())
{
if (visited.Contains(neighbor)) continue;
double newGScore = gScore[current] + Distance(current, neighbor);
if (!fScoreQueue.Contains(neighbor))
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.Enqueue(neighbor, newGScore + Heuristic(neighbor, goal));
}
else if (newGScore < gScore[neighbor])
{
parents[neighbor] = current;
gScore[neighbor] = newGScore;
fScoreQueue.UpdatePriority(neighbor, newGScore + Heuristic(neighbor, goal));
}
}
}
return null;
}
private Node Dijstra(by source, by target, Politik politik, pakke sendtPakke, float multiplier)
{
var queue = new HeapPriorityQueue<Node>(_byliste.Count * 2);
_nodes = new List<Node>();
Node targetBy = null;
foreach (var by in _byliste)
{
var node = new Node
{
By = by
};
if (by.CityId == target.CityId)
{
targetBy = node;
}
node.Distance = by.CityId == source.CityId ? 0 : double.MaxValue;
_nodes.Add(node);
queue.Enqueue(node, node.Distance);
}
while (queue.Any())
{
var node = queue.Dequeue();
if (node == targetBy && node.Distance != double.MaxValue)
return node;
GetRoutes(node, politik, sendtPakke, multiplier);
foreach (var neighbour in getNeighbourghNodes(node, queue))
{
if (neighbour == null || !queue.Contains(neighbour))
continue;
var dist = node.Distance + DistanceBetween(node, neighbour, politik);
if (dist < neighbour.Distance)
{
neighbour.Distance = dist;
neighbour.Previous = node;
queue.UpdatePriority(neighbour, dist);
}
}
}
return null;
}
public List <Vector2> AStarPath(Vector2 startPos, Vector2 goalPos)
{
int goalPosX = (int)goalPos.x;
int goalPosY = (int)goalPos.y;
Node startNode = new Node((int)startPos.x, (int)startPos.y, 0, null);
HashSet <Node> closedNodes = new HashSet <Node>();
HeapPriorityQueue <Node> q = new HeapPriorityQueue <Node>(10000);
List <Vector2> path = null;
q.Enqueue(startNode, AStarHeuristic(startNode.x, startNode.y, goalPosX, goalPosY));
while (q.Count > 0 && q.Count < q.MaxSize - 5)
{
Node currNode = q.Dequeue();
// print ("currNode: " + currNode.x + ", " + currNode.y + " - moves: " + currNode.g + ", h: " + AStarHeuristic(currNode.x, currNode.y, goalPosX, goalPosY) + ", prio: " + currNode.Priority);
if (currNode.x == goalPosX && currNode.y == goalPosY)
{
return(GetPathThroughNodes(currNode));
}
closedNodes.Add(currNode);
foreach (Node neighbor in NeighborNodes(currNode))
{
if (closedNodes.Contains(neighbor))
{
// print ("node was in closedNodes!");
continue;
}
if (q.Contains(neighbor))
{
// print ("node was in closedNodes!");
continue;
}
// float tentativeG =
q.Enqueue(neighbor, neighbor.g + AStarHeuristic(currNode.x, currNode.y, goalPosX, goalPosY));
// print ("neighbor added: " + neighbor.x + ", " + neighbor.y + " - moves: " + neighbor.g + ", h: " + AStarHeuristic(neighbor.x, neighbor.y, goalPosX, goalPosY) + ", prio: " + neighbor.Priority);
}
// add current to closedset
// for each neighbor in neighbor_nodes(current)
// if neighbor in closedset
// continue
// tentative_g_score := g_score[current] + dist_between(current,neighbor)
//
// if neighbor not in openset or tentative_g_score < g_score[neighbor]
// came_from[neighbor] := current
// g_score[neighbor] := tentative_g_score
// f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
// if neighbor not in openset
// add neighbor to openset
// if (closedNodes.Contains(currNode)){
// continue;
// }
// Node
// q.Enqueue(startNode, AStarHeuristic(startNode.x, startNode.y, goalPosX, goalPosY));
// closedNodes.Add(currNode);
}
return(path);
}
/// <summary>
/// Runs the Dijkstra algorithm which calculates the shortest paths from the source to any other node
/// which is reachable from there.
///
/// If this method is called multiple times with the same source it does only calculate the paths the first time
///
/// Exceptions:
/// ArgumentException if the nodes Enumerable is null, the source is null or the source is not part of the graph (the nodes)
/// </summary>
public void Run(IEnumerable <AdjacencyNode <T> > nodes, AdjacencyNode <T> source)
{
if (nodes == null || source == null)
{
throw new ArgumentException("Nodes Enumerable or Source is null");
}
if (Source != null && Source.Equals(source))
{
return;
}
/**
* Initialize the Algorithm
*/
Source = source;
// Holds the shortest distance between the source and the node
Dictionary <AdjacencyNode <T>, int> distance = new Dictionary <AdjacencyNode <T>, int>();
// Holds the node from which we need to go to the current node if we are taking the shortest path
PreviousNode = new Dictionary <AdjacencyNode <T>, AdjacencyNode <T> >();
// Fast Access to the Node (of the Nodes to inspect) which has the shortest distance and thus needs to be processed next
// if we processed all nodes in that queue or the remaining ones are not reachable the algorithm is finished
HeapPriorityQueue <AdjacencyNode <T> > distanceQueue = new HeapPriorityQueue <AdjacencyNode <T> >();
foreach (AdjacencyNode <T> n in nodes)
{
// previous nodes are unknown at the start
PreviousNode.Add(n, null);
// distance is assumed to be the maximum possible value. Therefore it can be improved if we find a shorter one
distance.Add(n, int.MaxValue);
distanceQueue.Enqueue(n, int.MaxValue);
}
if (!distanceQueue.Contains(source))
{
throw new ArgumentException("The source is not part of the graph (nodes)");
}
/**
* Execute the Algorithm
*/
distance[Source] = 0;
distanceQueue.UpdatePriority(Source, 0);
while (!distanceQueue.IsEmpty())
{
// The nearest node is a node which has never been reached (otherwise its path would have been improved)
// This means all other nodes can also not be reached and our algorithm is finished...
if (distanceQueue.PeekPriority() == int.MaxValue)
{
break;
}
AdjacencyNode <T> nearestNode = distanceQueue.Dequeue();
// Check all neighbours that still need to be inspected
foreach (AdjacencyNode <T> neighbour in nearestNode.AdjacentNodes)
{
if (!distanceQueue.Contains(neighbour))
{
continue;
}
// calculate distance with the currently inspected neighbour
int neighbourDist = distance[nearestNode] + 1;
// set the neighbour as shortest if it is better than the currently known shortest distance
if (neighbourDist < distance[neighbour])
{
distance[neighbour] = neighbourDist;
distanceQueue.UpdatePriority(neighbour, neighbourDist);
PreviousNode[neighbour] = nearestNode;
}
}
}
}