public AStarSearch(WeightedGraph<Position> graph, Position start, Position goal) { var frontier = new SimplePriorityQueue<Position>(); frontier.Enqueue(start, 0); cameFrom[start] = start; costSoFar[start] = 0; while (frontier.Count > 0) { var current = frontier.Dequeue(); if(current.Equals(goal)) { break; } foreach (var next in graph.Neighbors(current)) { int newCost = costSoFar[current] + graph.Cost(current, next); if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { costSoFar[next] = newCost; int priority = newCost + Heuristic(next, goal); frontier.Enqueue(next, priority); cameFrom[next] = current; } } } }
/// <summary> /// Generar el árbol SPF de la topología en un proyecto, tomando el parámetro como punto de origen /// </summary> /// <param name="idRouterOrigen"></param> /// <param name="idProyecto"></param> /// <returns></returns> public static SimplePriorityQueue<NodoDijkstra> GenerarRutas(NodoDijkstra idRouterOrigen, int idProyecto) { idRouterOrigen.nMinDistancia = 0.0; SimplePriorityQueue<NodoDijkstra> routerQueue = new SimplePriorityQueue<NodoDijkstra>(); routerQueue.Enqueue(idRouterOrigen, 1); while (routerQueue.Count > 0) { NodoDijkstra currentRouter = routerQueue.Dequeue(); //Visita cada enlace adyacente al router u foreach (var enlace in currentRouter.listaEnlaces) { NodoDijkstra vecino = new NodoDijkstra(enlace.idRouterB, idProyecto); double nPesoBandwidth = enlace.nBandwidth; double nDistanciaTotal = currentRouter.nMinDistancia + nPesoBandwidth; if (nDistanciaTotal < vecino.nMinDistancia) { routerQueue.Remove(vecino); vecino.nMinDistancia = nDistanciaTotal; vecino.idRouterPrevio = currentRouter; routerQueue.Enqueue(vecino, 1); } } } return routerQueue; }
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 Queue<Vector2> GetPath(Node start, Node goal) { List<Node> closedSet = new List<Node>(); // The set of nodes already evaluated SimplePriorityQueue<Node> openSet = new SimplePriorityQueue<Node>(); // The set of tentative nodes to be evaluated openSet.Enqueue(start, 0); Dictionary<Node, Node> came_from = new Dictionary<Node, Node>(); Dictionary<Node, float> g_score = new Dictionary<Node, float>(); Dictionary<Node, float> f_score = new Dictionary<Node, float>(); foreach (Node node in graph.nodes.Values) { g_score[node] = Mathf.Infinity; f_score[node] = Mathf.Infinity; } g_score[start] = 0f; f_score[start] = heuristic_cost_estimate(start, goal); while(openSet.Count > 0) { Node current = openSet.Dequeue(); if(current == goal ) { return reconstruct_path(came_from, current); } closedSet.Add(current); foreach(Node neighbour in current.edges.Keys) { if (closedSet.Contains(neighbour)) continue; float tentative_g_score = g_score[current] + dist_between(current, neighbour); // length of this path. if (openSet.Contains(neighbour) && tentative_g_score >= g_score[neighbour]) { continue; } came_from[neighbour] = current; g_score[neighbour] = tentative_g_score; f_score[neighbour] = g_score[neighbour] + heuristic_cost_estimate(neighbour, goal); openSet.Enqueue(neighbour, f_score[neighbour]); } } // Failed to find a path. return null; }
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]); }
private void AddToQueue(MethodData methodData) { lock (workingSet) { if (!workingSet.Contains(methodData)) { workingSet.Add(methodData); Interlocked.Increment(ref totalMethods); } } lock (queue) { if (queue.Contains(methodData)) { //Debug.WriteLine($"Already in Queue: {method}"); return; // already queued } //Debug.WriteLine($"Queued: {method}"); var priority = GetCompilePriorityLevel(methodData); queue.Enqueue(methodData, priority); Interlocked.Increment(ref totalQueued); } }
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); } } }
static void VisitCell(HexCell cell, HexCell from, float priority, ref SimplePriorityQueue <HexCell> frontier, ref Dictionary <HexCell, HexCell> visited) { frontier.Enqueue(cell, priority); visited[cell] = from; }
// 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); } } } } }
private int FindPath() { openList = new SimplePriorityQueue <GridCell>(); closedList = new List <GridCell>(); // Add the starting cell to the open list GridCell startingCell = maze[startRow, startCol]; openList.Enqueue(startingCell, startingCell.f); while (openList.Count != 0) { GridCell currentSquare = openList.Dequeue(); closedList.Add(currentSquare); if (currentSquare.row == destRow && currentSquare.col == destCol) { return(currentSquare.g); } // Check the four possible neighbour squares GridCell top = (currentSquare.row != 0) ? maze[currentSquare.row - 1, currentSquare.col] : null; GridCell bottom = (currentSquare.row != totalRows - 1) ? maze[currentSquare.row + 1, currentSquare.col] : null; GridCell left = (currentSquare.col != 0) ? maze[currentSquare.row, currentSquare.col - 1] : null; GridCell right = (currentSquare.col != totalCols - 1) ? maze[currentSquare.row, currentSquare.col + 1] : null; // Process each of the potential neighbour cells ProcessCell(currentSquare, top, openList, closedList); ProcessCell(currentSquare, bottom, openList, closedList); ProcessCell(currentSquare, left, openList, closedList); ProcessCell(currentSquare, right, openList, closedList); } return(-1); }
public static void IDAS(State initialState, int size) { SimplePriorityQueue <State> priorityQueue = new SimplePriorityQueue <State>(); priorityQueue.Enqueue(initialState, initialState.Weight); while (priorityQueue.Count != 0) { priorityQueue.TryDequeue(out State currentState); int h = currentState.CalculateHeuristic(); if (h == 0) { currentState.PrintState(); Console.WriteLine($"\nDistance from the goal state -> {currentState.Distance}"); foreach (var movement in currentState.PathDirections) { Console.WriteLine(movement.ToString()); } Console.WriteLine($"Steps count -> {currentState.PathDirections.Count}"); break; } else { GetChildStates(currentState, priorityQueue); } } }
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); }
//So slow, bfs public T GetClosestTile(Vector2 pos, T nullValue) { Vector2 currentIndex = GetTileIndex(pos); Vector2[] dirs = NevPointMap.Dirs; SimplePriorityQueue <Vector2> searchQueue = new SimplePriorityQueue <Vector2>(); HashSet <Vector2> searchedSet = new HashSet <Vector2>(); Action <Vector2> insertIndex = (index) => { searchQueue.Enqueue(index, (GetTileIndexCenter(index) - originPos).magnitude); searchedSet.Add(index); }; insertIndex(currentIndex); while (searchQueue.Count != 0) { Vector2 index = searchQueue.Dequeue(); T tile = GetTileByIndex(index); if (!tile.Equals(nullValue)) { return(tile); } foreach (var dir in dirs) { Vector2 tar = index + dir; if (!searchedSet.Contains(tar)) { insertIndex(tar); } } } return(nullValue); }
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); } }
private void UpdateChunkQueues() { for (int x = -LoadDistance; x <= LoadDistance; x++) { for (int z = -LoadDistance; z <= LoadDistance; z++) { for (int y = 0; y < Height; y++) { ChunkLocation l = new ChunkLocation(centerX + x, y, centerZ + z); if (GetDistanceSquared(l) <= LoadDistance * LoadDistance && !currentlyLoading.Contains(l) && !chunkMap.ContainsKey(l)) { if (chunkLoadQueue.Contains(l)) { chunkLoadQueue.TryUpdatePriority(l, GetDistanceSquared(l, true)); } else { chunkLoadQueue.Enqueue(l, GetDistanceSquared(l, true)); } } } } } foreach (Chunk chunk in chunkMap.Values.Where(c => GetDistanceSquared(c.Location) > UnloadDistance * UnloadDistance)) { chunkUnloadStack.Push(chunk); } }
public bool AStarSearchEx() { sw = Stopwatch.StartNew(); while (openSet.Count > 0) { var current = openSet.Dequeue(); Debug.WriteLine("dequeu" + current.ToString()); if (current.Equals(goal)) { sw.Stop(); return(true); } foreach (var next in graph.Neighbors(current)) { float newG = g[current] + graph.Cost(current, next); if (!g.ContainsKey(next) || newG < g[next]) { g[next] = newG; float h = Heuristic(next, goal); gh[next] = new float[] { newG, h }; //I want to store g and h. float f = newG + h; openSet.Enqueue(next, f); cameFrom[next] = current; } } } sw.Stop(); return(false); }
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 void RegisterEntity(ITimeNode node) { if (!turnQueue.Contains(node)) { turnQueue.Enqueue(node, node.Tick); } }
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps) { bool found = false; path = new List <GraphNode>(); //A* algorithm SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>(); source.Cost = 0; source.Heuristic = Vector3.Distance(source.transform.position, destination.transform.position); nodes.Enqueue(source, source.Cost + source.Heuristic); int steps = 0; while (!found && nodes.Count > 0 && steps++ < maxSteps) { GraphNode node = nodes.Dequeue(); if (node == destination) { found = true; continue; } foreach (GraphNode.Edge edge in node.Edges) { float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position); if (cost < edge.nodeB.Cost) { edge.nodeB.Cost = cost; edge.nodeB.Parent = node; edge.nodeB.Heuristic = Vector3.Distance(edge.nodeB.transform.position, destination.transform.position); nodes.EnqueueWithoutDuplicates(edge.nodeB, edge.nodeB.Cost + edge.nodeB.Heuristic); } } } if (found) { GraphNode node = destination; while (node != null) { path.Add(node); node = node.Parent; } path.Reverse(); } else { while (nodes.Count > 0) { path.Add(nodes.Dequeue()); } } return(found); }
public void Search(IGraph graph) { Path.RemoveRange(_posIndex, Path.Count - _posIndex); var cameFrom = new Dictionary <AbstractTile, AbstractTile>(); var gScore = new Dictionary <AbstractTile, double>(); var closed = new HashSet <AbstractTile>(); var open = new SimplePriorityQueue <AbstractTile, double>(); gScore[Position] = 0; open.Enqueue(Position, Helpers.Metrics.Octile(Position, Target)); // TODO add agitation noise (see Silver) AbstractTile current; while (open.Count > 0) { current = open.Dequeue(); closed.Add(current); if (current == Target) { Path.AddRange(ReconstructPath(cameFrom, current)); return; } foreach (var neighbor in graph.AdjacentVertices(current)) { if (closed.Contains(neighbor)) { continue; } var tentative_gScore = gScore.GetValueOrDefault(current, double.PositiveInfinity) + Helpers.Metrics.Octile(current, neighbor); // TODO add agitation noise (see Silver) if (tentative_gScore < gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { cameFrom[neighbor] = current; gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + Helpers.Metrics.Octile(neighbor, Target); if (!open.TryUpdatePriority(neighbor, fScore)) { open.Enqueue(neighbor, fScore); } } } } }
public static HashSet <Point> FindUnwrappedCellsWithin(Point start, Map map, int maxDistance, bool unwrappedBlocks) { var dist = new Dictionary <Point, int>(); var prev = new Dictionary <Point, Point>(); var Q = new SimplePriorityQueue <Point, int>(); var found = new HashSet <Point>(); dist[start] = 0; Q.Enqueue(start, 0); while (Q.Count > 0) { var u = Q.Dequeue(); if (map.CellAt(u) == Map.CellType.Empty) { found.Add(u); } var alt = dist[u] + 1; if (alt > maxDistance) { continue; } foreach (var v in map.Neighbors(u, unwrappedBlocks)) { if (!dist.ContainsKey(v) || alt < dist[v]) { dist[v] = alt; prev[v] = u; if (Q.Contains(v)) { Q.UpdatePriority(v, alt); } else { Q.Enqueue(v, alt); } } } } ; return(found); }
public static Stack <TileNode> Astar(TileGraph tg, TileNode start, TileNode end) { frontier = new SimplePriorityQueue <TileNode, float>(); frontier.Enqueue(start, 0f); //node_dict = new Dictionary<TileNode, NodeInfo>(); node_dict = DijkstraInitialDictLoad(start, tg); List <TileNode> Expanded = new List <TileNode>(); TileNode v; TileNode other; float cost_so_far; float edge_weight; float dist_to_node; float manhattanDistance; while (frontier.Count > 0) { v = frontier.Dequeue(); cost_so_far = node_dict[v].dist; Expanded.Add(v); List <Edge> experiment = tg.getAdjacentEdges(v) as List <Edge>; foreach (Edge adj_edge in tg.getAdjacentEdges(v)) { other = adj_edge.getNeighbor(v); edge_weight = adj_edge.weight; dist_to_node = node_dict[other].dist; manhattanDistance = Mathf.Abs(end.transform.position.z - other.transform.position.z) + Mathf.Abs(end.transform.position.x - other.transform.position.x); if (cost_so_far + edge_weight < dist_to_node) { node_dict[other].dist = cost_so_far + edge_weight; node_dict[other].heuristic = node_dict[other].dist + manhattanDistance; node_dict[other].parent = v; } if (!Expanded.Any(node => node.isEqual(other)) && !frontier.Any(node => node.isEqual(other))) { frontier.Enqueue(other, node_dict[other].heuristic); } } } Path = NodeDictToPath(node_dict, start, end); return(Path); }
//HashSet<Edge> Edges = new HashSet<Edge>(); public ArtistsConnectionsForm() { InitializeComponent(); var config = SpotifyClientConfig.CreateDefault().WithAuthenticator(new ClientCredentialsAuthenticator("e6bf2e305d98443190c472ee318fd511", "96bad35ecf9c41f581a761eb3a85348b")); Spotify = new SpotifyClient(config); ArtistsToCheck.Enqueue("4JxdBEK730fH7tgcyG3vuv", 0); int n = 0; while (ArtistsToCheck.Count > 0 && ArtistsToCheck.GetPriority(ArtistsToCheck.First) <= 2) { string first = ArtistsToCheck.First; float priority = ArtistsToCheck.GetPriority(first); ArtistsToCheck.Dequeue(); Task <List <string> > artistsTask = FindAllConnectedArtists(first, priority); artistsTask.Wait(); n++; } List <string> edges = new List <string>(); foreach (Edge edge in ArtistsGraph.Edges) { string sourceName; string targetName; ArtistIdToName.TryGetValue(edge.Source, out sourceName); ArtistIdToName.TryGetValue(edge.Source, out targetName); edges.Add(sourceName + " - " + edge.LabelText + " - " + targetName); } ListBox1.DataSource = edges; while (ArtistsToCheck.Count > 0) { string name; ArtistIdToName.TryGetValue(ArtistsToCheck.First, out name); float priority = ArtistsToCheck.GetPriority(ArtistsToCheck.First); ArtistsToCheck.Dequeue(); ListBox3.Items.Add(name + " - " + priority); } foreach (string id in CheckedArtists) { string name; ArtistIdToName.TryGetValue(id, out name); ListBox2.Items.Add(name); } label1.Text = ListBox1.Items.Count.ToString(); label2.Text = ListBox2.Items.Count.ToString(); label3.Text = ListBox3.Items.Count.ToString(); }
public static int FindShortestPathLength(Board board, int player) { SimplePriorityQueue <QueueNode> open = new SimplePriorityQueue <QueueNode>(); bool[,] visited = new bool[Board.BOARD_SIZE, Board.BOARD_SIZE]; int GOAL; if (player > 1) //4 players { GOAL = board.playerStatus[player].goalY; } else { GOAL = board.playerStatus[player].goalX; } QueueNode root = new QueueNode(board.playerStatus[player].x, board.playerStatus[player].y, 0); open.Enqueue(root, F(root)); while (open.Count != 0) { QueueNode current = open.Dequeue(); if ((player > 1 && current.Pos.Y == GOAL) || (player <= 1 && current.Pos.X == GOAL)) { return(current.PathCost); } int x = current.Pos.X; int y = current.Pos.Y; visited[x, y] = true; foreach (Position pos in new List <Position> { new Position(x + 1, y), new Position(x - 1, y), new Position(x, y + 1), new Position(x, y - 1) }) { QueueNode temp = new QueueNode(pos, current.PathCost + 1); if ((pos.X >= 0 && pos.X < Board.BOARD_SIZE) && (pos.Y >= 0 && pos.Y < Board.BOARD_SIZE) && visited[pos.X, pos.Y] == false && !open.Contains(temp) && board.IsPawnMoveLegalSimplified(x, y, pos.X, pos.Y)) { open.Enqueue(temp, F(temp)); } } } return(-1); }
public List <NetworkLane> CalculateRoute(string startID, string endID) { var q = new SimplePriorityQueue <NetworkLaneConnection> (); var distances = new Dictionary <string, double> (); var previous = new Dictionary <string, NetworkLaneConnection> (); NetworkLaneConnection start; if (!connectivityGraph.TryGetValue(startID, out start)) { Debug.Log("Start node was not found!"); return(null); } distances [startID] = 0; q.Enqueue(start, distances [startID]); while (q.Count > 0) { var current = q.Dequeue(); if (current.id == endID) { return(BacktrackRoute(current, previous)); } foreach (string id in current.adjacentLanes) { double newDist = distances [current.id] + current.Weight(id); if (!distances.ContainsKey(id) || newDist < distances [id]) { previous [id] = current; if (!distances.ContainsKey(id)) { q.Enqueue(connectivityGraph [id], newDist); } else { q.TryUpdatePriority(connectivityGraph [id], newDist); } distances [id] = newDist; } } } return(null); }
public SimplePriorityQueue <Generator> FindGeneratorToAccept(ItemOrder io) { int num = io.amount; string item = io.GetItemName(); GameObject[] objs = GameObject.FindGameObjectsWithTag("Generator"); SimplePriorityQueue <Generator> queue = new SimplePriorityQueue <Generator>(); if (objs.Length == 0) { return(queue); } foreach (GameObject go in objs) { Generator gen = go.GetComponent <Generator>(); //if null, continue if (gen == null) { continue; } if (!gen.Operational) { continue; } int index = gen.IngredientIndex(item); //only add to list if it needs this ingredient if (index == -1) { continue; } //only add to list if it has an entrance List <Node> entrancesHere = GetAdjRoadTiles(); List <Node> entrancesThere = gen.GetAdjRoadTiles(); if (entrancesHere.Count == 0 || entrancesThere.Count == 0) { continue; } //only add to list if it has none of this item if (gen.IngredientNeeded(index) <= 0) { continue; } float distance = entrancesHere[0].DistanceTo(entrancesThere[0]); queue.Enqueue(gen, distance); } return(queue); }
/* * A node gets added to the priority queue, and the 'contains' list for the priority * queue */ private void addNode(State <T> node) { openList.Enqueue(node, Convert.ToSingle(node.cost)); if (!openListContains.ContainsKey(node)) { openListContains.Add(node, node.cost); } }
public void StartSearch() { reachedGoal = false; isReplaying = false; isSearching = true; simRenderer.enabled = isGhostVisualized; simPlayer.transform.position = playerMovement.transform.position; priorityQueue.Enqueue(new Node(GetSimPlayerState(new Action(ActionType.WalkR, 0)), null, GetHeuristic(), 0), GetHeuristic()); startTime = Time.unscaledTime; mainPlayerRB.constraints = RigidbodyConstraints2D.FreezeRotation; simPlayerRB.constraints = RigidbodyConstraints2D.FreezeRotation; }
public static List <Node> Search(GridGraph graph, Node start, Node goal) { Dictionary <Node, Node> came_from = new Dictionary <Node, Node>(); Dictionary <Node, float> cost_so_far = new Dictionary <Node, float>(); List <Node> path = new List <Node>(); SimplePriorityQueue <Node> frontier = new SimplePriorityQueue <Node>(); frontier.Enqueue(start, 0); came_from.Add(start, start); cost_so_far.Add(start, 0); Node current = new Node(0, 0); while (frontier.Count > 0) { current = frontier.Dequeue(); if (current == goal) { break; // Early exit } foreach (Node next in graph.Neighbours(current)) { float new_cost = cost_so_far[current] + graph.Cost(next); if (!cost_so_far.ContainsKey(next) || new_cost < cost_so_far[next]) { cost_so_far[next] = new_cost; came_from[next] = current; float priority = new_cost + Heuristic(next, goal); frontier.Enqueue(next, priority); next.Priority = new_cost; } } } while (current != start) { path.Add(current); current = came_from[current]; } path.Reverse(); return(path); }
void Enqueue(Vector v, Vector father) { Tuple <Vector, double, double> t = new Tuple <Vector, double, double> (father, values[father].Item2 + 1, Heuristic(v)); queue.Enqueue(v, (float)(t.Item2 + t.Item3)); values[v] = t; }
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 List <Action> GetShortestPath(State fromState, State toState) { SimplePriorityQueue <SearchNode <State, Action>, float> frontier = new SimplePriorityQueue <SearchNode <State, Action>, float>(); HashSet <State> exploredSet = new HashSet <State>(); Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >(); SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action)); frontier.Enqueue(startNode, 0); frontierMap.Add(fromState, startNode); while (true) { if (frontier.Count == 0) { return(null); } SearchNode <State, Action> node = frontier.Dequeue(); if (node.state.Equals(toState)) { return(BuildSolution(node)); } exploredSet.Add(node.state); // expand node and add to frontier foreach (Action action in info.Expand(node.state)) { State child = info.ApplyAction(node.state, action); SearchNode <State, Action> frontierNode = null; bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode); if (!exploredSet.Contains(child) && !isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); frontier.Enqueue(searchNode, searchNode.f); exploredSet.Add(child); } else if (isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); if (frontierNode.f > searchNode.f) { //frontier.Replace(frontierNode, frontierNode.f, searchNode.f); frontier.UpdatePriority(frontierNode, searchNode.f); } } } } }
public AstarResult Astar(int start, int end) { if (start >= this.NodesCount() || end >= this.NodesCount()) { throw new ArgumentException("Nodes not in graph"); } var gScore = new Dictionary <int, (double distance, int prev)> { { start, (0, -1) } }; HashSet <int> closedset = new HashSet <int>(); var openset = new SimplePriorityQueue <int, double>(); openset.Enqueue(start, 0 + this.HeuristicDistance(this.Nodes[start], this.Nodes[end])); while (openset.Count > 0) { int u = openset.Dequeue(); closedset.Add(u); if (u == end) { return(GenAstarResult(end, start, gScore)); } foreach (Edge e in this.OutEdges(u)) { if (closedset.Contains(e.To)) { continue; } if (!openset.Contains(e.To)) { gScore.Add(e.To, (double.MaxValue, -1)); openset.Enqueue(e.To, double.MaxValue); } double ndist = gScore[u].distance + e.Value * this.HeuristicDistance(this.Nodes[u], this.Nodes[e.To]); if (gScore[e.To].distance > ndist) { gScore[e.To] = (ndist, u); openset.UpdatePriority(e.To, ndist + this.HeuristicDistance(this.Nodes[e.To], this.Nodes[end])); } } } throw new Exception("Not Connected Nodes"); }
public static Queue <Point> GetPath(Point start, Point destination) { var openSet = new SimplePriorityQueue <Point>(); openSet.Enqueue(start, 0); var closedSet = new HashSet <Point>(); var cameFrom = new Dictionary <Point, Point>(); var costSoFar = new Dictionary <Point, float>(); costSoFar[start] = 0; while (openSet.Count > 0) { var current = openSet.Dequeue(); if (current == destination) { return(ConstructPath(cameFrom, current)); } closedSet.Add(current); foreach (var neighbour in GetNeighbours(current)) { if (closedSet.Contains(neighbour)) { continue; } float cost = costSoFar[current] + Distance(current, neighbour); if (!costSoFar.ContainsKey(neighbour) || cost < costSoFar[neighbour]) { costSoFar[neighbour] = cost; openSet.Enqueue(neighbour, cost); cameFrom[neighbour] = current; } } } return(null); }
/// <summary> /// Performs an A* search following the Node Array A* implementation /// </summary> public Path FindPath(IMap map, int start, int target) { this.isGoal = nodeId => nodeId == target; this.calculateHeuristic = nodeId => map.GetHeuristic(nodeId, target); this.map = map; var heuristic = calculateHeuristic(start); var startNode = new AStarNode(start, 0, heuristic, CellStatus.Open); var openQueue = new SimplePriorityQueue<int>(); openQueue.Enqueue(start, startNode.F); // The open list lookup is indexed by the number of nodes in the graph/map, // and it is useful to check quickly the status of any node that has been processed var nodeLookup = new AStarNode?[map.NrNodes]; nodeLookup[start] = startNode; while (openQueue.Count != 0) { var nodeId = openQueue.Dequeue(); var node = nodeLookup[nodeId].Value; if (isGoal(nodeId)) { return ReconstructPath(nodeId, nodeLookup); } ProcessNeighbours(nodeId, node, nodeLookup, openQueue); // Close the node. I hope some day the will implement something // like the records in F# with the "with" keyword nodeLookup[nodeId] = new AStarNode(node.Parent, node.G, node.H, CellStatus.Closed); } // No path found. We could return a null, but since I read the book "Code Complete" I decided // its best to return an empty path, and I'll return a -1 as PathCost // TODO: Additionally, all those magic numbers like this -1 should be converted to explicit, // clearer constants return new Path(new List<int>(), -1); }
/// <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; } } }
public Path_AStar(World world, Tile tileStart, Tile tileEnd) { // Check to see if we have a valid tile graph if(world.tileGraph == null) { world.tileGraph = new Path_TileGraph(world); } // A dictionary of all valid, walkable nodes. Dictionary<Tile, Path_Node<Tile>> nodes = world.tileGraph.nodes; // Make sure our start/end tiles are in the list of nodes! if(nodes.ContainsKey(tileStart) == false) { Debug.LogError("Path_AStar: The starting tile isn't in the list of nodes!"); return; } if(nodes.ContainsKey(tileEnd) == false) { Debug.LogError("Path_AStar: The ending tile isn't in the list of nodes!"); return; } Path_Node<Tile> start = nodes[tileStart]; Path_Node<Tile> goal = nodes[tileEnd]; // Mostly following this pseusocode: // https://en.wikipedia.org/wiki/A*_search_algorithm List<Path_Node<Tile>> ClosedSet = new List<Path_Node<Tile>>(); /* List<Path_Node<Tile>> OpenSet = new List<Path_Node<Tile>>(); OpenSet.Add( start ); */ SimplePriorityQueue<Path_Node<Tile>> OpenSet = new SimplePriorityQueue<Path_Node<Tile>>(); OpenSet.Enqueue( start, 0); Dictionary<Path_Node<Tile>, Path_Node<Tile>> Came_From = new Dictionary<Path_Node<Tile>, Path_Node<Tile>>(); Dictionary<Path_Node<Tile>, float> g_score = new Dictionary<Path_Node<Tile>, float>(); foreach(Path_Node<Tile> n in nodes.Values) { g_score[n] = Mathf.Infinity; } g_score[ start ] = 0; Dictionary<Path_Node<Tile>, float> f_score = new Dictionary<Path_Node<Tile>, float>(); foreach(Path_Node<Tile> n in nodes.Values) { f_score[n] = Mathf.Infinity; } f_score[ start ] = heuristic_cost_estimate( start, goal ); while( OpenSet.Count > 0 ) { Path_Node<Tile> current = OpenSet.Dequeue(); if(current == goal) { // We have reached our goal! // Let's convert this into an actual sequene of // tiles to walk on, then end this constructor function! reconstruct_path(Came_From, current); return; } ClosedSet.Add(current); foreach(Path_Edge<Tile> edge_neighbor in current.edges) { Path_Node<Tile> neighbor = edge_neighbor.node; if( ClosedSet.Contains(neighbor) == true ) continue; // ignore this already completed neighbor float movement_cost_to_neighbor = neighbor.data.movementCost * dist_between(current, neighbor); float tentative_g_score = g_score[current] + movement_cost_to_neighbor; if(OpenSet.Contains(neighbor) && tentative_g_score >= g_score[neighbor]) continue; Came_From[neighbor] = current; g_score[neighbor] = tentative_g_score; f_score[neighbor] = g_score[neighbor] + heuristic_cost_estimate(neighbor, goal); if(OpenSet.Contains(neighbor) == false) { OpenSet.Enqueue(neighbor, f_score[neighbor]); } } // foreach neighbour } // while // If we reached here, it means that we've burned through the entire // OpenSet without ever reaching a point where current == goal. // This happens when there is no path from start to goal // (so there's a wall or missing floor or something). // We don't have a failure state, maybe? It's just that the // path list will be null. }
/// <summary> /// Generar y retornar el árbol SPF de la topología en un proyecto, tomando el parámetro como punto de origen /// </summary> /// <param name="idRouterOrigen"></param> /// <param name="idProyecto"></param> /// <param name="minBW"></param> /// <returns></returns> public static List<NodoDijkstra> GenerarRutas(NodoDijkstra idRouterOrigen, int idProyecto, double minBW, int nTipoMetrica = 2, int idAfinidad = 0) { idRouterOrigen.nMinDistancia = 0.0; SimplePriorityQueue<NodoDijkstra> routerQueue = new SimplePriorityQueue<NodoDijkstra>(); routerQueue.Enqueue(idRouterOrigen, 1); //mantiene el registro de todos los nodos de la topologia por el que se pasa List<NodoDijkstra> routerList = new List<NodoDijkstra>(); routerList.Add(idRouterOrigen); while (routerQueue.Count > 0) { NodoDijkstra currentRouter = routerQueue.Dequeue(); //Visita cada enlace adyacente al router u foreach (var enlace in currentRouter.listaEnlacesDijkstra) { int idRouterVecino = 0; //Fix: Asegurandose de que se use el id del router adyacente en el enlace if (enlace.idRouterB != currentRouter.idRouter) { idRouterVecino = enlace.idRouterB; //enlace.target = enlace.targetB; } else { idRouterVecino = enlace.idRouterA; //enlace.target = enlace.targetA; } //NodoDijkstra vecino = new NodoDijkstra(idRouterVecino, idProyecto); NodoDijkstra vecino = enlace.target; double nPesoBandwidth = 0; switch(nTipoMetrica) //ignore var name, aqui va lo del tipo de peso { case 1: //Pesos Administrativos nPesoBandwidth = enlace.nPesoAdministrativo; break; case 2: //Minima Cantidad de Saltos nPesoBandwidth = 1; break; case 3: // 1/BW Reservado nPesoBandwidth = 1.00 / (enlace.nBandwidth - enlace.nBandwidthDisponible); break; case 4: // 1/BW Disponible nPesoBandwidth = 1.00 / enlace.nBandwidthDisponible; break; default: nPesoBandwidth = 1; break; } double nDistanciaTotal = currentRouter.nMinDistancia + nPesoBandwidth; //Aqui ocurre el filtro por afinidad if (idAfinidad == 0) //No afinidad definida { //En este if ocurre el filtro por BW disponible if (nDistanciaTotal < vecino.nMinDistancia && minBW < enlace.nBandwidth) //Constraint check { if (routerQueue.Contains(vecino)) routerQueue.Remove(vecino); vecino.nMinDistancia = nDistanciaTotal; vecino.idRouterPrevio = currentRouter; enlace.nBandwidthDisponible -= minBW; //reservar el BW en el enlace routerQueue.Enqueue(vecino, 1); } } else //Afinidad definida { if (idAfinidad == enlace.idAfinidad) //Afinidad check { //En este if ocurre el filtro por BW disponible if (nDistanciaTotal < vecino.nMinDistancia && minBW < enlace.nBandwidth) //Constraint check { if (routerQueue.Contains(vecino)) routerQueue.Remove(vecino); vecino.nMinDistancia = nDistanciaTotal; vecino.idRouterPrevio = currentRouter; enlace.nBandwidthDisponible -= minBW; //reservar el BW en el enlace routerQueue.Enqueue(vecino, 1); } } } //Agrega el router (bueno, los 2) al registro int indexTarget = routerList.FindIndex(n => n.idRouter == vecino.idRouter); if (indexTarget != -1) routerList[indexTarget] = vecino; else routerList.Add(vecino); int indexSource = routerList.FindIndex(n => n.idRouter == currentRouter.idRouter); if (indexSource != -1) routerList[indexSource] = currentRouter; else routerList.Add(currentRouter); } } return routerList; }
/// <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; }