public void TestAddingSameNodesLater()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
Node node3 = new Node(3);
Node node4 = new Node(4);
Node node5 = new Node(5);
HeapPriorityQueue<Node> queue = new HeapPriorityQueue<Node>(5);
Enqueue(queue, node2);
Enqueue(queue, node5);
Enqueue(queue, node1);
Enqueue(queue, node3);
Enqueue(queue, node4);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node2, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
Assert.AreEqual(node5, Dequeue(queue));
Enqueue(queue, node5);
Enqueue(queue, node3);
Enqueue(queue, node1);
Enqueue(queue, node2);
Enqueue(queue, node4);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node2, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
Assert.AreEqual(node5, Dequeue(queue));
}
public void BuildForComment(Guid commentId, int? limit, int? maxDepth, int context = 0, bool continueThread = true, bool loadMore = true)
{
Clear();
if (context < 0)
context = 0;
var candidates = new HeapPriorityQueue<CommentQueue>(_tree.CommentIds.Count);
var currentComment = (Guid?)commentId;
var path = new List<Guid>();
while (currentComment.HasValue && path.Count <= context)
{
path.Add(currentComment.Value);
currentComment = _tree.Parents[currentComment.Value];
}
foreach (var comment in path)
{
var parent = _tree.Parents[comment];
_tree.Tree[parent ?? Guid.Empty] = new List<Guid> {comment};
}
_dontCollapse.AddRange(path);
}
// Initial state, reduces matrix
public State(City[] cities)
{
queue = new HeapPriorityQueue<State>(10000);
root = this;
Watch = new Stopwatch();
Watch.Start();
State.cities = cities;
cityCount = cities.Length;
matrix = new double[cities.Length, cities.Length];
for (int i = 0; i < cities.Length; i++)
{
for (int j = 0; j < cities.Length; j++)
{
matrix[i, j] = cities[i].costToGetTo(cities[j]);
// Set diagonals to infinity (negative)
if (i == j)
{
matrix[i, j] = double.PositiveInfinity;
}
}
}
bound = 0;
route = new ArrayList();
visited = new ArrayList();
pathsLeft = cityCount;
setInitialBSSF();
reduceMatrix();
// Start expanding until agenda is empty, time is up, or BSSF cost is equal to original LB.
Expand();
}
public void HeapPriorityQueueEnqueTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
queue.Enqueue (5, 1);
queue.Enqueue (6, 2);
queue.Enqueue (6, 2);
queue.Enqueue (7, 2);
queue.Enqueue (7, 2);
queue.Enqueue (7, 2);
Assert.AreEqual (6, queue.Count);
}
public Vector2[] FindPath(int start_x, int start_y, int end_x, int end_y)
{
if(start_x < 0 || start_x >= m_Width) return null;
if(start_y < 0 || start_y >= m_Height) return null;
if(start_x == end_x && start_y == end_y) return null;
ResetMap();
HeapPriorityQueue<MapNode> openQ = new HeapPriorityQueue<MapNode>(m_Height * m_Width); //Probably way beyond worst-case size...
//int x = start_x, y = start_y;
bool found = false;
if (!m_Map[start_x, start_y].passable || !m_Map[end_x, end_y].passable) return null;
m_Map[start_x, start_y].g = 0;
m_Map[start_x, start_y].count = 0;
m_Map[start_x, start_y].h = Heuristic(start_x, start_y, end_x, end_y);
m_Map[start_x, start_y].f = m_Map[start_x, start_y].h;
openQ.Enqueue(m_Map[start_x, start_y], 0);
do
{
MapNode m = openQ.Dequeue();
m.closed = true;
if(m.pos.x == end_x && m.pos.y == end_y)
{
found = true;
}
else
{
ProcessNode(openQ, m, end_x, end_y);
}
} while(!found && openQ.Count > 0);
if(found)
{
Vector2[] rv = new Vector2[m_Map[end_x, end_y].count]; //would be g+1 if we needed to return the first node, but we don't obv.
int x = end_x, y = end_y;
for(int i = m_Map[end_x, end_y].count-1; i >=0; i--)
{
MapNode m = m_Map[x,y];
rv[i].x = m.pos.x;
rv[i].y = m.pos.y;
x = m.previous.x;
y = m.previous.y;
}
return rv;
}
else
{
return null;
}
}
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>());
}
public void HeapPriorityQueueDequeTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
queue.Enqueue (5, 1);
queue.Enqueue (6, 5);
queue.Enqueue (6, 5);
queue.Enqueue (7, 6);
queue.Enqueue (7, 7);
queue.Enqueue (7, 2);
int first = queue.Dequeue ();
int second = queue.Dequeue ();
Assert.AreEqual (5, first);
Assert.AreEqual (7, second);
Assert.AreEqual(4, queue.Count);
}
public QSearchResult AStar(QState startState, bool output = false, int maxQueue = 1000)
{
HashSet<QState> explored = new HashSet<QState>();
Dictionary<QState, decimal> bestSoFar = new Dictionary<QState, decimal>() { { startState, 0 } };
HeapPriorityQueue<QStateContainer> toDo = new HeapPriorityQueue<QStateContainer>(maxQueue);
toDo.Enqueue(new QStateContainer(startState), 0);
Dictionary<QState, QSearchResult> pathTo = new Dictionary<QState, QSearchResult>() { { startState, new QSearchResult() } };
if (output) WriteOutput("Searching for shortest path via A-Star Search...");
int steps = 0;
while (toDo.Count > 0 && isRunning)
{
steps++;
QState current = toDo.Dequeue().qstate;
if (current.IsEnd())
{
if (output) WriteOutput("Shortest path of " + pathTo[current].Count + " step(s) found after " + steps + " iteration(s).");
return pathTo[current];
}
else
{
explored.Add(current);
foreach (QAction action in current.GetActions())
{
QState newState = current.GetNewState(action);
if (!explored.Contains(newState))
{
decimal actualCost = bestSoFar[current] - current.GetValue();
if (!bestSoFar.ContainsKey(newState) || actualCost < bestSoFar[newState])
{
pathTo[newState] = new QSearchResult(pathTo[current]);
pathTo[newState].actionsList.Add(action);
pathTo[newState].QStatesList.Add(newState);
bestSoFar[newState] = actualCost;
toDo.Enqueue(new QStateContainer(newState), bestSoFar[newState] - 1 * newState.GetValueHeuristic());
}
}
}
}
}
if (output) WriteOutput("No path found after " + steps + " iteration(s).");
return null;
}
public void HeapPriorityQueueEmptyItTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
queue.Enqueue (5, 1);
queue.Enqueue (6, 2);
queue.Enqueue (6, 3);
queue.Enqueue (7, 4);
queue.Enqueue (8, 5);
queue.Enqueue (9, 6);
queue.Dequeue();
queue.Dequeue();
queue.Dequeue();
queue.Dequeue();
queue.Dequeue();
Assert.AreEqual(9, queue.Dequeue());
Assert.AreEqual (0, queue.Count);
}
public void TestBackwardOrder()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
Node node3 = new Node(3);
Node node4 = new Node(4);
Node node5 = new Node(5);
HeapPriorityQueue<Node, int> queue = new HeapPriorityQueue<Node, int>(5);
Enqueue(queue, node5);
Enqueue(queue, node4);
Enqueue(queue, node3);
Enqueue(queue, node2);
Enqueue(queue, node1);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node2, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
Assert.AreEqual(node5, Dequeue(queue));
}
public void TestAddingDifferentNodesLater()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
Node node3 = new Node(3);
Node node4 = new Node(4);
Node node5 = new Node(5);
HeapPriorityQueue<Node> queue = new HeapPriorityQueue<Node>(5);
Enqueue(queue, node2);
Enqueue(queue, node5);
Enqueue(queue, node1);
Enqueue(queue, node3);
Enqueue(queue, node4);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node2, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
Assert.AreEqual(node5, Dequeue(queue));
Node node6 = new Node(6);
Node node7 = new Node(7);
Node node8 = new Node(8);
Node node9 = new Node(9);
Node node10 = new Node(10);
Enqueue(queue, node6);
Enqueue(queue, node7);
Enqueue(queue, node8);
Enqueue(queue, node10);
Enqueue(queue, node9);
Assert.AreEqual(node6, Dequeue(queue));
Assert.AreEqual(node7, Dequeue(queue));
Assert.AreEqual(node8, Dequeue(queue));
Assert.AreEqual(node9, Dequeue(queue));
Assert.AreEqual(node10, Dequeue(queue));
}
static void Main(string[] args)
{
//First, we create the priority queue. We'll specify a max of 10 users in the queue
HeapPriorityQueue<User> priorityQueue = new HeapPriorityQueue<User>(MAX_USERS_IN_QUEUE);
//Next, we'll create 5 users to enqueue
User user1 = new User("1 - Jason");
User user2 = new User("2 - Tyler");
User user3 = new User("3 - Valerie");
User user4 = new User("4 - Joseph");
User user42 = new User("4 - Ryan");
//Now, let's add them all to the queue (in some arbitrary order)!
priorityQueue.Enqueue(user4, 4);
priorityQueue.Enqueue(user2, 0); //Note: Priority = 0 right now!
priorityQueue.Enqueue(user1, 1);
priorityQueue.Enqueue(user42, 4);
priorityQueue.Enqueue(user3, 3);
//Change user2's priority to 2. Since user2 is already in the priority queue, we call UpdatePriority() to do this
priorityQueue.UpdatePriority(user2, 2);
//Finally, we'll dequeue all the users and print out their names
while(priorityQueue.Count != 0)
{
User nextUser = priorityQueue.Dequeue();
Console.WriteLine(nextUser.Name);
}
Console.ReadKey();
//Output:
//1 - Jason
//2 - Tyler
//3 - Valerie
//4 - Joseph
//4 - Ryan
//Notice that when two users with the same priority were enqueued, they were dequeued in the same order that they were enqueued.
}
/**
* Uses a breadth-first-search to calculate the distance between a given Detective and Mr X.
* Only considers the absolute distance between the Detective and Mr. X. Ticket amounts are
* not accounted for.
*/
public static int calculateValue(GamePosition game, Player player)
{
int mrXId = 0;
var nodes = new HashSet<Node>(game.Board.Values);
foreach(Node node in nodes){
node.Color = Color.white;
node.Value = Int32.MaxValue;
node.Parent = null;
}
player.getLocation().Color = Color.gray;
player.getLocation().Value = 0;
player.getLocation().Parent = null;
int MAX_NUMBER_OF_NODES = 200;
HeapPriorityQueue<Node> pq = new HeapPriorityQueue<Node>(MAX_NUMBER_OF_NODES);
Node playerLocation = player.getLocation();
pq.Enqueue(playerLocation, playerLocation.Value);
while(pq.First != null){
Node u = pq.Dequeue();
foreach(Node v in u.getAllEdges()){
if(v.Color == Color.white){
v.Color = Color.gray;
v.Value = u.Value + 1;
v.Parent = u;
if(v == game.Players[mrXId].getLocation()){
return v.Value;
}
pq.Enqueue(v, v.Value);
}
}
u.Color = Color.black;
}
throw new Exception ("calculate value error!!!!!!!");
//Not all code paths return a value, should the following line be here?
return MAX_NUMBER_OF_NODES;
}
public void TestPriorityQueue()
{
int[] queueTestOrder = { 10, 3, 11, 6, -3, 17, 13, -6, 2, 8, -2, -8 };
int nodeCount = 0;
for (int i = 0; i < queueTestOrder.Length; i++)
nodeCount = Math.Max(queueTestOrder[i] + 1, nodeCount);
HeapPriorityQueue<TestNode> queue = new HeapPriorityQueue<TestNode>(nodeCount);
TestNode[] testNodes = new TestNode[nodeCount];
for (int i = 0; i < nodeCount; i++)
testNodes[i] = new TestNode();
for (int i = 0; i < queueTestOrder.Length; i++)
{
int t = queueTestOrder[i];
if (t > 0)
queue.Enqueue(testNodes[t], t);
if (t < 0)
Assert.IsTrue(queue.Dequeue().Priority == -t);
}
}
// 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;
}
/// <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;
}
}
}
}
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);
}
}
}
void ProcessNode(HeapPriorityQueue<MapNode> openQ, MapNode m, int end_x, int end_y)
{
bool L = IsAccessible(m.pos.x+1, m.pos.y);
bool R = IsAccessible(m.pos.x-1, m.pos.y);
bool T = IsAccessible(m.pos.x, m.pos.y+1);
bool B = IsAccessible(m.pos.x, m.pos.y-1);
bool TL = T && L && IsAccessible(m.pos.x+1, m.pos.y+1);
bool TR = T && R && IsAccessible(m.pos.x-1, m.pos.y+1);
bool BL = B && L && IsAccessible(m.pos.x+1, m.pos.y-1);
bool BR = B && R && IsAccessible(m.pos.x-1, m.pos.y-1);
if(L) SetVals(openQ, m.pos.x+1, m.pos.y, m.g, 10, m.count, m.pos, end_x, end_y);
if(R) SetVals(openQ, m.pos.x-1, m.pos.y, m.g, 10, m.count, m.pos, end_x, end_y);
if(T) SetVals(openQ, m.pos.x, m.pos.y+1, m.g, 10, m.count, m.pos, end_x, end_y);
if(B) SetVals(openQ, m.pos.x, m.pos.y-1, m.g, 10, m.count, m.pos, end_x, end_y);
if(TL) SetVals(openQ, m.pos.x+1, m.pos.y+1, m.g, 14, m.count, m.pos, end_x, end_y);
if(TR) SetVals(openQ, m.pos.x-1, m.pos.y+1, m.g, 14, m.count, m.pos, end_x, end_y);
if(BL) SetVals(openQ, m.pos.x+1, m.pos.y-1, m.g, 14, m.count, m.pos, end_x, end_y);
if(BR) SetVals(openQ, m.pos.x-1, m.pos.y-1, m.g, 14, m.count, m.pos, end_x, end_y);
}
private IEnumerable<Node> getNeighbourghNodes(Node source, HeapPriorityQueue<Node> queue)
{
List<Node> nodeList = source.Ruter.ConvertAll(input => input.To);
return nodeList;
}
public void TestMoreComplicatedQueue()
{
Node node11 = new Node(1);
Node node12 = new Node(1);
Node node13 = new Node(1);
Node node14 = new Node(1);
Node node15 = new Node(1);
Node node21 = new Node(2);
Node node22 = new Node(2);
Node node23 = new Node(2);
Node node24 = new Node(2);
Node node25 = new Node(2);
Node node31 = new Node(3);
Node node32 = new Node(3);
Node node33 = new Node(3);
Node node34 = new Node(3);
Node node35 = new Node(3);
Node node41 = new Node(4);
Node node42 = new Node(4);
Node node43 = new Node(4);
Node node44 = new Node(4);
Node node45 = new Node(4);
Node node51 = new Node(5);
Node node52 = new Node(5);
Node node53 = new Node(5);
Node node54 = new Node(5);
Node node55 = new Node(5);
HeapPriorityQueue<Node> queue = new HeapPriorityQueue<Node>(25);
Enqueue(queue, node31);
Enqueue(queue, node51);
Enqueue(queue, node52);
Enqueue(queue, node11);
Enqueue(queue, node21);
Enqueue(queue, node22);
Enqueue(queue, node53);
Enqueue(queue, node41);
Enqueue(queue, node12);
Enqueue(queue, node32);
Enqueue(queue, node13);
Enqueue(queue, node42);
Enqueue(queue, node43);
Enqueue(queue, node44);
Enqueue(queue, node45);
Enqueue(queue, node54);
Enqueue(queue, node14);
Enqueue(queue, node23);
Enqueue(queue, node24);
Enqueue(queue, node33);
Enqueue(queue, node34);
Enqueue(queue, node55);
Enqueue(queue, node35);
Enqueue(queue, node25);
Enqueue(queue, node15);
Assert.AreEqual(node11, Dequeue(queue));
Assert.AreEqual(node12, Dequeue(queue));
Assert.AreEqual(node13, Dequeue(queue));
Assert.AreEqual(node14, Dequeue(queue));
Assert.AreEqual(node15, Dequeue(queue));
Assert.AreEqual(node21, Dequeue(queue));
Assert.AreEqual(node22, Dequeue(queue));
Assert.AreEqual(node23, Dequeue(queue));
Assert.AreEqual(node24, Dequeue(queue));
Assert.AreEqual(node25, Dequeue(queue));
Assert.AreEqual(node31, Dequeue(queue));
Assert.AreEqual(node32, Dequeue(queue));
Assert.AreEqual(node33, Dequeue(queue));
Assert.AreEqual(node34, Dequeue(queue));
Assert.AreEqual(node35, Dequeue(queue));
Assert.AreEqual(node41, Dequeue(queue));
Assert.AreEqual(node42, Dequeue(queue));
Assert.AreEqual(node43, Dequeue(queue));
Assert.AreEqual(node44, Dequeue(queue));
Assert.AreEqual(node45, Dequeue(queue));
Assert.AreEqual(node51, Dequeue(queue));
Assert.AreEqual(node52, Dequeue(queue));
Assert.AreEqual(node53, Dequeue(queue));
Assert.AreEqual(node54, Dequeue(queue));
Assert.AreEqual(node55, Dequeue(queue));
}
public void TestClear()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
Node node3 = new Node(3);
Node node4 = new Node(4);
Node node5 = new Node(5);
HeapPriorityQueue<Node> queue = new HeapPriorityQueue<Node>(5);
Enqueue(queue, node2);
Enqueue(queue, node5);
queue.Clear();
Enqueue(queue, node1);
Enqueue(queue, node3);
Enqueue(queue, node4);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
}
void Start()
{
money = startingMoney;
costs = new HeapPriorityQueue <CostNode>(maxCostNodes);
}
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);
}
private Node Dequeue(HeapPriorityQueue<Node> queue)
{
Node returnMe = queue.Dequeue();
Assert.IsTrue(queue.IsValidQueue());
return returnMe;
}
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;
}
void Awake(){
if(GameObject.Find("LobbyGUI") != null){
sessionManager = SessionManager.Instance;
FindPlatforms();
beginTime = float.PositiveInfinity;
playerSpawnVectors = new List<Vector3>();
foreach(Transform location in spawnPositions){
playerSpawnVectors.Add(location.position);
}
hudTools = GetComponent<HUDTools>();
playersReady = new List<NetworkPlayer>();
allTimedSpawns = new HeapPriorityQueue<PowerSpawn>(30);
powerPrefabs = GetComponent<PowerPrefabs>();
GameObject placementRoot = Instantiate(placementRootPrefab,
placementRootPrefab.transform.position, Quaternion.identity) as GameObject;
placementUI = placementRoot.GetComponent<PlacementUI>();
placementUI.Initialize(powerPrefabs);
placementUI.SwitchToLive(false);
placementUI.Enable();
}
}
// Method fills ValidMoves list with all cells that the unit is capable of moving to
// Uses slightly modified Dijkstra's Algorithm
public void GetAvailableDestinations(Cell origin, float movePoints)
{
ValidConnections.Clear(); // Empty ValidMoves and recalculate in the following
var frontier = new HeapPriorityQueue <Cell>();
frontier.Enqueue(origin, 0);
var cameFrom = new Dictionary <Cell, Cell>();
cameFrom[origin] = null;
var costSoFar = new Dictionary <Cell, float>();
costSoFar[origin] = 0;
while (frontier.Count > 0)
{
Cell current = frontier.Dequeue();
foreach (var next in current.Neighbours)
{
// Don't add if can't be reached
if (!CanReach(current, next))
{
continue;
}
// Don't add if cost exceeds movement points
float newCost;
float movementCost = 0f;
// If the unit shares OccupyingSpace (ground, air, etc) with the cell
if (SharesSpace(next))
{
foreach (Cell cell in new List <Cell>(GetGridSizeCells(next)))
{
if (cell.MovementCost > movementCost)
{
movementCost = cell.MovementCost;
}
}
newCost = costSoFar[current] + (movementCost * current.GetDistance(next));
}
else // Otherwise the unit isn't effected by movement cost of the cell
{
newCost = costSoFar[current] + current.GetDistance(next);
}
if (newCost > MovementPoints)
{
continue;
}
if (!costSoFar.Keys.Contains(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
frontier.Enqueue(next, newCost);
cameFrom[next] = current;
}
}
}
ValidConnections = cameFrom;
}
public void TestOrderedQueue()
{
Node node1 = new Node(1);
Node node2 = new Node(1);
Node node3 = new Node(1);
Node node4 = new Node(1);
Node node5 = new Node(1);
HeapPriorityQueue<Node> queue = new HeapPriorityQueue<Node>(5);
Enqueue(queue, node1);
Enqueue(queue, node2);
Enqueue(queue, node3);
Enqueue(queue, node4);
Enqueue(queue, node5);
Assert.AreEqual(node1, Dequeue(queue));
Assert.AreEqual(node2, Dequeue(queue));
Assert.AreEqual(node3, Dequeue(queue));
Assert.AreEqual(node4, Dequeue(queue));
Assert.AreEqual(node5, Dequeue(queue));
}
/// <summary>
/// Setup all the necessary lists for the algorithm to work
/// </summary>
private void setup()
{
//initialize SLAV
SLAV = new List<CircularLinkedList<Vertex>>();
//initialize priority queue Q
Q = new HeapPriorityQueue<Event>(500);
//initialize first LAV
CircularLinkedList<Vertex> LAV = new CircularLinkedList<Vertex>();
for (int i = 0; i < polygon.ControlVertices.Count; i++)
{
//Create vertex for each controlpoint and store edges + calc bisector ray
Vertex vertex = new Vertex(polygon.ControlVertices[i], id++);
vertex.prevEdge = new Edge(polygon.ControlVertices.PreviousVertex(i), vertex.getPoint());
vertex.nextEdge = new Edge(vertex.getPoint(), polygon.ControlVertices.NextVertex(i));
vertex.update();
//Add them to initial LAV
LAV.AddLast(vertex);
}
SLAV.Add(LAV);
//initial event creation
testDots = new List<OwnVector2>();
for(int i = 0; i < LAV.Count; i++)
{
Vertex prev = LAV[i].Previous.Value;
Vertex current = LAV[i].Value;
Vertex next = LAV[i].Next.Value;
findClosestIntersectionAndStore(LAV, prev, current, next);
}
}
private void Enqueue(HeapPriorityQueue<Node> queue, Node node)
{
queue.Enqueue(node, node.Priority);
Assert.IsTrue(queue.IsValidQueue());
}
void Awake(){
sessionManager = SessionManager.Instance;
sessionManager.psInfo.LevelReset();
playerSpawnVectors = new List<Vector3>();
foreach(Transform location in spawnPositions){
playerSpawnVectors.Add(location.position);
}
hudTools = GetComponent<HUDTools>();
playersReady = new List<NetworkPlayer>();
allTimedSpawns = new HeapPriorityQueue<PowerSpawn>(30);
powerPrefabs = GetComponent<PowerPrefabs>();
GameObject placementRoot = Instantiate(placementRootPrefab,
placementRootPrefab.transform.position, Quaternion.identity) as GameObject;
placementUI = placementRoot.GetComponent<PlacementUI>();
timer = GameObject.Find("timer").GetComponent<Timer>();
timer.Hide();
ScoreUI scoreUI = placementRoot.GetComponent<ScoreUI>();
scoreUI.Initialize(sessionManager.psInfo);
livesUI = placementRoot.GetComponent<LivesUI>();
livesUI.Initialize(sessionManager.psInfo, sessionManager.psInfo.livesPerRound);
pointTracker = GetComponent<PointTracker>();
pointTracker.Initialize(scoreUI);
}
void Start()
{
money = startingMoney;
costs = new HeapPriorityQueue<CostNode>(maxCostNodes);
}
public static IEnumerable <PathNode> SelectAstarPath(PathNode startNode, PathNode endNode, PathNode[,] pathNodes)
{
if (startNode != endNode)
{
HeapPriorityQueue <PathNode> frontier = new HeapPriorityQueue <PathNode>(PathNode.ConnectedNodes.Keys.Count);
frontier.Enqueue(startNode, 0);
Dictionary <PathNode, PathNode> cameFrom = new Dictionary <PathNode, PathNode>();
Dictionary <PathNode, float> costSoFar = new Dictionary <PathNode, float>();
cameFrom.Add(startNode, null);
costSoFar.Add(startNode, 0);
while (frontier.Count != 0)
{
PathNode current = frontier.Dequeue();
if (current == endNode)
{
break;
}
//Console.WriteLine("Processing Node " + current.X + " " + current.Y);
foreach (var next in PathNode.ConnectedNodes[current])
{
if (!next.Item1.Enabled)
{
continue;
}
if (cameFrom.ContainsKey(next.Item1))
{
continue;
}
float newCost = costSoFar[current] + next.Item2;
if (!costSoFar.ContainsKey(next.Item1) || newCost < costSoFar[next.Item1])
{
costSoFar[next.Item1] = newCost;
float priority = newCost + CalculateHeuristic(next.Item1.X, next.Item1.Y, endNode.X, endNode.Y);
frontier.Enqueue(next.Item1, priority);
cameFrom[next.Item1] = current;
}
}
}
//Console.WriteLine("Done Next up building");
var result = new List <PathNode>();
PathNode currentNode = endNode;
do
{
//Console.WriteLine("Processing Node2 " + currentNode.X + " " + currentNode.Y);
PathNode next = null;
if (!cameFrom.TryGetValue(currentNode, out next))
{
break;
}
yield return(currentNode = next);
} while (startNode != currentNode);
}
else
{
yield return(startNode);
}
}
