public void Build()
{
nodes = new HeapPriorityQueue <Node>(Frequencies.Count);
foreach (KeyValuePair <string, int> symbol in Frequencies)
{
nodes.Enqueue(new Node()
{
Symbol = symbol.Key, Frequency = symbol.Value
}, symbol.Value);
}
while (nodes.Count > 1)
{
if (nodes.Count >= 2)
{
// Take first two items
// List<Node> taken = orderedNodes.Take(2).ToList<Node>();
Node a = nodes.Dequeue();
Node b = nodes.Dequeue();
// Create a parent node by combining the frequencies
Node parent = new Node()
{
Symbol = "*",
Frequency = a.Frequency + b.Frequency,
Left = a,
Right = b
};
nodes.Enqueue(parent, parent.Frequency);
}
}
this.Root = nodes.Dequeue();
}
public void TestPriorityQueueOrder()
{
int SIZE = 10;
HeapPriorityQueue <Node> q = new HeapPriorityQueue <Node>(SIZE);
q.Enqueue(new Node("a"), 0);
q.Enqueue(new Node("b"), 1);
Assert.AreEqual("a", q.Dequeue().Str);
Assert.AreEqual("b", q.Dequeue().Str);
}
public void TestEmptyHeapQueue()
{
PriorityQueue <double, string> queue = new HeapPriorityQueue <double, string>();
queue.Enqueue("1", 1);
queue.Enqueue("2", 2);
Assert.AreEqual(queue.Dequeue(), "2");
Assert.AreEqual(queue.Dequeue(), "1");
Assert.Throws <InvalidOperationException>(() => queue.Dequeue());
queue.Enqueue("3", 3);
Assert.AreEqual(queue.Dequeue(), "3");
}
private void RemoveOperation(OperationContext operationContext)
{
lock (queuedOperations)
{
// Lock to prevent operation from being run while trying to remove it
if (!queuedOperations.Contains(operationContext))
{
throw new ArgumentOutOfRangeException(nameof(operationContext));
}
queuedOperations.Dequeue();
}
}
public void Test3()
{
HeapPriorityQueue priorityQueue = new HeapPriorityQueue();
priorityQueue.Enqueue(2, () => { return("Item with priority 2"); });
priorityQueue.Enqueue(4, () => { return("Item with priority 4"); });
priorityQueue.Enqueue(3, () => { return("Item with priority 3"); });
priorityQueue.Enqueue(1, () => { return("Item with priority 1"); });
Assert.AreEqual(priorityQueue.Dequeue().Invoke(), "Item with priority 1");
Assert.AreEqual(priorityQueue.Dequeue().Invoke(), "Item with priority 2");
Assert.AreEqual(priorityQueue.Dequeue().Invoke(), "Item with priority 3");
Assert.AreEqual(priorityQueue.Dequeue().Invoke(), "Item with priority 4");
}
public void TestSamePrioritiesHeapQueue()
{
PriorityQueue <int, string> queue = new HeapPriorityQueue <int, string>();
int[] keys = { 1, 3, 5, 3, 4, 3, 2, 4 };
string[] vals = { "1", "3a", "5", "3b", "4a", "3c", "2", "4b" };
for (int i = 0; i < keys.Length; i++)
{
queue.Enqueue(vals[i], keys[i]);
}
Assert.AreEqual(queue.Dequeue(), "5");
string first4 = queue.Dequeue();
Assert.IsTrue(first4.StartsWith("4"));
string second4 = queue.Dequeue();
Assert.IsTrue(second4.StartsWith("4"));
Assert.AreNotEqual(first4, second4);
string first3 = queue.Dequeue();
Assert.IsTrue(first3.StartsWith("3"));
string second3 = queue.Dequeue();
Assert.IsTrue(second3.StartsWith("3"));
string third3 = queue.Dequeue();
Assert.IsTrue(third3.StartsWith("3"));
Assert.AreEqual(queue.Dequeue(), "2");
Assert.AreEqual(queue.Dequeue(), "1");
}
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);
}
private Node Dequeue(HeapPriorityQueue <Node> queue)
{
Node returnMe = queue.Dequeue();
Assert.IsTrue(queue.IsValidQueue());
return(returnMe);
}
public Stack <IntPoint2D> ComputePath(IntPoint2D start, IntPoint2D end, bool openTravel)
{
//Debug.Log ("Computing path from "+start.ToString()+ " to " + end.ToString());
HashSet <IntPoint2D> closed = new HashSet <IntPoint2D> ();
closed.Add(start);
int queueSize = (this.gridHeight * this.gridWidth) * 16;
HeapPriorityQueue <SearchNode> searchList = new HeapPriorityQueue <SearchNode> (queueSize);
Stack <IntPoint2D> path = new Stack <IntPoint2D> ();
SearchNode root = new SearchNode(null, start, 0, Math.Abs(start.xCoord - end.xCoord) + Math.Abs(start.yCoord - end.yCoord));
searchList.Enqueue(root, root.Priority);
SearchNode curNode = null;
bool found = false;
List <SearchNode> children;
while (!found && searchList.Count > 0)
{
curNode = searchList.Dequeue();
//Debug.Log("checking node: "+curNode.data.ToString());
if (curNode.AtGoal(end))
{
found = true;
}
else
{
children = curNode.GetChildren(end, this, openTravel);
foreach (SearchNode child in children)
{
if (searchList.Count == queueSize)
{
Debug.Log("Priority queue size: " + queueSize.ToString() + " exceeded");
}
//Debug.Log("enqueueing node: "+child.data.ToString());
//Debug.Log (" Priority: "+child.Priority.ToString());
if (!closed.Contains(child.data))
{
searchList.Enqueue(child, child.cost + child.heuristic);
closed.Add(child.data);
}
}
}
}
if (found)
{
//Debug.Log ("pushing path");
while (curNode != null)
{
path.Push(curNode.data);
// Debug.Log(curNode.data.ToString());
curNode = curNode.parent;
}
path.Pop();
}
else
{
//Debug.Log ("no path found");
}
return(path);
}
/// <summary>
/// Creates a new A* Search
/// </summary>
public AStarSearch(Hexagon start, Hexagon end, Hexagon AIHex = null)
{
HeapPriorityQueue <Location> frontier = new HeapPriorityQueue <Location>(15); //TODO could cause issues if pathfinding more than 15 units
frontier.Enqueue(new Location(start), 0);
cameFrom.Add(start, start);
costSoFar.Add(start, 0);
while (frontier.Count > 0)
{
Location curr = frontier.Dequeue();
if (curr.hex == end)
{
FoundPath = true;
break;
}
foreach (Hexagon h in BoardManager.instance.GetNeighborsMovement(curr.hex, AIHex))
{
float newCost = costSoFar[curr.hex] + Cost(curr.hex, h);
if (!costSoFar.ContainsKey(h))
{
costSoFar.Add(h, newCost);
float priority = newCost + Heuristic(h, end);
frontier.Enqueue(new Location(h), priority);
cameFrom.Add(h, curr.hex);
}
}
FoundPath = false;
}
}
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);
}
}
}
public void LoadChunks()
{
int xWidth = WorldConfig.Instance.extendChunkWidth * Chunk.chunkWidth;
int zWidth = WorldConfig.Instance.extendChunkWidth * Chunk.chunkDepth;
for (int x = -xWidth; x <= xWidth; x += Chunk.chunkWidth)
{
for (int z = -zWidth; z <= zWidth; z += Chunk.chunkDepth)
{
WorldPos chunkPos = new WorldPos(_curChunkPos.x + x, _curChunkPos.y, _curChunkPos.z + z);
if (!world.chunks.ContainsKey(chunkPos))
{
double dis = x * x + z * z;
if (dis <= loadPowWidth)
{
PriorityWorldPos priorityWorldPos = GetPriorityPos(chunkPos);
loadQueue.Enqueue(priorityWorldPos, priorityWorldPos.Priority);
}
}
}
}
while (loadQueue.Count > 0)
{
PriorityWorldPos priorityPos = loadQueue.Dequeue();
WorldPos pos = priorityPos.pos;
world.WorldGenerator.GenerateFromServer(pos.x, pos.y, pos.z);
}
}
public Path FindPath(Point origin, Point destination)
{
//Normally I dont use this like this. Point Connection should be cached to increase performance.
foreach (var point in grid.points.Values)
{
point.SetConnections();
}
//
var frontier = new HeapPriorityQueue <Point>();
var cameFrom = new Dictionary <Point, Point>();
var costSoFar = new Dictionary <Point, float>();
frontier.Enqueue(origin, 0);
cameFrom.Add(origin, default);
costSoFar.Add(origin, 0);
while (frontier.Count != 0)
{
var current = frontier.Dequeue();
if (current == destination)
{
break;
}
foreach (var next in current.Connections.Keys)
{
var cost = costSoFar[current] + current.Connections[next];
if (costSoFar.ContainsKey(next) && cost >= costSoFar[next])
{
continue;
}
costSoFar[next] = cost;
var priority = cost + GetEuclideanDistance(next, destination);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
var path = new Path();
if (!cameFrom.ContainsKey(destination))
{
return(path);
}
path.Add(destination);
var temp = destination;
while (temp != origin)
{
var waypoint = cameFrom[temp];
path.Add(waypoint);
temp = waypoint;
}
if (!path.Contains(origin))
{
path.Add(origin);
}
path.Reverse();
return(path);
}
private void CleanupQueue()
{
var oneHour = new TimeSpan(1, 0, 0);
var oneHourAgo = DateTime.UtcNow.Subtract(oneHour);
while (_queue.Count != 0 && _queue.First.Kill.KillTime < oneHourAgo)
{
_queue.Dequeue();
}
}
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 override List <T> FindPath <T>(Dictionary <T, Dictionary <T, float> > edges, T originNode, T destinationNode)
{
IPriorityQueue <T> frontier = new HeapPriorityQueue <T>();
frontier.Enqueue(originNode, 0);
Dictionary <T, T> cameFrom = new Dictionary <T, T>();
cameFrom.Add(originNode, default(T));
Dictionary <T, float> costSoFar = new Dictionary <T, float>();
costSoFar.Add(originNode, 0);
while (frontier.Count != 0)
{
var current = frontier.Dequeue();
if (current.Equals(destinationNode))
{
break;
}
var neighbours = GetNeigbours(edges, current);
foreach (var neighbour in neighbours)
{
var newCost = costSoFar[current] + edges[current][neighbour];
if (!costSoFar.ContainsKey(neighbour) || newCost < costSoFar[neighbour])
{
costSoFar[neighbour] = newCost;
cameFrom[neighbour] = current;
var priority = newCost + Heuristic(destinationNode, neighbour);
frontier.Enqueue(neighbour, priority);
}
}
}
List <T> path = new List <T>();
if (!cameFrom.ContainsKey(destinationNode))
{
return(path);
}
path.Add(destinationNode);
var temp = destinationNode;
while (!cameFrom[temp].Equals(originNode))
{
var currentPathElement = cameFrom[temp];
path.Add(currentPathElement);
temp = currentPathElement;
}
return(path);
}
public List <Field> FindPath(Field start, Field end)
{
HeapPriorityQueue <Field> openList = new HeapPriorityQueue <Field>((gameBoard.SizeX + 1) * (gameBoard.SizeY + 1));
ResetAGwiazdka();
start.G = 0;
start.H = 0;
start.Priority = 0;
openList.Enqueue(start, 0);
start.Opened = true;
while (openList.Count != 0)
{
var unit = openList.Dequeue();
unit.Closed = true;
if (unit == end)
{
var list = new List <Field>();
var node = unit;
list.Add(node);
while (node.Parent != null)
{
node = node.Parent;
list.Add(node);
}
list.RemoveAt(list.Count - 1);
return(list);
}
var positions = gameBoard.GetAdjacentPositionsForAStar(unit).ToList();
foreach (var position in positions)
{
if (!position.Closed)
{
var distance = unit.G + 1;
if (!position.Opened || distance < position.G)
{
position.G = distance;
position.H = DistanceFields(position, end);
position.Priority = position.G + position.H;
position.Parent = unit;
if (!position.Opened)
{
openList.Enqueue(position, position.Priority);
position.Opened = true;
}
else
{
openList.UpdatePriority(position, position.Priority);
}
}
}
}
}
return(null);
}
public List <SearchNode> getPath(SearchNode start, SearchNode end, scoringFunction func)
{
List <SearchNode> optimalPath = new List <SearchNode>();
HashSet <int> closeSet = new HashSet <int>();
HeapPriorityQueue <SearchNode> openSet = new HeapPriorityQueue <SearchNode>(80000);
Dictionary <int, double> gScore = new Dictionary <int, double>();
Dictionary <int, double> fScore = new Dictionary <int, double>();
Dictionary <int, SearchNode> nodes = new Dictionary <int, SearchNode>();
Dictionary <SearchNode, SearchNode> cameFrom = new Dictionary <SearchNode, SearchNode>();
gScore[start.GetHashCode()] = 0;
fScore[start.GetHashCode()] = start.dist(end);
openSet.Enqueue(start, 0);
while (openSet.Count > 0)
{
SearchNode current = openSet.Dequeue();
if (current == end)
{
optimalPath.Add(current);
while (cameFrom.ContainsKey(current))
{
current = cameFrom[current];
optimalPath.Add(current);
}
return(optimalPath);
}
closeSet.Add(current.GetHashCode());
foreach (var neighbor in GetNeighbors(current))
{
if (!closeSet.Contains(neighbor.GetHashCode()))
{
double tentativeGScore = gScore[current.GetHashCode()] + func(current, neighbor);
if (!gScore.ContainsKey(neighbor.GetHashCode()))
{
cameFrom[neighbor] = current;
gScore[neighbor.GetHashCode()] = tentativeGScore;
fScore[neighbor.GetHashCode()] = tentativeGScore + neighbor.dist(end);
openSet.Enqueue(neighbor, fScore[neighbor.GetHashCode()]);
}
else if (gScore[neighbor.GetHashCode()] > tentativeGScore)
{
cameFrom[neighbor] = current;
gScore[neighbor.GetHashCode()] = tentativeGScore;
fScore[neighbor.GetHashCode()] = tentativeGScore + neighbor.dist(end);
openSet.UpdatePriority(neighbor, fScore[neighbor.GetHashCode()]);
}
}
}
}
return(optimalPath);
}
public List <T> FindPath <T>(Dictionary <T, Dictionary <T, int> > edges, T origin, T destination) where T : Tile
{
HeapPriorityQueue <T> HPQueue = new HeapPriorityQueue <T>();
HPQueue.Enqueue(origin, 0);
Dictionary <T, T> cameFrom = new Dictionary <T, T>();
cameFrom.Add(origin, default(T));
Dictionary <T, int> costSoFar = new Dictionary <T, int>();
costSoFar.Add(origin, 0);
while (HPQueue.Count != 0)
{
T current = HPQueue.Dequeue();
if (current.Equals(destination))
{
break;
}
List <T> neighbours = GetNeigbours(edges, current);
foreach (T next in neighbours)
{
int newCost = costSoFar[current] + edges[current][next];
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
int priority = newCost + Heuristic(destination, next);
HPQueue.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
List <T> path = new List <T>();
if (!cameFrom.ContainsKey(destination))
{
return(path);
}
path.Add(destination);
T temp = destination;
while (!cameFrom[temp].Equals(origin))
{
T currentPathElement = cameFrom[temp];
path.Add(currentPathElement);
temp = currentPathElement;
}
return(path);
}
private NetPriorityChunk DeNetGeneratorQueue()
{
lock (_netGeneratorQueue)
{
if (_netGeneratorQueue.Count > 0)
{
return(_netGeneratorQueue.Dequeue());
}
return(null);
}
}
public NetPriorityChunk DeNetChunkLoadQueue()
{
lock (_netChunkLoadQueue)
{
if (_netChunkLoadQueue.Count > 0)
{
return(_netChunkLoadQueue.Dequeue());
}
return(null);
}
}
private PriorityChunk DeLoadQueue()
{
lock (_selfLoadQueue)
{
if (_selfLoadQueue.Count > 0)
{
PriorityChunk c = _selfLoadQueue.Dequeue();
return(c);
}
return(null);
}
}
protected Vector3 FindAvoidanceVector(Intersection next)
{
//0. Create all necessary data structures to hold potential directions and directions we don't need
PotentialDirection FutureDirection = new PotentialDirection(Vector3.zero);
bool bDirectionFound = false;
Vector3 vResultForce = Vector3.zero;
Intersection collisionInfo = next;
//1. loop until we find a direction (this does have the potential to loop forever)
while (!bDirectionFound)
{
//A. Based on the object we collided with, get the potential directions (assign heuristics in there)
//if (vFutureDirection != Vector3.zero)
//{
if (DOColliderType.Sphere == collisionInfo.Obstacle.ColliderShape)
{
GetAvoidanceVectorsForSphere(collisionInfo);
}
else if (DOColliderType.Box == collisionInfo.Obstacle.ColliderShape)
{
GetAvoidanceVectorsForBox(collisionInfo);
}
else if (DOColliderType.Capsule == collisionInfo.Obstacle.ColliderShape)
{
GetAvoidanceVectorsForCapsule(collisionInfo);
}
else if (DOColliderType.Mesh == collisionInfo.Obstacle.ColliderShape)
{
GetAvoidanceVectorsForMesh(collisionInfo);
}
else if (DOColliderType.Terrain == collisionInfo.Obstacle.ColliderShape)
{
GetAvoidanceVectorsForTerrain(collisionInfo);
}
//}
//B. Select direction and test
FutureDirection = m_pqPotentialDirections.Dequeue();
bDirectionFound = true;
if (m_pqPotentialDirections.Count <= 0)
{
break;
}
}
vResultForce = FutureDirection.Direction * Vehicle.MaxForce;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + vResultForce, Color.green);
#endif
return(vResultForce);
}
public void TestDebugDequeueThrowsOnCorruptedQueue()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
HeapPriorityQueue <Node> queue = new HeapPriorityQueue <Node>(3);
Enqueue(queue, node1);
Enqueue(queue, node2);
node1.Priority = 3; //Don't ever do this! (use queue.UpdatePriority(node1, 3) instead)
Assert.Throws <InvalidOperationException>(() => queue.Dequeue());
}
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 void TestDebugDequeueThrowsOnEmptyQueue2()
{
Node node1 = new Node(1);
Node node2 = new Node(2);
HeapPriorityQueue <Node> queue = new HeapPriorityQueue <Node>(3);
Enqueue(queue, node1);
Enqueue(queue, node2);
Dequeue(queue);
Dequeue(queue);
Assert.Throws <InvalidOperationException>(() => queue.Dequeue());
}
private Chunk DeGeneratorQueue()
{
lock (_selfGeneratorQueue)
{
if (_selfGeneratorQueue.Count > 0)
{
PriorityChunk c = _selfGeneratorQueue.Dequeue();
Chunk chunk = c.chunk;
_manager.SetPriorityChunk(c);
return(chunk);
}
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 Dictionary <FloorNode, List <FloorNode> > findAllPaths(Dictionary <FloorNode, Dictionary <FloorNode, int> > edges, FloorNode originNode, int m_Range)
{
IPriorityQueue <FloorNode> frontier = new HeapPriorityQueue <FloorNode>();
frontier.Enqueue(originNode, 0);
Dictionary <FloorNode, FloorNode> cameFrom = new Dictionary <FloorNode, FloorNode>();
cameFrom.Add(originNode, default(FloorNode));
Dictionary <FloorNode, int> costSoFar = new Dictionary <FloorNode, int>();
costSoFar.Add(originNode, 0);
while (frontier.Count != 0)
{
var current = frontier.Dequeue();
List <FloorNode> neighbours = GetNeigbours(edges, current);
foreach (FloorNode neighbour in neighbours)
{
int newCost = costSoFar[current] + edges[current][neighbour];
if (!costSoFar.ContainsKey(neighbour) || newCost < costSoFar[neighbour])
{
if (newCost > m_Range)
{
break;
}
costSoFar[neighbour] = newCost;
cameFrom[neighbour] = current;
frontier.Enqueue(neighbour, newCost);
}
}
}
Dictionary <FloorNode, List <FloorNode> > paths = new Dictionary <FloorNode, List <FloorNode> >();
foreach (FloorNode destination in cameFrom.Keys)
{
List <FloorNode> path = new List <FloorNode>();
var current = destination;
while (!current.Equals(originNode))
{
path.Add(current);
current = cameFrom[current];
}
paths.Add(destination, path);
}
return(paths);
}
/*
* Update costs by going through costs items, retrieving minPriority (lowest time)
* and checking if that time has arrived yet.
* If so, dequeue, subtract hourly cost, and put back in queue
*/
void UpdateCost()
{
if (costs.Count > 0)
{
if (costs.First.TimeInMinutes <= TimeController.TimeInMinutes)
{
// dequeue and subtract cost
CostNode costNode = costs.Dequeue();
money -= costNode.CostPerHour;
// reset time in minutes for next hour and enqueue
costNode.TimeInMinutes = TimeController.TimeInMinutes + 60;
costs.Enqueue(costNode, costNode.TimeInMinutes);
}
}
}
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 Dictionary <Coord, List <Coord> > findAllPaths(Dictionary <Coord, Dictionary <Coord, int> > edges, Coord originNode)
{
IPriorityQueue <Coord> frontier = new HeapPriorityQueue <Coord>();
frontier.Enqueue(originNode, 0);
Dictionary <Coord, Coord> cameFrom = new Dictionary <Coord, Coord>();
cameFrom.Add(originNode, default(Coord));
Dictionary <Coord, int> costSoFar = new Dictionary <Coord, int>();
costSoFar.Add(originNode, 0);
while (frontier.Count != 0)
{
var current = frontier.Dequeue();
var neighbours = GetNeigbours(edges, current);
foreach (var neighbour in neighbours)
{
var newCost = costSoFar[current] + edges[current][neighbour];
if (!costSoFar.ContainsKey(neighbour) || newCost < costSoFar[neighbour])
{
costSoFar[neighbour] = newCost;
cameFrom[neighbour] = current;
frontier.Enqueue(neighbour, newCost);
}
}
}
Dictionary <Coord, List <Coord> > paths = new Dictionary <Coord, List <Coord> >();
foreach (Coord destination in cameFrom.Keys)
{
List <Coord> path = new List <Coord>();
var current = destination;
while (!current.Equals(originNode))
{
path.Add(current);
current = cameFrom[current];
}
paths.Add(destination, path);
}
return(paths);
}
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 void BuildReachability()
{
for (UInt32 pIndex = 0; pIndex < _points.Length; pIndex++)
{
if (_points[pIndex].WasProcessed)//点已经输出到结果队列
{
continue;
}
List <PointRelation> neighborOfPoint = new List <PointRelation>();
GetNeighborhood(pIndex, neighborOfPoint);//获取当前点的所有直接密度可达样本点
_points[pIndex].WasProcessed = true;
AddOutputIndex(pIndex); //将当前点放入结果队列中
double coreDistance = CoreDistance(neighborOfPoint); //计算当前点的核心距离
if (!double.IsNaN(coreDistance)) //当前点满足核心对象条件
{
_seeds.Clear();
Update(pIndex, neighborOfPoint, coreDistance);
List <PointRelation> neighborInner = new List <PointRelation>();
while (_seeds.Count > 0)//执行至顺序队列为空
{
UInt32 pInnerIndex = _seeds.Dequeue().Index;
GetNeighborhood(pInnerIndex, neighborInner);
_points[pInnerIndex].WasProcessed = true;
AddOutputIndex(pInnerIndex);
double coreDistanceInner = CoreDistance(neighborInner);
if (!double.IsNaN(coreDistanceInner))
{
Update(pInnerIndex, neighborInner, coreDistanceInner);
}
}
}
}
}
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 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;
}
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 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);
}
/// <summary>
/// Uses Prim's algorithm to build an MST spanning the mstNodes.
/// </summary>
/// <param name="startFrom">A GraphNode to start from.</param>
/// <returns>A list of GraphEdges forming the MST.</returns>
public List<GraphEdge> Span(GraphNode startFrom)
{
/// With n nodes, we can have up to n (actually n-1) edges adjacent to each node.
HeapPriorityQueue<GraphEdge> adjacentEdgeQueue = new HeapPriorityQueue<GraphEdge>(mstNodes.Count * mstNodes.Count);
/// Removing all edges that satisfy a property (here a certain "outside"
/// node) from the queue is not actually trivial, since you could only
/// iterate over all entries (and you want to avoid that) if you don't
/// have the references to the edges at hand.
/// I guess this is the easiest way to do it...
Dictionary<GraphNode, List<GraphEdge>> edgesLeadingToNode =
new Dictionary<GraphNode, List<GraphEdge>>();
foreach (GraphNode node in mstNodes)
edgesLeadingToNode[node] = new List<GraphEdge>();
// All nodes that are already included.
HashSet<GraphNode> inMst = new HashSet<GraphNode>();
// All nodes that are not yet included.
HashSet<GraphNode> toAdd = new HashSet<GraphNode>(mstNodes);
List<GraphEdge> mstEdges = new List<GraphEdge>();
// Initialize the MST with the start nodes.
inMst.Add(startFrom);
toAdd.Remove(startFrom);
edgesLeadingToNode[startFrom] = new List<GraphEdge>();
foreach (GraphNode otherNode in toAdd)
{
GraphEdge adjacentEdge = new GraphEdge(startFrom, otherNode);
adjacentEdgeQueue.Enqueue(adjacentEdge, distances.GetDistance(adjacentEdge));
edgesLeadingToNode[otherNode].Add(adjacentEdge);
}
while (toAdd.Count > 0 && adjacentEdgeQueue.Count > 0)
{
GraphEdge shortestEdge = adjacentEdgeQueue.Dequeue();
mstEdges.Add(shortestEdge);
GraphNode newIn = shortestEdge.outside;
//if (inMst.Contains(newIn)) throw new Exception();
//if (!toAdd.Contains(newIn)) throw new Exception("No edge to this node should remain!");
inMst.Add(newIn);
toAdd.Remove(newIn);
// Remove all edges that are entirely inside the MST now.
foreach (GraphEdge obsoleteEdge in edgesLeadingToNode[newIn])
{
//if (!inMst.Contains(obsoleteEdge.inside)) throw new Exception("This edge's inside node is not inside");
adjacentEdgeQueue.Remove(obsoleteEdge);
}
edgesLeadingToNode.Remove(newIn);
// Find all newly adjacent edges and enqueue them.
foreach (GraphNode otherNode in toAdd)
{
GraphEdge adjacentEdge = new GraphEdge(newIn, otherNode);
adjacentEdgeQueue.Enqueue(adjacentEdge, distances.GetDistance(adjacentEdge));
edgesLeadingToNode[otherNode].Add(adjacentEdge);
}
}
if (toAdd.Count > 0)
throw new DistanceLookup.GraphNotConnectedException();
this.SpanningEdges = mstEdges;
_isSpanned = true;
return mstEdges;
}
public void UpdatePathMapAvoidClaimedSpaces(Unit u, List<Unit> units)
{
Vector3 currentPositionVect = u.transform.position;
int currentX = (int)currentPositionVect.x;
int currentY = (int)currentPositionVect.z;
bool [,] validSpacesMap = new bool[width, height];
for (int x = 0; x < width; x++){
for (int y = 0; y < height; y++) {
distanceMap [x, y] = 1000000;
pathMap [x, y] = new Coordinate (-1, -1);
validSpacesMap[x,y] = !(board[x,y] == null);
}
}
//Unit[] units = FindObjectsOfType(typeof(Unit)) as Unit[];
foreach (Unit unit in units) {
Vector3 unitPosition = unit.transform.position;
validSpacesMap[(int)unitPosition.x,(int)unitPosition.z] = false;
}
distanceMap [currentX, currentY] = 0;
validSpacesMap [currentX, currentY] = true;
HeapPriorityQueue<Coordinate> priorityQueue = new HeapPriorityQueue<Coordinate>(height*width);
priorityQueue.Enqueue(new Coordinate (currentX, currentY), 0);
while( priorityQueue.Count > 0)
{
Coordinate c = priorityQueue.Dequeue();
int cDistanceInline = distanceMap[c.X, c.Y] + 2;
int cDistanceDiagonal = distanceMap[c.X, c.Y] + 3;
for (int x = c.X - 1; x <= c.X + 1; x++)
{
for (int y = c.Y - 1; y <= c.Y + 1; y++)
{
//if(board[c.X, c.Y] == null)
// continue;
if(!validSpacesMap[c.X,c.Y])
continue;
if(x == c.X && y == c.Y)
continue;
if(x < 0 || x >= width)
continue;
if(y < 0 || y >= height)
continue;
if(c.X == x || c.Y == y)
{
if(cDistanceInline < distanceMap[x,y])
{
distanceMap[x,y] = cDistanceInline;
priorityQueue.Enqueue(new Coordinate (x, y), cDistanceInline);
pathMap[x,y] = new Coordinate(c.X, c.Y);
}
}
else
{
if(cDistanceDiagonal < distanceMap[x,y])
{
distanceMap[x,y] = cDistanceDiagonal;
priorityQueue.Enqueue(new Coordinate (x, y), cDistanceDiagonal);
pathMap[x,y] = new Coordinate(c.X, c.Y);
}
}
}
}
}
priorityQueue.Clear();
}
private Node Dequeue(HeapPriorityQueue<Node> queue)
{
Node returnMe = queue.Dequeue();
Assert.IsTrue(queue.IsValidQueue());
return returnMe;
}
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);
}
}
public void UpdatePathMap(Unit u)
{
//SortedList<int, int[]> sortedList = new SortedList <int, int[]>();
//var sortedList = new SortedList();
//currentUnit = u;
Vector3 currentPositionVect = u.transform.position;
int currentX = (int)currentPositionVect.x;
int currentY = (int)currentPositionVect.z;
for (int x = 0; x < width; x++){
for (int y = 0; y < height; y++) {
distanceMap [x, y] = 1000000;
pathMap [x, y] = new Coordinate (-1, -1);
}
}
distanceMap [currentX, currentY] = 0;
//sortedList.Add(0,currentPosition);
HeapPriorityQueue<Coordinate> priorityQueue = new HeapPriorityQueue<Coordinate>(height*width); //need refine O(height*width)
priorityQueue.Enqueue(new Coordinate (currentX, currentY), 0);
//while (sortedList.Count > 0)
while( priorityQueue.Count > 0)
{
Coordinate c = priorityQueue.Dequeue();
int cDistanceInline = distanceMap[c.X, c.Y] + 2;
int cDistanceDiagonal = distanceMap[c.X, c.Y] + 3;
for (int x = c.X - 1; x <= c.X + 1; x++)
{
for (int y = c.Y - 1; y <= c.Y + 1; y++)
{
if(x == c.X && y == c.Y)
continue;
if(x < 0 || x >= width)
continue;
if(y < 0 || y >= height)
continue;
if (board[x, y] == null)
continue;
if(c.X == x || c.Y == y)
{
if(cDistanceInline < distanceMap[x,y])
{
distanceMap[x,y] = cDistanceInline;
priorityQueue.Enqueue(new Coordinate (x, y), cDistanceInline);
pathMap[x,y] = new Coordinate(c.X, c.Y);
}
}
else
{
if(cDistanceDiagonal < distanceMap[x,y])
{
distanceMap[x,y] = cDistanceDiagonal;
priorityQueue.Enqueue(new Coordinate (x, y), cDistanceDiagonal);
pathMap[x,y] = new Coordinate(c.X, c.Y);
}
}
}
}
}
priorityQueue.Clear();
}
// 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;
}
public void HeapPriorityQueueRemoveEmptyTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
Assert.AreEqual (0, queue.Dequeue());
}
public void HeapPriorityQueueUpdatePriorityLowestTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
queue.Enqueue (5, 2);
queue.Enqueue (6, 3);
queue.Enqueue (7, 4);
queue.Enqueue (8, 5);
queue.Enqueue (9, 6);
queue.Enqueue (10, 7);
queue.UpdatePriority(5, 0);
queue.UpdatePriority(6, 1);
int first = queue.Dequeue ();
int second = queue.Dequeue ();
int third = queue.Dequeue();
Assert.AreEqual (5, first);
Assert.AreEqual (6, second);
Assert.AreEqual (7, third);
Assert.AreEqual(3, queue.Count);
}
public void HeapPriorityQueueUpdatePriorityTest()
{
HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10);
queue.Enqueue (5, 1);
queue.Enqueue (6, 3);
queue.Enqueue (7, 5);
queue.Enqueue (8, 6);
queue.Enqueue (9, 7);
queue.Enqueue (10, 2);
queue.UpdatePriority(5, 10);
queue.UpdatePriority(6, 11);
int first = queue.Dequeue ();
int second = queue.Dequeue ();
Assert.AreEqual (10, first);
Assert.AreEqual (7, second);
Assert.AreEqual(4, queue.Count);
}
public void BuildForTopLevelComments(int? limit, int? maxDepth, bool continueThread = true, bool loadMore = true)
{
Clear();
var candidates = new HeapPriorityQueue<CommentQueue>(_tree.CommentIds.Count);
var topComments = _tree.Tree.ContainsKey(Guid.Empty) ? _tree.Tree[Guid.Empty] : new List<Guid>();
UpdateCandidates(candidates, _tree.Sorter, topComments);
var items = new List<Guid>();
while ((!limit.HasValue || items.Count < limit.Value) && candidates.Count > 0)
{
var candidate = candidates.Dequeue();
var commentDepth = _tree.Depth[candidate.CommentId];
if (!maxDepth.HasValue || commentDepth < maxDepth.Value)
{
items.Add(candidate.CommentId);
if (_tree.Tree.ContainsKey(candidate.CommentId))
UpdateCandidates(candidates, _tree.Sorter, _tree.Tree[candidate.CommentId]);
}
else if (continueThread && _tree.Parents[candidate.CommentId] != null)
{
var parentId = _tree.Parents[candidate.CommentId].Value;
if (!_moreRecursion.Contains(parentId))
_moreRecursion.Add(parentId);
}
}
_comments.AddRange(items);
_topLevelComments.AddRange(_comments.Where(x => _tree.Depth[x] == 0));
UpdateChildrenCount(_topLevelComments);
if (!loadMore)
return;
return;
foreach (var visibleComment in _comments)
{
if (_moreRecursion.Contains(visibleComment))
continue;
var children = _tree.Tree[visibleComment];
var missingChildren = children.Except(_comments).ToList();
if (missingChildren.Count > 0)
{
var visibleChildren = children.Intersect(_comments);
}
}
}
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;
}