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); }
/// <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 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); }
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 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); }
public void AddRange(IEnumerable <KillResult> kills) { foreach (var curKill in kills) { StargateLocation location; _stargateLocationRepository.TryGetStargateLocation(curKill.SolarSystemId, curKill.Position, out location); _queue.Enqueue(new KillNode(curKill, location), GetPriority(curKill)); } }
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 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 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); }
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 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"); }
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 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); }
private void Update(UInt32 pIndex, List <PointRelation> neighbors, double coreDistance) { for (int i = 0; i < neighbors.Count; i++) { UInt32 p2Index = neighbors[i].To; if (_points[p2Index].WasProcessed) { continue; } double newReachabilityDistance = Math.Max(coreDistance, neighbors[i].Distance); if (double.IsNaN(_points[p2Index].ReachabilityDistance)) { _points[p2Index].ReachabilityDistance = newReachabilityDistance; _seeds.Enqueue(_points[p2Index], newReachabilityDistance); } else if (newReachabilityDistance < _points[p2Index].ReachabilityDistance) { _points[p2Index].ReachabilityDistance = newReachabilityDistance; _seeds.UpdatePriority(_points[p2Index], newReachabilityDistance); } } }
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 void EnNetChunkLoadQueue(NetPriorityChunk netPriorityChunk) { lock (_netChunkLoadQueue) { _netChunkLoadQueue.Enqueue(netPriorityChunk, netPriorityChunk.Priority); } }
public void TestEnumeratorWhileChangingEntriesWithSamePriorityHeapQueue() { PriorityQueue <int, string> queue = new HeapPriorityQueue <int, string>(); int[] keys = new int[] { 9, 9, 9, 7, 7 }; string[] values = new string[] { "9a", "9b", "9c", "7a", "7b" }; for (int i = 0; i < values.Length; i++) { queue.Enqueue(values[i], keys[i]); } IEnumerator <string> enumerator = queue.GetEnumerator(); //First moving, then deleting enumerator.MoveNext(); queue.Remove("9a"); string val1 = enumerator.Current; Assert.IsTrue(val1.Equals("9a") || val1.Equals("9b") || val1.Equals("9c")); enumerator.MoveNext(); string val2 = enumerator.Current; Assert.IsTrue(val1.Equals("9b") || val2.Equals("9b") || val2.Equals("9c")); Assert.AreNotEqual(val2, val1); enumerator.MoveNext(); //First deleting, then moving queue.Remove("7a"); enumerator.MoveNext(); val1 = enumerator.Current; enumerator.MoveNext(); val2 = enumerator.Current; Assert.IsTrue(val1.Equals("7a") || val1.Equals("7b")); Assert.IsTrue(val2.Equals("7a") || val2.Equals("7b")); Assert.AreNotEqual(val1, val2); }
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 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); } } }
/* * 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 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); } }
private void Update(UInt32 pIndex, List <PointRelation> neighbors, double coreDistance) { for (int i = 0; i < neighbors.Count; i++)//遍历邻域点集 { UInt32 p2Index = neighbors[i].To; if (_points[p2Index].WasProcessed) { continue; } //计算该邻域点到中心点pIndex的可达距离 double newReachabilityDistance = Math.Max(coreDistance, neighbors[i].Distance); //如果该邻域点的可达距离不存在 if (double.IsNaN(_points[p2Index].ReachabilityDistance)) { _points[p2Index].ReachabilityDistance = newReachabilityDistance; //将该点加入有序队列 _seeds.Enqueue(_points[p2Index], newReachabilityDistance); } //该点的可达距离存在,则更新可达距离 else if (newReachabilityDistance < _points[p2Index].ReachabilityDistance) { _points[p2Index].ReachabilityDistance = newReachabilityDistance; _seeds.UpdatePriority(_points[p2Index], newReachabilityDistance); } } }
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 TestDebugEnqueueThrowsOnAlreadyEnqueuedNode() { Node node = new Node(1); HeapPriorityQueue <Node> queue = new HeapPriorityQueue <Node>(3); Enqueue(queue, node); Assert.Throws <InvalidOperationException>(() => queue.Enqueue(node, 1)); }
/** * 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 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 EnLoadQueue(Chunk chunk, WorldPos curInChunkPos) { lock (_selfLoadQueue) { if (_selfLoadQueue.Count > _selfLoadQueue.MaxSize) { return; } _selfLoadQueue.Enqueue(_manager.GetPriorityChunk(chunk), _manager.GetPriority(chunk.worldPos, curInChunkPos)); } }
public void EnGeneratorQueue(PriorityChunk priorityChunk) { lock (_selfGeneratorQueue) { if (_selfGeneratorQueue.Count > _selfGeneratorQueue.MaxSize) { return; } _selfGeneratorQueue.Enqueue(priorityChunk, priorityChunk.Priority); } }
public void EnNetGeneratorQueue(NetPriorityChunk priorityChunk) { lock (_netGeneratorQueue) { if (_netGeneratorQueue.Count > _netGeneratorQueue.MaxSize) { return; } _netGeneratorQueue.Enqueue(priorityChunk, priorityChunk.Priority); } }
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. }
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; } }
/** * 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 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 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 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 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; }
private void Enqueue(HeapPriorityQueue<Node> queue, Node node) { queue.Enqueue(node, node.Priority); Assert.IsTrue(queue.IsValidQueue()); }
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); } }
private void UpdateCandidates(HeapPriorityQueue<CommentQueue> candidates, Dictionary<Guid, double> sorter, List<Guid> comments) { foreach (var comment in comments.Where(sorter.ContainsKey)) candidates.Enqueue(new CommentQueue(comment), sorter[comment]); }
// 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 HeapPriorityQueuePeekTest() { HeapPriorityQueue<int> queue = new HeapPriorityQueue<int> (10); queue.Enqueue (5, 3); queue.Enqueue (6, 2); Assert.AreEqual (6, queue.Peek ()); Assert.AreEqual (2, queue.Count); }
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(); }
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(); }
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 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 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 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); }