/// <summary> /// Creates a minimum spanning tree using Prim's algorithm. The graph this is called on has to be connected. /// </summary> /// <param name="random">Random object for getting random starter element. As parameter to keep random seed predictability.</param> /// <returns>The minimum spanning tree.</returns> public RoomGraph MinSpanningTree(Random random, Func <Room, Room, double> DistFunc) { if (Count == 0) { return(new RoomGraph()); } SimplePriorityQueue <Room> queue = new SimplePriorityQueue <Room>(); Dictionary <Room, double> distanceToSpannTree = new Dictionary <Room, double>(Count); Dictionary <Room, Room> parentRoom = new Dictionary <Room, Room>(Count); foreach (Room r in rooms) { queue.Enqueue(r, double.MaxValue); distanceToSpannTree.Add(r, double.MaxValue); parentRoom.Add(r, null); } int start = random.Next(Count); distanceToSpannTree[rooms[start]] = 0; queue.UpdatePriority(rooms[start], 0); while (queue.Count > 0) { Room r = queue.Dequeue(); foreach (Room other in connections[r]) { if (queue.Contains(other)) //could keep track of this separatly (HashSet) because this is linear. { double dist = DistFunc(r, other); if (dist < distanceToSpannTree[other]) { parentRoom[other] = r; distanceToSpannTree[other] = dist; queue.UpdatePriority(other, dist); } } } } //insert the parentNode pairs into a new graph RoomGraph minSpanTree = new RoomGraph(); foreach (Room r in rooms) { minSpanTree.rooms.Add(r); minSpanTree.connections.Add(r, new List <Room>(8)); } foreach (var p in parentRoom) { if (p.Key != null && p.Value != null) { minSpanTree.AddConnectionBothWays(p.Key, p.Value); } } return(minSpanTree); }
public double Prim(long pointCount, long[][] points) { double path = 0; double[] cost = new double[pointCount]; SimplePriorityQueue <long, double> priorityQ = new SimplePriorityQueue <long, double>(); for (int I = 0; I < pointCount; I++) { cost[I] = int.MaxValue; priorityQ.Enqueue(I, int.MaxValue); } cost[0] = 0; priorityQ.UpdatePriority(0, 0); while (priorityQ.Count != 0) { long v = priorityQ.Dequeue(); path += cost[v]; for (int i = 0; i < pointCount; i++) { if (i == v || !priorityQ.Contains(i)) { continue; } double dist = RealDist(points[v][0], points[v][1], points[i][0], points[i][1]); if (cost[i] > dist) { priorityQ.UpdatePriority(i, dist); cost[i] = dist; } } } return(path); }
private int Dijkstry(int from, int to) { int[] distance = new int[count]; int[] prev = new int[count]; SimplePriorityQueue <int> queue = new SimplePriorityQueue <int>(); for (int i = 0; i < count; i++) { distance[i] = int.MaxValue; queue.Enqueue(i, float.MaxValue); } distance[from] = 0; queue.UpdatePriority(from, 0); while (queue.Count > 0) { int v = queue.Dequeue(); foreach (var e in matrix[v]) { if (distance[e.To] > distance[v] + e.Weight) { distance[e.To] = distance[v] + e.Weight; prev[e.To] = v; queue.UpdatePriority(e.To, distance[e.To]); } } if (v == target) { break; } } return(distance[to]); }
public List <Vertex> GetPathDijkstra(GameObject srcObj, GameObject desObj) { if (srcObj == null || desObj == null) { return(new List <Vertex>()); } Vertex srcVertex = GetNearestVertex(srcObj.transform.position); Vertex desVertex = GetNearestVertex(desObj.transform.position); Vertex vertex; Edge[] edges; Vertex[] neighbours; SimplePriorityQueue <Vertex> frontier = new SimplePriorityQueue <Vertex>(); Dictionary <Vertex, Vertex> comeFrom = new Dictionary <Vertex, Vertex>(); HashSet <Vertex> closed = new HashSet <Vertex>(); comeFrom.Add(srcVertex, null); //将每个节点的代价置为最大,同时更新源节点 foreach (Vertex v in vertices) { frontier.Enqueue(v, Mathf.Infinity); } //先搜索源节点 frontier.UpdatePriority(srcVertex, 0); while (frontier.Count != 0) { vertex = frontier.Dequeue(); closed.Add(vertex); if (ReferenceEquals(vertex, desVertex)) { return(BuildPath()); } edges = GetEdges(vertex); foreach (Edge e in edges) { //计算新的带价值,如果小于目前的代价那么进行更新 float costNew = frontier.GetPriority(vertex) + e.cost; //如果在闭集中则直接删除,因为此时闭集中已经时最短路了 if (closed.Contains(e.vertex)) { continue; } //如果新的路径带价值小则进行更新 if (costNew < frontier.GetPriority(e.vertex)) { frontier.UpdatePriority(e.vertex, costNew); comeFrom[e.vertex] = vertex; } //Relax } } return(new List <Vertex>()); }
public void ItemChanged(TileItem item) { if (cfg.filter.Applies(item.def)) { if (unavailable.Remove(item)) { if (item.amtAvailable > 0) { queue.Enqueue(item, Priority(item)); } else { unavailable.Add(item); } } else { if (item.amtAvailable > 0) { queue.UpdatePriority(item, Priority(item)); } else { queue.Remove(item); unavailable.Add(item); } } } }
private void ExpandNode(Point currentNode, Func <Point, float> heuristic) { foreach (var dir in UnobstructedDirections(currentNode)) { var nextNode = new Point(currentNode.X + (int)dir.X, currentNode.Y + (int)dir.Y); if (mVisited.Contains(nextNode)) { continue; } var nextWeight = mWeights[currentNode] + dir.Length(); if (mPaths.Contains(nextNode) && nextWeight >= mWeights[nextNode]) { continue; } mPathRecord[nextNode] = currentNode; mWeights[nextNode] = nextWeight; var priority = nextWeight + heuristic(nextNode); if (mPaths.Contains(nextNode)) { mPaths.UpdatePriority(nextNode, priority); } else { mPaths.Enqueue(nextNode, priority); } } }
// Determine what to with the given cell in terms of adding it to the open list private void ProcessCell(GridCell currentSquare, GridCell cell, SimplePriorityQueue <GridCell> openList, List <GridCell> closedList) { if (cell != null) { if (cell.walkable && !closedList.Contains(cell)) { if (!openList.Contains(cell)) { cell.g = currentSquare.g + 1; cell.f = cell.g + cell.CalculateH(destRow, destCol); openList.Enqueue(cell, cell.f); } else { // If this path would be shorter if (cell.g > (currentSquare.g + 1)) { cell.g = currentSquare.g + 1; cell.f = cell.g + cell.CalculateH(destRow, destCol); openList.UpdatePriority(cell, cell.f); } } } } }
public void Start(float timeout, bool periodic, Action?handlerFunc) { _elapsedTime = 0f; _timeout = timeout < MinimumTimeout ? MinimumTimeout : timeout; _periodic = periodic; _handlerFunc = handlerFunc; if (_queue.Contains(this)) { _queue.UpdatePriority(this, _time + timeout); } else { _queue.Enqueue(this, _time + timeout); } }
public override IEnumerable <Node> AStar(Node from, Node to) { var openSet = new SimplePriorityQueue <Node>(); openSet.Enqueue(from, 0f); IEnumerable <Node> path = new List <Node>(); while (openSet.Count > 0) { Parallel.For(0, Math.Min(4, openSet.Count), i => { var currentNode = openSet.Dequeue(); if (currentNode == to) { path = ReconstructPath(currentNode); } currentNode.ClosedSet = 1; foreach (var neighbor in currentNode.Neighbors) { { if (neighbor.ClosedSet != 0) { continue; } var tentativeGScore = currentNode.GScore + currentNode.Distance(neighbor); if (openSet.Contains(neighbor) && tentativeGScore >= neighbor.GScore) { continue; } Interlocked.Exchange(ref neighbor.CameFrom, currentNode); Interlocked.Exchange(ref neighbor.GScore, tentativeGScore); var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to); if (openSet.Contains(neighbor)) { openSet.UpdatePriority(neighbor, fs); } else { openSet.Enqueue(neighbor, fs); } } } }); } foreach (var node in this) { node.Reset(); } return(path); }
public void DeterministicIterateDequeueAndUpdateAndEnqueue(int loopCount, decimal spd) { List <int> results1 = new List <int>(); List <int> results2 = new List <int>(); for (int i = 0; i < 2; i++) { List <int> targetResults = i == 0 ? results1 : results2; SimplePriorityQueue <int, decimal> queue = new SimplePriorityQueue <int, decimal>(); queue.Enqueue(0, spd); queue.Enqueue(1, spd); queue.Enqueue(2, spd); for (int j = 0; j < loopCount; j++) { Assert.True(queue.TryDequeue(out int index)); targetResults.Add(index); foreach (int otherIndex in queue) { decimal priority = queue.GetPriority(otherIndex); queue.UpdatePriority(otherIndex, priority); } queue.Enqueue(index, spd); } } Assert.Equal(results1, results2); }
public bool CalculateAvailableNearestTree(Villager inVillager, out TreeScript nearestTree) { if (!AreAvailableTreesInSanctuary()) { nearestTree = null; return(false); } foreach (var tree in TreesPriorityQueue) { var distance = (inVillager.transform.position - tree.transform.position).magnitude; TreesPriorityQueue.UpdatePriority(tree, distance); } var treesArray = TreesPriorityQueue.ToArray(); for (var i = 0; i < treesArray.Length; i++) { if (!treesArray[i].isOccupied) { nearestTree = treesArray[i]; return(true); } } inVillager.UpdateAIText("No Available Trees"); nearestTree = null; return(false); }
public static void RunExample() { //First, we create the priority queue. SimplePriorityQueue<string> priorityQueue = new SimplePriorityQueue<string>(); //Now, let's add them all to the queue (in some arbitrary order)! priorityQueue.Enqueue("4 - Joseph", 4); priorityQueue.Enqueue("2 - Tyler", 0); //Note: Priority = 0 right now! priorityQueue.Enqueue("1 - Jason", 1); priorityQueue.Enqueue("4 - Ryan", 4); priorityQueue.Enqueue("3 - Valerie", 3); //Change one of the string's priority to 2. Since this string is already in the priority queue, we call UpdatePriority() to do this priorityQueue.UpdatePriority("2 - Tyler", 2); //Finally, we'll dequeue all the strings and print them out while(priorityQueue.Count != 0) { string nextUser = priorityQueue.Dequeue(); Console.WriteLine(nextUser); } //Output: //1 - Jason //2 - Tyler //3 - Valerie //4 - Joseph //4 - Ryan //Notice that when two strings with the same priority were enqueued, they were dequeued in the same order that they were enqueued. }
public void UpdateVertex(int x, int y, int nx, int ny) { worldNode current = worldNodes[x, y]; worldNode next = worldNodes[nx, ny]; float css = cost(x, y, nx, ny); if (css == 0) { closedList.Add(next); return; } if (current.g + css < next.g) { next.g = current.g + css; next.h = computeHeuristic(nx, ny); next.f = next.g + next.h; next.parent = current; if (fringe.Contains(next)) { fringe.UpdatePriority(next, next.f); } else { fringe.Enqueue(next, next.f); // f- c*g where c = 200 } //next.g + h - 200*next.g } }
private static void VisitNextStates(Heuristic heuristic, SimplePriorityQueue <State> open, HashSet <State> open2, HashSet <State> close, State currentState, List <State> nextStates, List <Operation> nextActions) { for (int i = 0; i < nextStates.Count; i++) { State nextState = nextStates[i]; Operation nextAction = nextActions[i]; if (!close.Contains(nextState)) { nextState.parent = currentState; if (open2.Contains(nextState)) { //Si existe deja, update les stats double newg = currentState.g + nextAction.cost; double newf = newg + heuristic.EstimateCost(nextState); if (newf < nextState.f) { nextState.g = newg; nextState.f = newf; open.UpdatePriority(nextState, (float)nextState.f); } } else { //Jamais été visité nextState.g = currentState.g + nextAction.cost; nextState.f = nextState.g + heuristic.EstimateCost(nextState); open2.Add(nextState); open.Enqueue(nextState, (float)nextState.f); } } } }
private void ProcessNeighbours(Id <TNode> nodeId, AStarNode <TNode> node) { var connections = _map.GetConnections(nodeId); foreach (var connection in connections) { var gCost = node.G + connection.Cost; var neighbour = connection.Target; if (_nodeLookup.NodeIsVisited(neighbour)) { var targetAStarNode = _nodeLookup.GetNodeValue(neighbour); // If we already processed the neighbour in the past or we already found in the past // a better path to reach this node that the current one, just skip it, else create // and replace a new PathNode if (targetAStarNode.Status == CellStatus.Closed || gCost >= targetAStarNode.G) { continue; } targetAStarNode = new AStarNode <TNode>(nodeId, gCost, targetAStarNode.H, CellStatus.Open); _openQueue.UpdatePriority(neighbour, targetAStarNode.F); _nodeLookup.SetNodeValue(neighbour, targetAStarNode); } else { var newHeuristic = _calculateHeuristic(neighbour); var newAStarNode = new AStarNode <TNode>(nodeId, gCost, newHeuristic, CellStatus.Open); _openQueue.Enqueue(neighbour, newAStarNode.F); _nodeLookup.SetNodeValue(neighbour, newAStarNode); } } }
private void Insert(SearchNode X, float newH) { if (X.IsNew) { X.Key = newH; } else if (X.Opened) { X.Key = Mathf.Min(X.Key, newH); } else if (X.Closed) { X.Key = Mathf.Min(X.H, newH); } X.Opened = true; X.H = newH; if (m_openQueue.Contains(X)) { m_openQueue.UpdatePriority(X, X.Key); } else { m_openQueue.Enqueue(X, X.Key); } }
/// <summary> /// Demonstrates the use of SimplePriorityQueue /// </summary> public static void RunExample() { // First, we create the priority queue SimplePriorityQueue <string> priorityQueue = new SimplePriorityQueue <string>(); // Now, let's add them all to the queue (in some arbitrary order)! priorityQueue.Enqueue("4 - Joseph", 4); priorityQueue.Enqueue("2 - Tyler", 0); // Note: Priority = 0 right now! priorityQueue.Enqueue("1 - Jason", 1); priorityQueue.Enqueue("4 - Ryan", 4); priorityQueue.Enqueue("3 - Valerie", 3); // Change one of the string's priority to 2 // Since this string is already in the priority queue, we call UpdatePriority() to do this priorityQueue.UpdatePriority("2 - Tyler", 2); // Finally, we'll dequeue all the strings and print them out while (priorityQueue.Count != 0) { string nextUser = priorityQueue.Dequeue(); Console.WriteLine(nextUser); } // Output: // 1 - Jason // 2 - Tyler // 3 - Valerie // 4 - Joseph // 4 - Ryan // Notice that when two strings with the same priority were enqueued, they were dequeued in the same order that they were enqueued. }
public long Dijkstra(long startNode, long endNode) { SimplePriorityQueue <long, long> H = new SimplePriorityQueue <long, long>(); long[] dist = new long[V]; for (int i = 0; i < V; i++) { dist[i] = long.MaxValue; } dist[startNode] = 0; for (int i = 0; i < V; i++) { H.Enqueue(i, dist[i]); } while (H.Count > 0) { var u = H.Dequeue(); foreach (var edge in adj[u]) { if (dist[u] != long.MaxValue) { if (dist[edge.Item1] > dist[u] + edge.Item2) { dist[edge.Item1] = dist[u] + edge.Item2; H.UpdatePriority(edge.Item1, dist[edge.Item1]); } } } } if (dist[endNode] != long.MaxValue) { return(dist[endNode]); } return(-1); }
public List <Node> Run(List <List <Node> > grid, Node startNode, Node finishNode) { UnvisitedNodes = GetAllNodes(grid); ResetDistances(UnvisitedNodes); List <Node> visitedNodesInOrder = new List <Node>(); startNode.Distance = 0; UnvisitedNodes.UpdatePriority(startNode, 0); while (true) { // unvisitedNodes = SortNodesByDistance(unvisitedNodes); Node closestNode = UnvisitedNodes.Dequeue(); if (closestNode.IsWall) { closestNode.IsVisited = true; continue; } if (closestNode.Distance == 100000000) { return(visitedNodesInOrder); } closestNode.IsVisited = true; visitedNodesInOrder.Add(closestNode); if (closestNode.IsFinish == true) { return(visitedNodesInOrder); } Console.WriteLine(closestNode.Id); UpdateUnvisitedNeighbors(closestNode, grid); } }
public void PlayTurn() { Unit unit = unitsByCooldown.First; unit.IncreaseCooldown(unit.position.y); unitsByCooldown.UpdatePriority(unit, unit.cooldown); Select(unitsByCooldown.First); }
public void Reschedule(Event e) { if (queue.Count(ev => ev == e) != 1) { throw new Exception("Double event?"); } queue.UpdatePriority(e, e.time); }
public static PathResult GetPath(Vector2Int startPosition, Vector2Int endPosition) { TileProperty start = Loki.map[startPosition]; TileProperty end = Loki.map[endPosition]; bool success = false; Vector2Int[] path = new Vector2Int[0]; start.parent = start; if (!start.blockPath && !end.blockPath) { SimplePriorityQueue <TileProperty> openSet = new SimplePriorityQueue <TileProperty>(); HashSet <TileProperty> closedSet = new HashSet <TileProperty>(); openSet.Enqueue(start, start.fCost); while (openSet.Count > 0) { TileProperty current = openSet.Dequeue(); if (current == end) { success = true; break; } closedSet.Add(current); for (int i = 0; i < 8; i++) { TileProperty neighbour = Loki.map[current.position + DirectionUtils.neighbours[i]]; if (neighbour == null || neighbour.blockPath || closedSet.Contains(neighbour)) { continue; } float neighbourCost = current.gCost + Utils.Distance(current.position, neighbour.position) + neighbour.pathCost; if (neighbourCost > neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = neighbourCost; neighbour.hCost = Utils.Distance(neighbour.position, end.position); neighbour.parent = current; if (!openSet.Contains(neighbour)) { openSet.Enqueue(neighbour, neighbour.fCost); } else { openSet.UpdatePriority(neighbour, neighbour.fCost); } } } } } if (success) { path = PathFinder.CalcPath(start, end); success = path.Length > 0; } return(new PathResult(path, success)); }
public static Queue <RobotAction> GetRouteTo(Point start, Point goal, Map map, int maxDistance = int.MaxValue) { if (start == goal) { return(new Queue <RobotAction>()); } var closed_set = new HashSet <Point>(); var came_from = new Dictionary <Point, Point>(); var g_score = new Dictionary <Point, float> { { start, 0 } }; var open_set = new SimplePriorityQueue <Point, float>(); open_set.Enqueue(start, GetDistance(start, goal)); while (open_set.Count != 0) { var current = open_set.Dequeue(); if (current == goal) { return(ReconstructPath(came_from, start, goal)); } closed_set.Add(current); var tentative_g_score = g_score[current] + 1; if (tentative_g_score > maxDistance) { continue; } foreach (var neighbor in map.Neighbors(current)) { if (closed_set.Contains(neighbor)) { continue; } if (!open_set.Contains(neighbor)) { open_set.Enqueue(neighbor, tentative_g_score + GetDistance(neighbor, goal)); } else if (tentative_g_score >= g_score[neighbor]) { continue; } came_from[neighbor] = current; g_score[neighbor] = tentative_g_score; open_set.UpdatePriority(neighbor, tentative_g_score + GetDistance(neighbor, goal)); } } return(null); }
private NevPoint[] aStarSearch(NevPoint from, NevPoint to) { SimplePriorityQueue <NevPoint> searchQueue = new SimplePriorityQueue <NevPoint>(); Dictionary <NevPoint, NevPoint> last = new Dictionary <NevPoint, NevPoint>(); Dictionary <NevPoint, float> dis = new Dictionary <NevPoint, float>(); Action <NevPoint, NevPoint, float, float> insertPoint = (point, lastPoint, _dis, priority) => { searchQueue.Enqueue(point, priority); last.Add(point, lastPoint); dis.Add(point, _dis); }; insertPoint(from, from, 0, 0); while (searchQueue.Count != 0) { NevPoint point = searchQueue.Dequeue(); if (point == to) { break; } foreach (var dir in Dirs) { if (point.GetAroundFlag(dir)) { NevPoint tar = GetClosestNevPoint(point.pos + dir * step); float nowDis = dis[point] + step; float priority = nowDis + (to.pos - tar.pos).magnitude; bool contain = last.ContainsKey(tar); if (!contain) { insertPoint(tar, point, nowDis, priority); } else if (searchQueue.Contains(tar) && searchQueue.GetPriority(tar) > priority) { searchQueue.UpdatePriority(tar, priority); last[tar] = point; dis[tar] = nowDis; } } } } Action <Action <NevPoint> > doeachPathNode = (a) => { var node = to; while (last[node] != from) { node = last[node]; a(node); } }; int pathSize = 0; doeachPathNode((p) => pathSize++); NevPoint[] result = new NevPoint[pathSize]; //Not contain from and to doeachPathNode((p) => { pathSize--; result[pathSize] = p; }); return(result); }
public double Astar(List <double>[] w, List <double>[] neighbours, long nodeCount, long start, long end, double[][] points) { double[] dist = new double[nodeCount]; //double[] cost = new double[nodeCount]; //bool[] process = new bool[nodeCount]; SimplePriorityQueue <long, double> cost = new SimplePriorityQueue <long, double>(); bool geo = false; for (int i = 0; i < nodeCount; i++) { cost.Enqueue(i, int.MaxValue); dist[i] = int.MaxValue; } long n = nodeCount; dist[start] = 0; cost.UpdatePriority(start, RealDist(points[start][0], points[start][1], points[end][0], points[end][1], geo)); while (n != 0) { //long v = FindMin(cost,nodeCount,process); //var value = cost.Min(x=>x.Value); long v = cost.Dequeue(); //cost.Remove(v); // process[v] = true; n--; long count = neighbours[v].Count; if (v == end) { return(dist[v]); } for (int i = 0; i < count; i++) { if (cost.Contains((int)neighbours[v][i]) && dist[(int)neighbours[v][i]] > dist[v] + w[v][i]) { long u = (long)neighbours[v][i]; dist[u] = dist[v] + w[v][i]; double realdist = RealDist(points[u][0], points[u][1], points[end][0], points[end][1], geo); cost.UpdatePriority(u, dist[u] + realdist); } } } return(dist[end]); }
public void PlayerDeterministicIterateDequeueAndUpdateAndEnqueue(int loopCount, decimal spd) { var sheets = TableSheetsImporter.ImportSheets(); TableSheets tableSheets = new TableSheets(sheets); Player[] players = new[] { new Player( 1, tableSheets.CharacterSheet, tableSheets.CharacterLevelSheet, tableSheets.EquipmentItemSetEffectSheet), new Player( 2, tableSheets.CharacterSheet, tableSheets.CharacterLevelSheet, tableSheets.EquipmentItemSetEffectSheet), new Player( 3, tableSheets.CharacterSheet, tableSheets.CharacterLevelSheet, tableSheets.EquipmentItemSetEffectSheet), }; List <Player> results1 = new List <Player>(); List <Player> results2 = new List <Player>(); for (int i = 0; i < 2; i++) { List <Player> targetResults = i == 0 ? results1 : results2; SimplePriorityQueue <Player, decimal> queue = new SimplePriorityQueue <Player, decimal>(); for (int j = 0; j < players.Length; j++) { queue.Enqueue(players[j], spd); } for (int j = 0; j < loopCount; j++) { Assert.True(queue.TryDequeue(out Player player)); targetResults.Add(player); foreach (Player otherPlayer in queue) { decimal priority = queue.GetPriority(otherPlayer); queue.UpdatePriority(otherPlayer, priority); } queue.Enqueue(player, spd); } } Assert.Equal(results1, results2); }
/// <summary> /// Calculate the shortest path with Dijkstra's algorithm. /// </summary> /// <param name="start">Node to start finding shortest path</param> /// <param name="graph">Graph represent nodes and edges linking nodes.</param> /// <returns>ShortestPathResult from start node to other nodes in the graph.</returns> public static ShortestPathResult FindShortestPath(Node start, Graph graph) { var distanceToNodes = new Dictionary <Node, int>(); var previousNode = new Dictionary <Node, Node>(); var VisitedNodes = new List <Node>(); var QueueNodes = new SimplePriorityQueue <Node>(); foreach (var n in graph.Nodes) { if (n.CompareTo(start) != 0) { distanceToNodes[n] = int.MaxValue; } else { distanceToNodes[n] = 0; } previousNode[n] = null; QueueNodes.Enqueue(n, distanceToNodes[n]); } while (QueueNodes.Count > 0) { var u = QueueNodes.Dequeue(); foreach (var e in graph.GetEdgesToNeighbors(u)) { var v = e.Destination; int alt; try { alt = checked (distanceToNodes[u] + e.Weight); } catch { alt = int.MaxValue; } if (alt < distanceToNodes[v]) { distanceToNodes[v] = alt; previousNode[v] = u; QueueNodes.UpdatePriority(v, distanceToNodes[v]); } } } return(new ShortestPathResult { Graph = graph, Start = start, DistanceToNodes = distanceToNodes, PreviousNode = previousNode }); }
public Stack <Direction> GetPathToNearest(Position start, List <TerrainType> closedTerrain, List <TerrainType> targetTerrain) { var distances = new Dictionary <CoordinatePair, Int32>(); var previouses = new Dictionary <CoordinatePair, CoordinatePair>(); var explored = new HashSet <CoordinatePair>(); var queue = new SimplePriorityQueue <CoordinatePair, Int32>(); explored.Add(start); queue.Enqueue(start, 0); distances[start] = 0; while (queue.Count > 0) { CoordinatePair node = queue.Dequeue(); TerrainType terrain = Map[node]; if (targetTerrain.Contains(terrain) && node != start) { // We've found the solution return(ExtractPath(previouses, node)); } Int32 nodeDist = distances[node]; Int32 candidateDist = nodeDist + 1; for (Int32 i = 0; i < Direction.DirectionCount; i++) { Direction dir = Direction.FromInt32(i); CoordinatePair neighbor = node + dir; if (closedTerrain.Contains(Map[neighbor])) { continue; } if (distances.TryGetValue(neighbor, out Int32 existingDist) && candidateDist > existingDist) { continue; } distances[neighbor] = candidateDist; previouses[neighbor] = node; if (explored.Contains(neighbor)) { queue.UpdatePriority(neighbor, candidateDist); } else { queue.Enqueue(neighbor, candidateDist); } explored.Add(neighbor); } } // There is no path to the destination. return(null); }
public override IEnumerable <Node> AStar(Node from, Node to) { var closedSet = new ConcurrentBag <Node>(); var openSet = new SimplePriorityQueue <Node>(); openSet.Enqueue(from, 0f); var cameFrom = new ConcurrentDictionary <Node, Node>(); var gScore = new ConcurrentDictionary <Node, float>(); gScore.TryAdd(from, 0f); while (openSet.Count > 0) { var currentNode = openSet.Dequeue(); if (currentNode == to) { return(ReconstructPath(cameFrom, currentNode)); } closedSet.Add(currentNode); Parallel.ForEach(currentNode.Neighbors, neighbor => { if (closedSet.Contains(neighbor)) { return; } var tentativeGScore = gScore[currentNode] + currentNode.Distance(neighbor); if (openSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor]) { return; } cameFrom[neighbor] = currentNode; gScore[neighbor] = tentativeGScore; var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to); if (openSet.Contains(neighbor)) { openSet.UpdatePriority(neighbor, fs); } else { openSet.Enqueue(neighbor, fs); } }); } return(new List <Node>()); }
public static List <Node> DoPathFind_AStar(Node startNode, Node endNode, Node[,] maps) { // clear everything foreach (Node n in maps) { n.cameFrom = null; } SimplePriorityQueue <Node> nodes = new SimplePriorityQueue <Node> (); nodes.Enqueue(startNode, startNode.costSoFar); while (nodes.Count > 0) { Node currentNode = nodes.Dequeue(); if (currentNode == endNode) { // we found the path break the loop !!! return(RetracePath(endNode, startNode)); } Node[] neighbour = NeighbourNode(currentNode, maps); foreach (Node n in neighbour) { float newCost = currentNode.costSoFar + ((n.movementCost == 0) ? 10000 : n.movementCost * ((n.isDiag) ? 1.414f : 1)); if (n.cameFrom == null || newCost < n.costSoFar) { n.costSoFar = newCost; float priority = n.costSoFar + HeuristicDistance(endNode, n); if (n.cameFrom != null) { if (nodes.Contains(n)) { nodes.UpdatePriority(n, priority); } else { nodes.Enqueue(n, priority); } } else { nodes.Enqueue(n, priority); } n.cameFrom = currentNode; } } } // if we reach her mean's we were unable to find path Debug.LogError("Unable to find Path"); return(null); }
public List <Vector2Int> FindPath(Vector2Int start, Vector2Int end) { for (int i = 0; i < grid.GetLength(0); i++) { for (int j = 0; j < grid.GetLength(1); j++) { grid[i, j].Reset(); } } var openSet = new SimplePriorityQueue <AStarNode>(); AStarNode startNode = GetNode(start); if (startNode == null || GetNode(end) == null) { return(null); } startNode.g = 0; startNode.h = Heuristic(startNode.pos, end); openSet.Enqueue(startNode, startNode.f); startNode.open = true; while (openSet.Count > 0) { AStarNode current = openSet.Dequeue(); if (current.pos == end) { return(FindPath(end)); } current.closed = true; foreach (AStarNode neighbor in GetNeighbors(current.pos)) { if (neighbor == null || neighbor.closed) { continue; } float tentativeG = current.g + Heuristic(current.pos, neighbor.pos); if (!neighbor.open) { neighbor.previous = current; neighbor.g = tentativeG; neighbor.h = Heuristic(neighbor.pos, end); openSet.Enqueue(neighbor, neighbor.f); neighbor.open = true; } else if (tentativeG < neighbor.g) { neighbor.previous = current; neighbor.g = tentativeG; openSet.UpdatePriority(neighbor, neighbor.f); } } } return(null); }
/// <summary> /// Finds a path through the puzzle originating from startNode and ending at endNode, going across from left to right. /// Based on A* pathfinding algorithm, it theoretically is supposed to find the shortest possible path, but this is thus far untested/unproven. /// /// First uses buildGraph to determine all possible and relevant edges between each tile/node, then uses a mostly standard A* algorithm, until /// an orange tile is encountered and the rules change (because of "scents"). Every time an orange tile is encountered, a nested A*-based loop (referred to as the "orange loop") /// is ran (but with orange-scented rules) which essentially starts at the orange tile and seeks any tile that will remove the orange scent, or the endNode. /// Every tile encountered that exits the orange loop is added to the main open set (the open set of the normal A* loop) with their cost to get there through the orange loop. /// /// </summary> /// <returns>Returns a list of all possible paths to the endNode, and the shortest one, or the one with the least tree height, is to be considered the answer</returns> public List<PathTreeNode> solve() { resetGraphEdges(); buildGraph(); //A*-based pathfinding algorithm List<Node> closedSet = new List<Node>(); SimplePriorityQueue<Node> openSet = new SimplePriorityQueue<Node>(); List<Node> closedOrangeSet = new List<Node>(); SimplePriorityQueue<Node> openOrangeSet; List<PathTreeNode> Leaves = new List<PathTreeNode>(); if(startNode.edges.Count == 0) { return Leaves; } startNode.g = 0; openSet.Enqueue(startNode, 0); PathTreeNode root = new PathTreeNode(startNode.row, -1); Leaves.Add(root); while (openSet.Count > 0) { Node current = openSet.Dequeue(); PathTreeNode currentStep = null; Predicate<PathTreeNode> matchingCurrentPos = aStep => aStep.row == currentStep.row && aStep.col == currentStep.col; if (current == endNode) { return Leaves; } if(current.edges.Count == 0) { continue; } foreach (PathTreeNode leaf in Leaves) { if(leaf.row == current.row && leaf.col == current.col) { if(currentStep == null || currentStep.height > leaf.height) { currentStep = leaf; } } } if (currentStep != null) { Leaves.RemoveAll(matchingCurrentPos); } if(current.color == 1) { openOrangeSet = new SimplePriorityQueue<Node>(); openOrangeSet.Enqueue(current, current.f); currentStep.isOrangeStep = true; Leaves.Add(currentStep); while(openOrangeSet.Count > 0) { Node currentOrange = openOrangeSet.Dequeue(); PathTreeNode currentOrangeStep = null; Predicate<PathTreeNode> matchingCurrentOrangePos = aStep => aStep.isOrangeStep && aStep.row == currentOrangeStep.row && aStep.col == currentOrangeStep.col; if (currentOrange.edges.Count == 0) { continue; } foreach (PathTreeNode leaf in Leaves) { if (leaf.isOrangeStep && leaf.row == currentOrange.row && leaf.col == currentOrange.col) { if (currentOrangeStep == null || currentOrangeStep.height > leaf.height) { currentOrangeStep = leaf; } } } if (currentOrangeStep != null) { Leaves.RemoveAll(matchingCurrentOrangePos); } closedOrangeSet.Add(currentOrange); foreach (Edge toOrangeNeighbor in currentOrange.edges) { if (closedSet.Contains(toOrangeNeighbor.childNode) || closedOrangeSet.Contains(toOrangeNeighbor.childNode)) { continue; } if (toOrangeNeighbor.childNode.col == cols) { toOrangeNeighbor.childNode.row = currentOrange.row; } int currentOrangeG = currentOrange.g + Math.Abs((toOrangeNeighbor.childNode.row - currentOrange.row) + (toOrangeNeighbor.childNode.col - currentOrange.col)) + toOrangeNeighbor.childNode.weight; if (openSet.Contains(toOrangeNeighbor.childNode) && toOrangeNeighbor.childNode.g < currentOrangeG) { continue; } PathTreeNode aNextStep; if ((toOrangeNeighbor.isScented && !toOrangeNeighbor.isOrangeScented) || toOrangeNeighbor.childNode == endNode) { toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col); toOrangeNeighbor.childNode.g = currentOrangeG; openSet.Enqueue(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f); aNextStep = new PathTreeNode(toOrangeNeighbor.childNode.row, toOrangeNeighbor.childNode.col, currentOrangeStep); Leaves.Add(aNextStep); continue; } if(toOrangeNeighbor.childNode.color == 4) { continue; } if (!openOrangeSet.Contains(toOrangeNeighbor.childNode)) { toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col); openOrangeSet.Enqueue(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f); } else if (currentOrangeG >= toOrangeNeighbor.childNode.g) { continue; } toOrangeNeighbor.childNode.g = currentOrangeG; toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col); openOrangeSet.UpdatePriority(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f); aNextStep = new PathTreeNode(toOrangeNeighbor.childNode.row, toOrangeNeighbor.childNode.col, currentOrangeStep, true); Leaves.Add(aNextStep); } } Predicate<PathTreeNode> isOrangeStepLeaf = aStep => aStep.isOrangeStep; Leaves.RemoveAll(isOrangeStepLeaf); closedSet.Add(current); } else { closedSet.Add(current); foreach (Edge toNeighbor in current.edges) { if (closedSet.Contains(toNeighbor.childNode)) { continue; } if(current.col == -1) { current.row = toNeighbor.childNode.row; } else if(toNeighbor.childNode.col == cols) { toNeighbor.childNode.row = current.row; } int currentG = current.g + Math.Abs((toNeighbor.childNode.row - current.row) + (toNeighbor.childNode.col - current.col)) + toNeighbor.childNode.weight; PathTreeNode aNextStep; if (!openSet.Contains(toNeighbor.childNode)) { toNeighbor.childNode.f = currentG + heuristic(toNeighbor.childNode.col); openSet.Enqueue(toNeighbor.childNode, toNeighbor.childNode.f); } else if (currentG >= toNeighbor.childNode.g) { continue; } toNeighbor.childNode.g = currentG; toNeighbor.childNode.f = currentG + heuristic(toNeighbor.childNode.col); openSet.UpdatePriority(toNeighbor.childNode, toNeighbor.childNode.f); aNextStep = new PathTreeNode(toNeighbor.childNode.row, toNeighbor.childNode.col, currentStep); Leaves.Add(aNextStep); } } } return Leaves; }
/// <summary> /// Processes every open or unexplored successor of nodeId /// </summary> private void ProcessNeighbours(int nodeId, AStarNode node, AStarNode?[] nodeLookup, SimplePriorityQueue<int> openQueue) { var successors = map.GetNeighbours(nodeId); foreach (var successor in successors) { var newg = node.G + successor.Cost; var successorTarget = successor.Target; var targetAStarNode = nodeLookup[successorTarget]; if (targetAStarNode.HasValue) { // If we already processed the neighbour in the past or we already found in the past // a better path to reach this node that the current one, just skip it, else create // and replace a new PathNode if (targetAStarNode.Value.Status == CellStatus.Closed || newg >= targetAStarNode.Value.G) continue; targetAStarNode = new AStarNode(nodeId, newg, targetAStarNode.Value.H, CellStatus.Open); nodeLookup[successorTarget] = targetAStarNode; openQueue.UpdatePriority(successorTarget, targetAStarNode.Value.F); } else { var newHeuristic = calculateHeuristic(successorTarget); var newAStarNode = new AStarNode(nodeId, newg, newHeuristic, CellStatus.Open); openQueue.Enqueue(successorTarget, newAStarNode.F); nodeLookup[successorTarget] = newAStarNode; } } }