public void FindPath(Grid _grid) { Node start = _grid.StartNode; Node end = _grid.EndNode; open = new Heap<Node>(_grid.GridMaxSize); close = new HashSet<Node>(); open.Add(start); while (open.Count > 0) { Node current = open.GetFirst(); if (current.GridBlock.Equals(end.GridBlock)) return; foreach(Node p in _grid.GetNeighbours(current)) { if (p.GridBlock.Type != GridBlock.BlockType.Obstacle || close.Contains(p)) continue; int gCost = current.gCost + GetDistance(current, p); if(gCost < current.gCost || !open.Contains(p)) { p.gCost = gCost; p.hCost = GetDistance(current, p); p.Parent = current; if (!open.Contains(p)) open.Add(p); } } } }
List<Point> FindRoadPath(Road a, Road b, RoadType T) { AstarObject[,] Set = new AstarObject[14, 9]; for (int x = 0; x < 14; x++) { for (int y = 0; y < 9; y++) { Set[x, y] = new AstarObject(x, y, this); } } Heap<AstarObject> OpenSet = new Heap<AstarObject>(14 * 9); HashSet<AstarObject> ClosedSet = new HashSet<AstarObject>(); AstarObject Start = Set[a.X, a.Y]; AstarObject End = Set[b.X, b.Y]; OpenSet.Add(Start); while (OpenSet.Count > 0) { AstarObject CurrentLocation = OpenSet.RemoveFirst(); ClosedSet.Add(CurrentLocation); if (CurrentLocation == End) { return RetracePath(Start, End); //Retracepath and stuff. } List<AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set, NeighbourhoodType.Neumann, MapsizeXR, MapsizeYR); foreach (AstarObject neighbour in Neighbours) { if (neighbour.RType != T || ClosedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, End); neighbour.parent = CurrentLocation; if (!OpenSet.Contains(neighbour)) { OpenSet.Add(neighbour); } else { OpenSet.UpdateItem(neighbour); } } } } return new List<Point>(); }
/* 1 Implement a class PriorityQueue<T> based * on the data structure "binary heap". * */ static void Main(string[] args) { var heap = new Heap<int>(); heap.Add(1); heap.Add(2); heap.Add(3); Debug.Assert(heap.SameContents(new[] { 1, 2, 3 })); Console.WriteLine(string.Join(",", heap)); Debug.Assert(heap.ChopHead() == 3); Debug.Assert(heap.ChopHead() == 2); Debug.Assert(heap.ChopHead() == 1); Debug.Assert(heap.IsEmpty); // higher string means lower priority var pqueue = new PriorityQueue<string, string>((s1, s2) => -s1.CompareTo(s2)); pqueue.Enqueue("18:00", "Buy food"); pqueue.Enqueue("06:00", "Walk dog"); pqueue.Enqueue("21:00", "Do homework"); pqueue.Enqueue("09:00", "Go to work"); pqueue.Enqueue("21:00", "Drink beer"); Debug.Assert(pqueue.Count == 5); Debug.Assert(pqueue.Dequeue() == "Walk dog"); Debug.Assert(pqueue.Dequeue() == "Go to work"); Debug.Assert(pqueue.Dequeue() == "Buy food"); Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue())); Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue())); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print ("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem (neighbour); } } } } yield return null; if (pathSuccess) { waypoints = RetracePath(startNode,targetNode); } requestManager.FinishedProcessingPath(waypoints,pathSuccess); }
void FindPath(Vector3 startPos, Vector3 targetPos) { // performs A* search on the grid to find a path startPos = new Vector3(startPos.x + 8.0f, 0, startPos.z - 2.0f); // offsets targetPos = new Vector3(targetPos.x + 8.0f, 0, targetPos.z - 2.0f); // offsets Node startNode = grid.GetNodeFromWorldPoint(startPos); Node targetNode = grid.GetNodeFromWorldPoint(targetPos); if (targetNode.walkable == false) return; // don't try to path find if we're on an unwalkable area Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while(openSet.Count > 0) { // we still have nodes Node currentNode = openSet.pop(); closedSet.Add(currentNode); if(currentNode == targetNode) { // we've found exit RetracePath(startNode, targetNode); path = backTrackPath(startNode, targetNode); return; } foreach(Node n in grid.GetNeighbours(currentNode)) { if (!n.walkable || closedSet.Contains(n)) continue; int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n); if(newMovementCostToNeighbour < n.gCost || !openSet.contains(n)) { n.gCost = newMovementCostToNeighbour; n.hCost = GetDistance(n, targetNode); n.parent = currentNode; if (!openSet.contains(n)) openSet.Add(n); // add our neighbour into open set else openSet.UpdateItem(n); } } } }
public void Simple() { var heap = new Heap<int>(HeapType.Minimum) { 5 }; Assert.AreEqual(heap.Count, 1); Assert.IsFalse(heap.IsEmpty); Assert.AreEqual(heap.Root, 5); heap.Add(2); Assert.AreEqual(heap.Count, 2); Assert.IsFalse(heap.IsEmpty); Assert.AreEqual(heap.Root, 2); heap.Add(3); Assert.AreEqual(heap.Count, 3); Assert.IsFalse(heap.IsEmpty); Assert.AreEqual(heap.Root, 2); Assert.AreEqual(heap.RemoveRoot(), 2); heap.Add(1); Assert.AreEqual(heap.Count, 3); Assert.IsFalse(heap.IsEmpty); Assert.AreEqual(heap.Root, 1); }
public void aStar(Vector3 startPos, Vector3 endPos) { Stopwatch watch = new Stopwatch(); watch.Start(); //List<Node_K> openSet = new List<Node_K>(); Heap<Node_K> openSet = new Heap<Node_K>(AIarea.GetMaxSize); HashSet<Node_K> closedSet = new HashSet<Node_K>(); Node_K sourceNode = AIarea.getNodeAtPos(startPos); Node_K targetNode = AIarea.getNodeAtPos(endPos); openSet.Add(sourceNode); while(openSet.Count > 0) { Node_K currentNode = openSet.RemoveFirst(); //Node_K currentNode = openSet[0]; //for (int i = 1; i < openSet.Count; i++) //{ // if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost) // { // currentNode = openSet[i]; // } //} //openSet.Remove(currentNode); closedSet.Add(currentNode); if(currentNode == targetNode) { reversePath(sourceNode, targetNode); watch.Stop(); time = watch.ElapsedMilliseconds.ToString(); print("Astar " + watch.ElapsedMilliseconds + "ms"); return; } List<Node_K> neighbors = new List<Node_K>(); neighbors = AIarea.neighbors(currentNode); for (int i = 0; i < neighbors.Count; i++) { if (neighbors[i].walkable == false || closedSet.Contains(neighbors[i])) continue; int cost = currentNode.gCost + moveCost(currentNode, neighbors[i]); if (!openSet.Contains(neighbors[i]) || (cost < neighbors[i].gCost)) { neighbors[i].gCost = cost; neighbors[i].hCost = moveCost(neighbors[i], targetNode); neighbors[i].parent = currentNode; if (!openSet.Contains(neighbors[i])) openSet.Add(neighbors[i]); else openSet.UpdateItem(neighbors[i]); } } } }
//Generate path from start to destination using A* public Path findPath(Vector3 startPos, Vector3 targetPos, int steps) { //Convert 3D Positions to GridNodes GridNode startNode = grid.getNode(startPos); GridNode targetNode = grid.getNode(targetPos); //Exit if nodes are out of bounds if (startNode == null || targetNode == null) { return null; } //Instantiate Open and Closed set DataStructures for A* Algorithm HashSet<GridNode> closedSet = new HashSet<GridNode>(); Heap<GridNode> openSet = new Heap<GridNode>(grid.MaxSize); openSet.Add(startNode); //Add start node to the open set int count = 0; //Iterate through as long as openset is not empty while(openSet.Count > 0 && count++ < steps) { //Store first node in collection GridNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); //Check if arrived at target node if(currentNode == targetNode) { //Retrace and get coordinates List<GridNode> points = retracePath(startNode, targetNode); //Convert to a path return getPath(points); } //Iterate through all neighbor nodes foreach(GridNode neighbor in grid.GetNeighbors(currentNode)) { //Skip node if not walkable or already used if (!neighbor.isWalkable() || closedSet.Contains(neighbor)) { continue; } //Calculate the move cost to the neighbor nodes int moveCost = currentNode.gCost + GridNode.getDistance(currentNode, neighbor); //Calculate neighbor's costs if(moveCost < neighbor.gCost || !openSet.Contains(neighbor)) { //Set Costs neighbor.gCost = moveCost; neighbor.hCost = GridNode.getDistance(neighbor, targetNode); neighbor.parent = currentNode; //If the open set doesn't already contain the neighboring node, add it if (!openSet.Contains(neighbor)) openSet.Add(neighbor); } } } //Unable to find a path return null; }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorlPoint (startPos); Node targetNode = grid.NodeFromWorlPoint (targetPos); if (startNode.Walkable && targetNode.Walkable) { Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.Walkable || closedSet.Contains(neighbour)) continue; int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.Parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else openSet.UpdateItem(neighbour); } } } yield return null; if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); } else { requestManager.FinishedProcessingPath(waypoints, pathSuccess); yield break; } }
//A* implementation public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){ #if DEBUG Stopwatch sw = new Stopwatch(); sw.Start(); #endif var openList = new Heap<PathNode>(allNodes.Length); var closedList = new HashSet<PathNode>(); var success = false; openList.Add(startNode); while (openList.Count > 0){ var currentNode = openList.RemoveFirst(); closedList.Add(currentNode); if (currentNode == endNode){ #if DEBUG sw.Stop(); //UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms."); #endif success = true; break; } foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){ if (closedList.Contains(neighbour)) continue; var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour ); if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){ neighbour.gCost = costToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)){ openList.Add(neighbour); openList.UpdateItem(neighbour); } } } } yield return null; if (success){ callback( RetracePath(startNode, endNode), true ); } else { callback( new Vector2[0], false ); } }
public void AddExample() { var heap = new Heap<string>(HeapType.Minimum); heap.Add("cat"); heap.Add("dog"); heap.Add("canary"); // There should be 3 items in the heap. Assert.AreEqual(3, heap.Count); }
Vector2[] FindPath(Vector2 from, Vector2 to) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector2[] waypoints = new Vector2[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(from); Node targetNode = grid.NodeFromWorldPoint(to); startNode.parent = startNode; if (startNode.walkable && targetNode.walkable) { Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print ("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour)+TurningCost(currentNode,neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } } if (pathSuccess) { waypoints = RetracePath(startNode,targetNode); } return waypoints; }
void FindPath(Vector2 startPos, Vector2 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while(openSet.Count >0 ) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode== targetNode){ sw.Stop(); print("Path found" + sw.ElapsedMilliseconds + "ms"); RetracePath(startNode,targetNode); return; } //check if neighbour is walkable foreach(Node neighbour in grid.GetNeighbours(currentNode)) { if(!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if( newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)){ neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { //Create and start stopwatch to log method completion time Stopwatch sw = new Stopwatch(); sw.Start(); //Initialize waypoints array Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; //Determine the start and target node based on the Gameobjects Vector 3 positions Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if(startNode.walkable && targetNode.walkable) { Heap<Node> openSet = new Heap<Node>(grid.MaxSize); //Holds all the nodes in the grid HashSet<Node> closedSet = new HashSet<Node>(); //Used to store nodes that are openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print("Path Found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach(Node neighbour in grid.GetNeighbours(currentNode)) { if(!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if(newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } } yield return null; if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
void CreatePath(Node startNode, Node targetNode) { bool pathSuccess = false; int traffic; int trafficCap; openSet = new Heap<Node>(grid.RoadSet.Count); closedSet = new HashSet<Node>(); openSet.Add(startNode); while (openSet.Count > 0) { // remove the smallest fCost from Heap Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.PathGetRoadNeighbours(currentNode)) { if (closedSet.Contains(neighbour)) continue; lock (currentNode) traffic = currentNode.trafficTimer.TrafficIntensity; trafficCap = currentNode.trafficTimer.InitialTrafficIntensity; int penalty = 17 - 17 * traffic / (trafficCap + 17/trafficCap); int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbour) + penalty; if (newMovementCostToNeighbor < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbor; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } requestManager.FinishedProcessingPath(ReverseParents(startNode, targetNode), pathSuccess); }
public bool FindPathImmediate(GridNode start, GridNode end, List<List<GridNode>> grid, out List<GridNode> path) { path = new List<GridNode>(); bool success = false; if (start.walkable && end.walkable) { Heap<GridNode> openSet = new Heap<GridNode>(grid.Count * grid[0].Count); HashSet<GridNode> closedSet = new HashSet<GridNode>(); openSet.Add(start); while (openSet.Count > 0) { GridNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); //path found if (currentNode == end) { success = true; break; } foreach (GridNode neighbour in currentNode.GetNeighbours()) { if (!neighbour.walkable || closedSet.Contains(neighbour)) continue; int newNeighbourMoveCost = currentNode.gCost + GetDistanceManhatten(currentNode, neighbour); if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newNeighbourMoveCost; neighbour.hCost = GetDistanceManhatten(neighbour, end); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } } if (success) { path = RetracePath(start, end); } return success; }
IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition)//here we get 2 positions from the game and check on wich node they are, we do that with NodeFromWorldPoint wich we have in the grid script { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPosition);//this is going to be the startng point f the path Node targetNode = grid.NodeFromWorldPoint(targetPosition);//this wil be the base or player etc, just the target if (startNode.walkable && targetNode.walkable) { Heap<Node> openSet = new Heap<Node>(grid.MaxSize);//list of nodes that havent been checked yet HashSet<Node> closedSet = new HashSet<Node>();//a list for the nodes that have been checked openSet.Add(startNode);//the verry first node that wil be checked, this is the starting position while (openSet.Count > 0)//as long as there are nodes to be checked, go on :) { Node currentNode = openSet.RemoveFirst();//first to be checked a closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); } } } } yield return null; if (pathSuccess) { waypoints = RetracePath(startNode,targetNode); } requestManager.FinishedProcessingPath(waypoints,pathSuccess); }
IEnumerator CalculatePath(Vector3 startPosition, Vector3 targetPosition, LongPathGrid longPathGrid) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; GridNode startNode = longPathGrid.GetGridNodeFromWorldPoint(startPosition); GridNode targetNode = longPathGrid.GetGridNodeFromWorldPoint(targetPosition); if (targetNode.walkable) { Heap<GridNode> openSet = new Heap<GridNode>(longPathGrid.MaxSize); HashSet<GridNode> closedSet = new HashSet<GridNode>(); openSet.Add(startNode); while(openSet.Count > 0) { GridNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (GridNode neighbour in longPathGrid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if(!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } } yield return null; if (pathSuccess) waypoints = RetracePath(startNode, targetNode); navGridManager.FinishProcessingLongPath(waypoints,pathSuccess); }
void FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch (); sw.Start (); Node startNode = grid.NodeFromWorldPoint (startPos); Node targetNode = grid.NodeFromWorldPoint (targetPos); //List<Node> openSet = new List<Node>(); Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node> (); openSet.Add (startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); // for (int i = 1; i < openSet.Count; i++) { // if (openSet [i].fCost < currentNode.fCost || // openSet[i].fCost == currentNode.fCost && // openSet[i].hCost < currentNode.hCost) { // currentNode = openSet [i]; // } // } // openSet.Remove (currentNode); closedSet.Add (currentNode); if (currentNode == targetNode) { // retrace steps to get the path using parent sw.Stop(); print("Path found in " + sw.ElapsedMilliseconds + " ms"); RetracePath(startNode,targetNode); return; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance (currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains (neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance (neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains (neighbour)) openSet.Add (neighbour); } } } }
public List<Portal> FindPath() { Stopwatch sw = new Stopwatch(); sw.Start(); bool success = false; CreateStartNode(startPos); CreateEndNode(targetPos); if (startPortal == null || endPortal == null) return null; Heap<Portal> frontier = new Heap<Portal>(portals.Count+2); HashSet<Portal> visited = new HashSet<Portal>(); frontier.Add(startPortal); while(frontier.size() > 0){ Portal current = frontier.RemoveFirst(); visited.Add(current); if (current == endPortal){ sw.Stop(); success = true; print("Path found in: "+ sw.ElapsedMilliseconds + " ms"); break; } foreach(KeyValuePair<Portal, int> nb in current.GetNeighbours()){ if (visited.Contains(nb.Key)){ continue; } int newDistance = current.gCost + nb.Value; if (newDistance < nb.Key.gCost || !frontier.Contains(nb.Key)){ nb.Key.gCost = newDistance; nb.Key.hCost = CalcDistance(nb.Key); nb.Key.SetParent(current); if (!frontier.Contains(nb.Key)){ frontier.Add(nb.Key); } } } } if (success){ return RetracePath(); } else return null; }
public List<Node> FindPath(Node start, Node destination) { // an open set to store nodes with the potentional to lead towards the goal openSet = new Heap<Node>(game.nodeGrid.nodesInGridWidth * game.nodeGrid.nodesInGridHeight); // a closed set to keep track of nodes already expanded from closedSet = new HashSet<Node>(); openSet.Add(start); // as long as the openset contains nodes we keep searching a path while (openSet.Count > 0) { // get the node with the lowest F value Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); // if we find the goal, retrace our path and send it to the requester if (currentNode == destination) return RetracePath(start, destination); // expand from the current node, get neighbours in all 8 directions foreach (Node neighbour in game.nodeGrid.GetAllNeighbours(currentNode)) { if (neighbour.blocked || closedSet.Contains(neighbour)) continue; // the new cost is the current Node G value + the distance to the neighbour (either 10 or 14) int newGCost = currentNode.G + GetDistance(currentNode, neighbour); if (newGCost < neighbour.G || !openSet.Contains(neighbour)) { neighbour.G = newGCost; neighbour.H = GetDistance(neighbour, destination); neighbour.parent = currentNode; // the openSet maintains itself in such away that the node with the lowest F value is always the first element. if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } // we went through every node we could reach and havent found the goal. it is unreachable return null; }
//Calculates the path from a start to finish using the A* Pathfinding algorithm IEnumerator CalculatePath(Vector3 startPosition, Vector3 endPosition) { Vector3[] waypoints = new Vector3[0]; bool success = false; Node startNode = grid.NodeFromPoint(startPosition);//Creates a node from the point Node endNode = grid.NodeFromPoint(endPosition); if (endNode.Walkable ) { //See if the end node is a walkable object or it is a resource Heap<Node> open = new Heap<Node> (grid.MaxGridSize); HashSet<Node> closed = new HashSet<Node> (); open.Add (startNode); while (open.Count > 0) { Node current = open.Pop ();//Removes first node in heap and re-sorts it closed.Add (current);//Adds the node to the closed list if (current == endNode) { success = true; break; } foreach (Node neighbour in grid.GetNeighbours(current)) {//Traverses through the neighbours of the nodes if (!neighbour.Walkable || closed.Contains (neighbour)) { continue; } int costToNeighbour = current.G + GetDist (current, neighbour); if (costToNeighbour < neighbour.G || !open.Contains (neighbour)) {//If the cost to neighbour < neighbouring g value neighbour.G = costToNeighbour; neighbour.H = GetDist (neighbour, endNode); neighbour.Parent = current; if (!open.Contains (neighbour)) { open.Add (neighbour); //Adds the neighbour to the heap } else{ open.UpdateItem(neighbour); // Resort the heap to include the neighbour } } } } } yield return null;//Makes wait for one frame if (success) // If successfully calculates path { waypoints = ReversePath(startNode, endNode); //reverse path since path is stored from end to beginning } request.FinishedProcessing(waypoints, success); }
void FindPath(Vector3 startPos, Vector3 targetPos) { Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); Heap<Node> openSet = new Heap<Node>(grid.MaxSize); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { RetracePath(startNode,targetNode); return; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); } } } }
public void ComparerDelegateWithIntialSize() { var heap = new Heap<SimpleClass>(HeapType.Maximum, 10, (x, y) => x.TestProperty.CompareTo(y.TestProperty)) { new SimpleClass("5") }; Assert.AreEqual(heap.Count, 1); Assert.AreEqual(heap.Root.TestProperty, "5"); heap.Add(new SimpleClass("2")); Assert.AreEqual(heap.Count, 2); Assert.AreEqual(heap.Root.TestProperty, "5"); heap.Add(new SimpleClass("3")); Assert.AreEqual(heap.Count, 3); Assert.AreEqual(heap.Root.TestProperty, "5"); Assert.AreEqual(heap.RemoveRoot().TestProperty, "5"); heap.Add(new SimpleClass("1")); Assert.AreEqual(heap.Count, 3); Assert.AreEqual(heap.Root.TestProperty, "3"); }
public void BuildTest() { // Build a heap Heap<int, int> heap = new Heap<int, int>((i) => i); int size = 255; Random rnd = new Random(); for (int index = 0; index < size; index++) { heap.Add(rnd.Next(0,255)); } // validate Assert.IsFalse(heap.IsEmpty); Assert.AreEqual<int>(8, GetMaxDepth(heap.root)); Assert.AreEqual<int>(8, GetMinDepth(heap.root)); Assert.IsTrue(IsHeap(heap.root)); }
public void Stress() { var heap = new Heap<int>(HeapType.Minimum); const int maximum = 5000; for (var i = maximum; i > 0; i--) { heap.Add(i); Assert.AreEqual(heap.Root, i); } for (var i = 1; i <= maximum; i++) { Assert.AreEqual(heap.RemoveRoot(), i); } }
public void Simple() { var heap = new Heap<int>(HeapType.Minimum); for (var i = 20; i > 0; i--) { heap.Add(i); Assert.AreEqual(heap.Root, i); } Assert.IsFalse(heap.IsEmpty); Assert.AreEqual(heap.Count, 20); heap.Clear(); Assert.AreEqual(heap.Count, 0); Assert.IsTrue(heap.IsEmpty); }
public void TestHeap() { Heap<int> heap = new Heap<int>(); heap.Add(6); heap.Add(2); heap.Add(7); heap.Add(1); heap.Add(4); heap.Add(5); heap.Add(3); Assert.AreEqual(1, heap.ExtractDominating()); Assert.AreEqual(2, heap.ExtractDominating()); Assert.AreEqual(3, heap.ExtractDominating()); Assert.AreEqual(4, heap.ExtractDominating()); Assert.AreEqual(5, heap.ExtractDominating()); Assert.AreEqual(6, heap.ExtractDominating()); Assert.AreEqual(7, heap.ExtractDominating()); }
/// <summary> /// Finds the connections. /// </summary> /// <param name="start">Start.</param> public void FindConnections(Node start) { Tile startTile = TileFromPosition (start.center); Dictionary<Tile, Node> targets = new Dictionary<Tile, Node> (); foreach (KeyValuePair<Vector3, Node> kv in nodes) { if (kv.Value != start) targets.Add (TileFromPosition (kv.Key), kv.Value); } if (startTile.cost == 255) return; Heap<Tile> frontier = new Heap<Tile> (sizeX * sizeY); HashSet<Tile> visited = new HashSet<Tile> (); frontier.Add (startTile); while (frontier.size () > 0) { Tile current = frontier.RemoveFirst (); visited.Add (current); Node intranode = null; targets.TryGetValue (current, out intranode); if (intranode != null && !intranode.ConnectedTo (start)) { Node.Connect (start, intranode, current.distance); } foreach (Tile neighbour in GetNeighbours(current)) { if (neighbour == null || neighbour.cost == 255 || visited.Contains (neighbour)) { continue; } neighbour.distance = current.distance + 1; if (!frontier.Contains (neighbour)) frontier.Add (neighbour); } } }
#pragma warning restore 649 public void FindPath(PathRequest request, Action <PathResult> callback) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; PathNode startNode = _grid.NodeFromWorldPoint(request.pathStart); PathNode targetNode = _grid.NodeFromWorldPoint(request.pathEnd); if (startNode.Walkable && targetNode.Walkable) { Heap <PathNode> openSet = new Heap <PathNode>(_grid.MaxSize); HashSet <PathNode> closedSet = new HashSet <PathNode>(); openSet.Add(startNode); while (openSet.Count > 0) { PathNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { /*sw.Stop(); * UnityEngine.Debug.Log("Path found in " + sw.Elapsed);*/ pathSuccess = true; break; } foreach (PathNode neighbour in _grid.GetNeighbours(currentNode)) { if (!neighbour.Walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.GCost + Mathf.RoundToInt(GetDistance(currentNode, neighbour) * neighbour.MovementPenalty); if (newMovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) { neighbour.GCost = newMovementCostToNeighbour; neighbour.HCost = GetDistance(neighbour, targetNode); neighbour.Parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); } }
/// <summary> /// Finds the shortest path with help of nodes, and according to the A-star algorithm /// </summary> /// <param name="request"></param> /// <param name="callback"></param> public void FindPath(PathRequest request, Action <PathResult> callback) { NodeGrid nodeGrid = NodeGrid.Instance; Vector3[] waypoints = new Vector3[0]; bool pathFound = false; Node startNode = nodeGrid.NodeFromWorldPoint(request.pathStart); Node targetNode = nodeGrid.NodeFromWorldPoint(request.pathEnd); if (startNode == null || targetNode == null) { callback(new PathResult(new Vector3[0], false, request.callback)); } else if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node activeNode = openSet.RemoveFirst(); closedSet.Add(activeNode); if (activeNode == targetNode) { pathFound = true; break; } foreach (Node neighbour in nodeGrid.GetNeighbours(activeNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newCostToNeighbour = activeNode.gCost + GetDistance(activeNode, neighbour); if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = activeNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } if (pathFound) { waypoints = RetracePath(startNode, targetNode); pathFound = waypoints.Length > 0; } callback(new PathResult(waypoints, pathFound, request.callback)); } }
internal static Path FindPath(NodeHandle startNode, NodeHandle targetNode, ConcurrentSet <NodeHandle> closedSet, uint maxMs, CancellationToken cancelToken, uint clearance, Action <NodeHandle> progress, bool debug = false) { var s = new Stopwatch(); findPathTimer.Start(); try { Vector3 targetNodePos = Position(targetNode); if (startNode == 0) { return(Fail($"[{targetNode}] FindPath failed: startNode is zero.")); } if (targetNode == 0) { return(Fail($"[{targetNode}] FindPath failed: targetNode is zero.")); } closedSetTimer.Start(); closedSet.Remove(startNode); if (closedSet.Contains(targetNode)) { return(Fail($"[{targetNode}] FindPath failed: targetNode is blocked")); } closedSetTimer.Stop(); // TODO: it would be best if we could combine fScore and openSet // fScore should be a heap that re-heaps when a value updates // isOpen(node) becomes fScore.Contains(node) // var fScore = new Dictionary<NodeHandle, float>(); fScoreTimer.Start(); var fScore = new Heap <NodeHandle>(); fScore.Add(startNode, Estimate(startNode, targetNode)); fScoreTimer.Stop(); var cameFrom = new Dictionary <NodeHandle, NodeHandle>(); var gScore = new Dictionary <NodeHandle, float>(); gScore.TryAdd(startNode, 0); float GScore(NodeHandle n) => gScore.ContainsKey(n) ? gScore[n] : float.MaxValue; s.Start(); fScoreTimer.Start(); while (fScore.TryPop(out NodeHandle best)) { fScoreTimer.Stop(); if (cancelToken.IsCancellationRequested) { return(Fail($"[{targetNode}] Cancelled.")); } if (s.ElapsedMilliseconds > maxMs) { return(Fail($"[{targetNode}] Searching for too long, ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms.")); } // close this node we are just about to visit closedSetTimer.Start(); closedSet.Add(best); closedSetTimer.Stop(); // update the progress callback progress(best); Vector3 curPos = Position(best); float dist = (curPos - targetNodePos).LengthSquared(); // Log($"dist = {dist:F2}"); if (dist <= .5f) { var ret = new Path(UnrollPath(cameFrom, best, debug)); Log($"[{targetNode}] Found a path of {ret.Count()} steps ({closedSet.Count} searched in {s.ElapsedMilliseconds}ms)"); return(ret); } foreach (NodeHandle e in Edges(best)) { closedSetTimer.Start(); bool closed = closedSet.Contains(e); closedSetTimer.Stop(); if (!closed && Clearance(e) >= clearance) { gScoreTimer.Start(); Vector3 ePos = Position(e); float scoreOfNewPath = GScore(best) + (curPos - ePos).Length(); float scoreOfOldPath = GScore(e); gScoreTimer.Stop(); if (scoreOfNewPath < scoreOfOldPath) { cameFrom[e] = best; gScore[e] = scoreOfNewPath; fScoreTimer.Start(); fScore.AddOrUpdate(e, gScore[e] // best path to e so far + Estimate(ePos, targetNodePos) // plus standard A* estimate + Abs(ePos.Z - curPos.Z) // plus a penalty for going vertical + ((15 - Clearance(e)) * .3f) // plus a penalty for low clearance ); fScoreTimer.Stop(); } } } fScoreTimer.Start(); } return(Fail($"[{targetNode}] Searched all reachable nodes ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms).")); } finally { findPathTimer.Stop(); fScoreTimer.Stop(); } }
/// <summary> /// Finds the path. /// </summary> private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { // Stopwatch sw = new Stopwatch (); // sw.Start (); Vector2[] wayPoints = new Vector2[0]; bool pathSuccess = false; // Get our Nodes based on our Vector3 positions. Node startNode = aStarGrid.NodeFromWorldPoint(startPos); Node targetNode = aStarGrid.NodeFromWorldPoint(targetPos); // IF our target node is walkable. if (targetNode.GetWalkable()) { // Create a Heap of Nodes with the max size being our 2D grid for A Star movement. // All start off being openNodes. Heap <Node> openSet = new Heap <Node>(aStarGrid.MaxSize); // Create an empty HashSet of Nodes for our Nodes that are closed. HashSet <Node> closedSet = new HashSet <Node>(); // Add our Starting Node to begin our pathing. openSet.Add(startNode); // WHILE we have Nodes in our Heap. while (openSet.Count > 0) { // Pop the first Node off and that is our current Node. Node currentNode = openSet.RemoveFirst(); // We know about this Node now so we move it to our closedSet. closedSet.Add(currentNode); // IF the Node we are on is the targetNode (our destination). if (currentNode == targetNode) { // sw.Stop (); // print ("Path found : " + sw.ElapsedMilliseconds + " ms"); // Our pathing has been successful pathSuccess = true; // Break the loop. break; } // Loop to each neighbour in our A Star Pathfinding Grid. foreach (Node neighbour in aStarGrid.GetNeighbours(currentNode)) { // IF the Node is NOT Walkable OR the Node is in our closedSet. if (!neighbour.GetWalkable() || closedSet.Contains(neighbour)) { // We go to the next next. continue; } // Get our movement cost if we were to move to this neighbour. int newMovementCostToNeighbour = currentNode.GetGCost() + GetDistance(currentNode, neighbour) + neighbour.GetMovementPenalty(); // IF the new movement cost to move to our neighbour is less than our previous GCost OR // IF the openSet does NOT contain our neighbour meaning we have not visited it yet if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour)) { // Set our GCost neighbour.SetGCost(newMovementCostToNeighbour); // Set our HCost neighbour.SetHCost(GetDistance(neighbour, targetNode)); // Set the parent of this node. neighbour.SetParent(currentNode); // IF this neighbour Node happened to be a node that is NOT in our openSet, // ELSE this neighbour Node that IS in our openSet. if (!openSet.Contains(neighbour)) { // We add this neighbour node to our openSet. openSet.Add(neighbour); } else { // It is in our openSet so lets update the Node. openSet.UpdateItem(neighbour); } } } } } // IF we happened to find a path to our targetNode. if (pathSuccess) { // Retrace our path and set those points as our wayPoints. wayPoints = RetracePath(startNode, targetNode); } // Finalize the path processing. requestManager.FinishedProcessingPath(wayPoints, pathSuccess); yield return(null); }
IEnumerator TrouverPath2(Vector3 positionDépart, Vector3 positionCible) { Stopwatch stopwatch = new Stopwatch(); stopwatch.Start(); Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; Node nodeDépart = Grille.NodePositionMonde(positionDépart); Node nodeCible = Grille.NodePositionMonde(positionCible); if (nodeCible.EstSurfacePourMarcher) //nodeDépart.EstSurfacePourMarcher && nodeCible.EstSurfacePourMarcher) { //List<Node> openSet = new List<Node>(Grille.TailleMax); Heap<Node> openSet = new Heap<Node>(Grille.TailleMax); HashSet<Node> closedSet = new HashSet<Node>(); openSet.Add(nodeDépart); while (openSet.Count > 0) { //Node nodeActuel = openSet[0]; //for (int i = 1; i < openSet.Count; ++i) //{ // if (openSet [i].FCost < nodeActuel.FCost || openSet [i].FCost == nodeActuel.FCost && openSet [i].HCost < nodeActuel.HCost) // { // nodeActuel = openSet [i]; // } //} //openSet.Remove (nodeActuel); Node nodeActuel = openSet.EnleverPremier(); closedSet.Add(nodeActuel); if (nodeActuel == nodeCible) { stopwatch.Stop(); Debug.Print("Path trouvé: " + stopwatch.ElapsedMilliseconds + " ms"); //RetracerPath(nodeDépart, nodeCible); //return; pathSuccess = true; break; } foreach (Node voisin in Grille.GetVoisins(nodeActuel)) { if (!voisin.EstSurfacePourMarcher || closedSet.Contains(voisin)) { continue; } int CoutNouveauMouvementVoisin = nodeActuel.GCost + GetDistance(nodeActuel, voisin); if (CoutNouveauMouvementVoisin < voisin.GCost || !openSet.Contains(voisin)) { voisin.GCost = CoutNouveauMouvementVoisin; voisin.HCost = GetDistance(voisin, nodeCible); voisin.Parent = nodeActuel; if (!openSet.Contains(voisin)) { openSet.Add(voisin); } else { openSet.UpdateObjet(voisin); } } } } } yield return null; // // // non if (pathSuccess) // // // non { // // // non wayPoints = RetracerPath2(nodeDépart, nodeCible); // // // non } // // // non RequêtePathManager.FinishingProcessingPath(wayPoints, pathSuccess); // // // non }
/// <summary> /// Sparsify vector A (keep at most <paramref name="top"/>+<paramref name="bottom"/> values) /// and optionally rescale values to the [-1, 1] range. /// <param name="a">Vector to be sparsified and normalized.</param> /// <param name="top">How many top (positive) elements to preserve after sparsification.</param> /// <param name="bottom">How many bottom (negative) elements to preserve after sparsification.</param> /// <param name="normalize">Whether to normalize results to [-1,1] range.</param> /// </summary> public static void SparsifyNormalize(ref VBuffer <Float> a, int top, int bottom, bool normalize) { Contracts.CheckParam(top >= 0, nameof(top), "Top count needs to be non-negative"); Contracts.CheckParam(bottom >= 0, nameof(bottom), "Bottom count needs to be non-negative"); Float absMax = 0; // In the top heap, we pop the smallest values, so that the 'top' largest remain. var topHeap = new Heap <KeyValuePair <int, Float> >((left, right) => right.Value < left.Value, top + 1); var bottomHeap = new Heap <KeyValuePair <int, Float> >((left, right) => right.Value > left.Value, bottom + 1); bool isDense = a.IsDense; for (int i = 0; i < a.Count; i++) { int idx = isDense ? i : a.Indices[i]; var value = a.Values[i]; if (value < 0 && bottom > 0) { if (bottomHeap.Count == bottom && value > bottomHeap.Top.Value) { continue; } bottomHeap.Add(new KeyValuePair <int, float>(idx, value)); if (bottomHeap.Count > bottom) { bottomHeap.Pop(); Contracts.Assert(bottomHeap.Count == bottom); } } if (value > 0 && top > 0) { if (topHeap.Count == top && value < topHeap.Top.Value) { continue; } topHeap.Add(new KeyValuePair <int, float>(idx, value)); if (topHeap.Count > top) { topHeap.Pop(); Contracts.Assert(topHeap.Count == top); } } } var newCount = topHeap.Count + bottomHeap.Count; var indices = a.Indices; Utils.EnsureSize(ref indices, newCount); Contracts.Assert(Utils.Size(a.Values) >= newCount); int count = 0; while (topHeap.Count > 0) { var pair = topHeap.Pop(); indices[count] = pair.Key; a.Values[count++] = pair.Value; } while (bottomHeap.Count > 0) { var pair = bottomHeap.Pop(); indices[count] = pair.Key; a.Values[count++] = pair.Value; } Contracts.Assert(count == newCount); if (normalize) { for (var i = 0; i < newCount; i++) { var value = a.Values[i]; var absValue = Math.Abs(value); if (absValue > absMax) { absMax = absValue; } } if (absMax != 0) { var ratio = 1 / absMax; for (var i = 0; i < newCount; i++) { a.Values[i] = ratio * a.Values[i]; } } } if (indices != null) { Array.Sort(indices, a.Values, 0, newCount); } a = new VBuffer <float>(a.Length, newCount, a.Values, indices); }
//replaced durch Courutine ////Updates with the movement of the gameObject attached to (seeker = player), (Target = opposite player) //void Update() //{ // //Only change if we ask it to // if (Input.GetButtonDown("Jump")) // FindPath(seeker.position, target.position); //} //ones it found a path it needs to call the finishprocessingpath of the pathrequest manager script public void FindPath(PathRequest request, Action <PathResult> callback) { //To see performance gain Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; //check if we find a path bool pathSuccess = false; //Convert Worldpos in Nodes, already done in grid method Node startNode = grid.NodeFromWorldPoint(request.pathStart); Node targetNode = grid.NodeFromWorldPoint(request.pathEnd); //Openset of nodes to be evaluated //List<Node> openSet = new List<Node>(); //if they are not walkable its impossible to find a path if (startNode.walkable && targetNode.walkable) { //Optimising with Heap Heap <Node> openSet = new Heap <Node>(grid.MaxSize); //Closedset of nodes to be already evaluated HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); //search for node with lowes f_cost is the most expensive part of algorithm while (openSet.Count > 0) { //find Node with the lowest f_cost Node currentNode = openSet.RemoveFirst(); ////now we found the node with the lowest fCost in the openset ////remove it from openSet and add to closeSet //openSet.Remove(currentNode); closedSet.Add(currentNode); //we found ouer path if (currentNode == targetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + " ms"); //found path pathSuccess = true; break; } //if not found the past, loop through each neighbouring node of current node foreach (Node neighbour in grid.GetNeighbours(currentNode)) { //check if the neighbour in the not walkable or in the close list, than skip if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } //Check if the new path to the neighbar is shorter than the old one, or if the neighbour is not in the node list int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { //set the fXost of the neighbour //we calculate the gCost and hCost neighbour.gCost = newMovementCostToNeighbour; //distance from the node to the end node neighbour.hCost = GetDistance(neighbour, targetNode); //set the parent of the neighbour to the current node neighbour.parent = currentNode; //check if the neighbour is not in the open set, if not add to open set if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); //fix problem when target moves only a little bit pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }
private IEnumerator FindThePath(Vector3 startPos, float waterLevel) { if (grid.nodes == null || grid.nodes.Length == 0) { ScanGrid(); } Path p = new Path(); bool pathSuccess = false; Node startNode = grid.NearWalkable(startPos); Node targetNode = grid.NearWaterable(startPos, waterLevel); if (startNode == null || targetNode == null) { OnProccesingDone(p, pathSuccess); yield break; } Node last = null; if (startNode.Walkable) { Heap <Node> open = new Heap <Node>(grid.maxSize); HashSet <Node> closed = new HashSet <Node>(); open.Add(startNode); while (open.Count > 0) { Node currentNode = open.RemoveFirst(); closed.Add(currentNode); if (open.Count == 1) { last = currentNode; pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (neighbour == null || !neighbour.Walkable || closed.Contains(neighbour) || neighbour.height > currentNode.height || currentNode.height <= waterLevel) { continue; } int newMovementCostToNeighbour = currentNode.gCost + grid.GetDistance(currentNode, neighbour); if ((newMovementCostToNeighbour < neighbour.gCost || !open.Contains(neighbour))) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = grid.GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!open.Contains(neighbour)) { open.Add(neighbour); } } } } } if (pathSuccess) { p = grid.RetracePath(startNode, last); } OnProccesingDone(p, true); }
public bool FindPath(Vector2Int start, Vector2Int end) { Reset(); //如果直接可以通过就不做寻路 if (CheckWalkAble(start, end)) { findResult.Add(end); findResult.Add(start); return(true); } if (start == end) { return(true);; } PathNode endNode = GetNode(end.x, end.y); PathNode startNode = GetNode(start.x, start.y); openList.Add(startNode); while (openList.Count > 0) { var currentNode = openList.RemoveFirst(); closeSet.Add(currentNode.Index); if (endNode == currentNode) { break; } Vector2Int currPos = new Vector2Int(Grid.GetX(currentNode.Index), Grid.GetY(currentNode.Index)); for (int i = 0; i < SurroundOffSet.Length; ++i) { var offset = SurroundOffSet[i]; Vector2Int pos = offset + currPos; if (!Grid.Get(pos.x, pos.y)) { continue; } var neighbour = GetNode(pos.x, pos.y); if (closeSet.Contains(neighbour.Index)) { continue; } int cost = SurroundCost[i]; int G = currentNode.G + cost; if (openList.Contains(neighbour)) { if (G < neighbour.G) { neighbour.Parent = currentNode; neighbour.G = G; } } else { neighbour.Parent = currentNode; neighbour.G = G; neighbour.H = CalcH(pos, end); openList.Add(neighbour); } } } if (endNode.Parent != null) { findResult.Add(end); var prePos = Grid.ToPos(endNode.Parent.Index); findResult.Add(prePos); Vector2Int normal = prePos - end; var preNode = endNode.Parent.Parent; //平滑路径处理 while (preNode != null) { Vector2Int curPos = Grid.ToPos(preNode.Index); Vector2Int newNormal = curPos - prePos; if (newNormal == normal) { findResult[findResult.Count - 1] = curPos; } else { findResult.Add(curPos); normal = newNormal; } prePos = curPos; preNode = preNode.Parent; } findResult.Reverse(); return(true); } return(false); }
public Vector3 generatePath(Vector3 _position, Vector3 _target, ref List <Vector3> _path) { int count = 0; if (collisionMap.CollisionTable == null) { return(Vector3.zero); } // if _target not walkable, re-compute to closer walkable node // if _start not walkable something bad happened, break and debug // create start node int startX = (int)_position.x; int startY = (int)-_position.y; Node startNode = new Node(!collisionMap.CheckCollision(startX, startY), startX, startY); if (!startNode.walkable) { Debug.Log("Unwalkable start node."); return(Vector3.zero); } // create target node int targetX = (int)_target.x; int targetY = (int)-_target.y; Node targetNode = new Node(!collisionMap.CheckCollision(targetX, targetY), targetX, targetY); // if target not walkable, consider moving target to closest walkable node Heap <Node> openSet = new Heap <Node>(collisionMap.MapWidth * collisionMap.MapHeight * 10); List <Node> closedSet = new List <Node>(); openSet.Add(startNode); // main loop while (openSet.Count > 0) { count++; if (count >= breakCount) { return(Vector3.zero); } // get node in openSet with lowest fCost Node current = openSet.RemoveFirst(); closedSet.Add(current); // check if we found target Node if (current.Equals(targetNode)) { Debug.Log("Path found: " + new Vector3(current.x, current.y, 0.0f)); _path = ReconstructPath(current); return(new Vector3(current.x, current.y, 0.0f)); } for (int i = 0; i < dirMap.Length; i++) { // create new Nodes based on dir and check for collision int neighX = current.x + dirMap[i].x; int neighY = current.y + dirMap[i].y; Node neighbour = new Node(!collisionMap.CheckCollision(neighX, neighY), neighX, neighY); if (!neighbour.walkable) { continue; } if (closedSet.Contains(neighbour)) { continue; } int tempGScore = current.gCost + 1; if (tempGScore < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = tempGScore; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = current; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } // failed Debug.Log("No path found"); return(Vector3.zero); }
public void FindPath(PathRequest a_request, Action <PathResult> a_callback) { Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(a_request.pathStart); Node targetNode = grid.NodeFromWorldPoint(a_request.pathEnd); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.maxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirstItem(); closedSet.Add(currentNode); if (currentNode == targetNode) { // Found the path pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { wayPoints = RetracePath(startNode, targetNode); pathSuccess = wayPoints.Length > 0; } a_callback(new PathResult(wayPoints, pathSuccess, a_request.callBack)); }
//for Ai public Node[] AIFindPath(Vector3 startPos, Vector3 targetPos, bool canFly, int unitPlayerID) { bool pathSuccess = false; HeuristicFunction heuristicFunction = new HeuristicFunction(GetDistance); Node startNode = gridRef.NodeFromWorldPoint(startPos); Node targetNode = gridRef.NodeFromWorldPoint(targetPos); Unit currentUnit = startNode.GetUnit(); if (startNode.canWalkHere && targetNode.canWalkHere) { Heap <Node> openSet = new Heap <Node>(gridRef.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in gridRef.GetNeighbours(currentNode)) { bool checkHostile = false; if (neighbour.GetUnit() != null) { if (neighbour.GetUnit().GetUnitPlayerID() != unitPlayerID) { checkHostile = true; } } if ((!canFly && !neighbour.canWalkHere) || closedSet.Contains(neighbour) || checkHostile) { continue; //Skips unwalkable nodes when unit cannot fly, or if any node in closed set or if the unit in the node is hostile //considers unwalkable nodes if unit can fly, and ignores any node if in closed set } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = heuristicFunction(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } } if (pathSuccess) { Node[] waypoints = RetracePath(startNode, targetNode); currentUnit.SetMovementSpeedLeft(currentUnit.GetMovementSpeedLeft() - (waypoints.Length - 1)); return(waypoints); } return(new[] { startNode }); }
public void FindPath(PathRequest request, Action <PathResult> callback) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = _tilemapReader.NodeFromWorldPoint(request.pathStart); Node targetNode = _tilemapReader.NodeFromWorldPoint(request.pathEnd); startNode.parent = startNode; if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(_tilemapReader.MaxTileSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); //print ("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in _tilemapReader.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }
//경로 찾기 IEnumerator FindPath(Vector3 startPos, Vector3 endPos) { //heap 구조 시간 Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; //시작 노드와 도착 노드 를 그리드 좌표로 Node startNode = grid.NodeFromWorldPoint(startPos); Node endNode = grid.NodeFromWorldPoint(endPos); if (startNode.walkable && endNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); //중복되지 않은 데이터 사용 할때 사용 값을 저장 HashSet <Node> closeSet = new HashSet <Node>(); //시작 노드 부터 시작 openSet.Add(startNode); while (openSet.Count > 0) { //첫번째 노드 Node currentNode = openSet.RemoveFirst(); //클로즈리스트에 노드를 더해준다 closeSet.Add(currentNode); //최근 노드와 도착노드가 같다면 끝낸다(while문 탈출) if (currentNode == endNode) { sw.Stop(); //print("Path found : " + sw.ElapsedMilliseconds + "ms"); pathSuccess = true; break; } //반복 주위에 있는 노드 foreach (Node neighbour in grid.GetNeighbours(currentNode)) { //걸을수 없는 위치거나 클로즈노드가 추가되어잇다면 건너뛰기 if (!neighbour.walkable || closeSet.Contains(neighbour)) { continue; } //주위에 이웃되어있는 노드들에 경로 값을 더하고 최솟값 경로를 추가해준다 int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, endNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } //이부분은 아직 도착노드에 하지 않은 상태이므로 다시 while 문 실행 } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, endNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
/// <summary> /// Finds the shortest path from the start node to the goal node /// </summary> /// <param name="AStartNode">Start node</param> /// <param name="AGoalNode">Goal node</param> public void FindPath(AStarNode AStartNode, AStarNode AGoalNode) { FStartNode = AStartNode; FGoalNode = AGoalNode; FOpenList.Add(FStartNode); int i = 0; while (FOpenList.Count > 0 && i < 2000) { // Get the node with the lowest TotalCost AStarNode NodeCurrent = (AStarNode)FOpenList.Pop(); // If the node is the goal copy the path to the solution array if (NodeCurrent.IsGoal()) { while (NodeCurrent != null) { FSolution.Insert(0, NodeCurrent); NodeCurrent = NodeCurrent.Parent; } break; } // Get successors to the current node NodeCurrent.GetSuccessors(FSuccessors); foreach (AStarNode NodeSuccessor in FSuccessors) { // Test if the currect successor node is on the open list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeOpen = null; if (FOpenList.Contains(NodeSuccessor)) { NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)]; } if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost)) { continue; } // Test if the currect successor node is on the closed list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeClosed = null; if (FClosedList.Contains(NodeSuccessor)) { NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)]; } if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost)) { continue; } // Remove the old successor from the open list FOpenList.Remove(NodeOpen); // Remove the old successor from the closed list FClosedList.Remove(NodeClosed); // Add the current successor to the open list FOpenList.Push(NodeSuccessor); } // Add the current node to the closed list FClosedList.Add(NodeCurrent); i++; } if (i == 2000) { m_pathPossible = false; } }
void FindPath(Vector3 startPosition, Vector3 targetPosition) { Stopwatch sw = new Stopwatch(); sw.Start(); Node startNode = grid.getNodeFromWorldPoint(startPosition); Node targetNode = grid.getNodeFromWorldPoint(targetPosition); //List<Node> openSet = new List<Node>(); Heap <Node> openSet = new Heap <Node>(grid.MaxHeapSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { //Node currentNode = openSet[0]; //// find node with lowest f-cost; costly part of the algorithm. //for(int x = 1; x < openSet.Count; x++) //{ // if(openSet[x].fCost < currentNode.fCost || openSet[x].fCost == currentNode.fCost) // { // if(openSet[x].hCost < currentNode.hCost) // { // currentNode = openSet[x]; // } // } //} Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); RetracePath(startNode, targetNode); print("Found path in: " + sw.ElapsedMilliseconds + " ms"); return; } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbor); if (newCostToNeighbour < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newCostToNeighbour; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } } } } }
private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { //Debug.Log($"START: {startPos}, END: {targetPos}"); // Stopwatch to see the performance gain through heap optimization Stopwatch sw = new Stopwatch(); // Start stopwatch sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = graph.NodeFromWorldPoint(startPos); Node targetNode = graph.NodeFromWorldPoint(targetPos); // If the start or end nodes are not walkable do not bother finding a path if (startNode.walkable && targetNode.walkable) { // Implementing the Heap Optimization into the former lists. Heap <Node> openSet = new Heap <Node>(graph.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { // This is equal to the first node in the open set Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { // Stop stopwatch sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in graph.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } // wait one frame before returning yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
/* This function finds a path between 2 nodes * * @param startPos - the starting node * @param targetPos - the target node * */ IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); /* This part of the algorithm is costly when using a list. * Comparing the fCost, or hCost as a backup, to get the least cost node before proceeding with the algorithm * By using a heap instead, finding a path takes less than one fourth of the time. * * Node currentNode = openSet[0]; * for (int i = 1; i< openSet.Count; i++) // Most costly part of algorithm * { * if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost) * { * currentNode = openSet[i]; * } * } * openSet.Remove(currentNode); */ closedSet.Add(currentNode); // We have reached the targetNode (found the path) if (currentNode == targetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + "ms"); pathSuccess = true; RetracePath(startNode, targetNode); break; // Coroutine requires this instead of return to exit pathfinding loop } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; //movementPenalty for weighted cost if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } } // wait for one frame before returning yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
/// <summary> /// Calculates Path from Start Node to Target Node and Returns List of Nodes within Path. /// </summary> /// <param name="a_startNode"></param> /// <param name="a_targetNode"></param> /// <returns></returns> private List <Node> PathFind(Node a_startNode, Node a_targetNode) { // Algorithm Begins List <Node> returnPath = new List <Node>(); // 2 Lists Required // Heap Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); // Add Start Node to Open Set openSet.Add(a_startNode); // Run as Long as we have unvisited nodes while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode.Equals(a_targetNode)) { // Destination Found, Retrace Path Here returnPath = RetracePath(a_startNode, currentNode); break; } // Still Havent Reached Target foreach (Node neighbour in NodeGrid.Instance.GetNeighbours(currentNode)) { // Check if Neighbour is walkable or hasn't already been checked if (closedSet.Contains(neighbour)) { continue; } // Create new movement cost float MovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour); if (MovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) { // Calculate new costs neighbour.GCost = MovementCostToNeighbour; neighbour.HCost = GetDistance(neighbour, a_targetNode); // Assign Previous Node neighbour.PreviousNode = currentNode; // Add neighbour to open set if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { // Hash Set Update Item openSet.UpdateItem(neighbour); } } } } return(returnPath); }
public void FindPath(PathRequest request, Action <PathResult> callback) { //Stopwatch sw = new Stopwatch(); //sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathFound = false; PathNode startNode = grid.NodeFromWorldPoint(request.pathStart); PathNode endNode = grid.NodeFromWorldPoint(request.pathEnd); if (endNode.clear != PathNode.Passability.Obstacle) { Heap <PathNode> openSet = new Heap <PathNode>(grid.MaxSize); HashSet <PathNode> closedSet = new HashSet <PathNode>(); openSet.Add(startNode); while (openSet.Count > 0) { PathNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); // Found path if (currentNode == endNode) { //sw.Stop(); pathFound = true; break; // Exit out of while } // Check neighbors foreach (PathNode neighbor in grid.FindNeighbors(currentNode)) { // Obstacle or already in our set if (neighbor.clear == PathNode.Passability.Obstacle || closedSet.Contains(neighbor)) { continue; } // Lower cost found or an unitialized node int newMoveCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor); if (newMoveCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { // Initialize movement costs neighbor.gCost = newMoveCostToNeighbor; neighbor.hCost = GetDistance(neighbor, endNode); neighbor.parent = currentNode; // Add to open set if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); // Updates automatically } else // Must manually update because costs changed { openSet.UpdateItem(neighbor); } } } } // while nodes in openset } // endpoints clear if (pathFound) { waypoints = RetracePath(startNode, endNode, request.pathStart, request.pathEnd); pathFound = waypoints.Length > 0; // Waypoints could be empty } callback(new PathResult(waypoints, pathFound, request.callback)); } // FindPath()
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.maxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } List <Node> nodeTest = grid.getNeighbours(currentNode); foreach (Node neighbour in grid.getNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } if (neighbour.movementPenalty > 0) { Debug.Log("Tough Road"); } int newMovementCost = currentNode.gCost + getDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCost < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCost; neighbour.hCost = getDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = retracePath(startNode, targetNode); } requestManager.finishedProcessingPath(waypoints, pathSuccess); }
public static Graph <T> KruskalsAlgorithm <T>(Graph <T> weightedGraph) { Guard.ArgumentNotNull(weightedGraph, "weightedGraph"); // There is going be Vertices - 1 edges when we finish var edgeCount = weightedGraph.Vertices.Count - 1; var vertexToParent = new Dictionary <Vertex <T>, Vertex <T> >(); var oldToNew = new Dictionary <Vertex <T>, Vertex <T> >(); var edgeQueue = new Heap <Association <double, Edge <T> > >( HeapType.Minimum, new AssociationKeyComparer <double, Edge <T> >()); // Now build the return graph, always return non directed. var returnGraph = new Graph <T>(false); // As we mew the new vertices for the new graph from the old // one, we also map the old ones to the new ones, and set up // our dictionary used to track forests of vertices. foreach (var vertex in weightedGraph.Vertices) { var vertex2 = new Vertex <T>(vertex.Data); oldToNew.Add(vertex, vertex2); returnGraph.AddVertex(vertex2); vertexToParent.Add(vertex, null); } // We need to move the edges into a priority queue // and use the weight in the association to sort them. foreach (var e in weightedGraph.Edges) { edgeQueue.Add(new Association <double, Edge <T> >(e.Weight, e)); } // We know when we are done, when we hit the number of edges // or when there is no more edges. while ((edgeQueue.Count > 0) && (edgeCount > 0)) { // Pull off the least weight edge that hasn't been added or discarded yet. var association = edgeQueue.RemoveRoot(); // Save the value of the association to the proper type, an edge var edge = association.Value; // Here is the start of search to make find the heads // of the forest, because if they are the same heads, // there is a cycle. var fromVertexHead = edge.FromVertex; var toVertexHead = edge.ToVertex; // Find the head vertex of the forest the fromVertex is in while (vertexToParent[fromVertexHead] != null) { fromVertexHead = vertexToParent[fromVertexHead]; } // Find the head vertex of the forest the toVertex is in while (vertexToParent[toVertexHead] != null) { toVertexHead = vertexToParent[toVertexHead]; } // Check to see if the heads are the same // if are equal, it is a cycle, and we cannot // include the edge in the new graph. if (fromVertexHead != toVertexHead) { // Join the FromVertex forest to the ToVertex vertexToParent[fromVertexHead] = edge.ToVertex; // We have one less edge we need to find edgeCount--; // Add the edge to the new new graph, map the old vertices to the new ones returnGraph.AddEdge(oldToNew[edge.FromVertex], oldToNew[edge.ToVertex], edge.Weight); } } // All done :) return(returnGraph); }
Vector2[] FindPath(Vector2 from, Vector2 to) { location = gameObject.transform.position; Stopwatch sw = new Stopwatch(); sw.Start(); Vector2[] waypoints = new Vector2[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(from - location); Node targetNode = grid.NodeFromWorldPoint(to - location); startNode.parent = startNode; if (!startNode.walkable) { startNode = grid.ClosestWalkableNode(startNode); } if (!targetNode.walkable) { targetNode = grid.ClosestWalkableNode(targetNode); } if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); // print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + TurningCost(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } return(waypoints); }
public static Graph <T> DijkstrasAlgorithm <T>(Graph <T> weightedGraph, Vertex <T> fromVertex) { #region Parameter Checks Guard.ArgumentNotNull(weightedGraph, "weightedGraph"); Guard.ArgumentNotNull(fromVertex, "fromVertex"); if (!weightedGraph.ContainsVertex(fromVertex)) { throw new ArgumentException(Graph <T> .couldNotBeFoundInTheGraph, "fromVertex"); } #endregion var heap = new Heap <Association <double, Vertex <T> > >( HeapType.Minimum, new AssociationKeyComparer <double, Vertex <T> >()); var vertexStatus = new Dictionary <Vertex <T>, VertexInfo <T> >(); // Initialise the vertex distances to the maximum possible. foreach (var vertex in weightedGraph.Vertices) { vertexStatus.Add(vertex, new VertexInfo <T>(double.MaxValue, null, false)); } vertexStatus[fromVertex].Distance = 0; // Add the source vertex to the heap - we'll be branching out from it. heap.Add(new Association <double, Vertex <T> >(0, fromVertex)); while (heap.Count > 0) { var item = heap.RemoveRoot(); var vertexInfo = vertexStatus[item.Value]; if (!vertexInfo.IsFinalised) { var edges = item.Value.EmanatingEdges; vertexStatus[item.Value].IsFinalised = true; // Enumerate through all the edges emanating from this node for (var i = 0; i < edges.Count; i++) { var edge = edges[i]; var partnerVertex = edge.GetPartnerVertex(item.Value); // Calculate the new distance to this distance var distance = vertexInfo.Distance + edge.Weight; var newVertexInfo = vertexStatus[partnerVertex]; // Found a better path, update the vertex status and add the // vertex to the heap for further analysis if (distance < newVertexInfo.Distance) { newVertexInfo.EdgeFollowed = edge; newVertexInfo.Distance = distance; heap.Add(new Association <double, Vertex <T> >(distance, partnerVertex)); } } } } return(BuildGraphDijkstra(weightedGraph, fromVertex, vertexStatus)); }
private IEnumerator FindPath(Vector3 _startPosition, Vector3 _targetPosition) { System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = AStarGrid.g.GetNodeFromWorldPosition(_startPosition); Node targetNode = AStarGrid.g.GetNodeFromWorldPosition(_targetPosition); //Debug for draw gizmos AStarGrid.g.targetNode = targetNode; //if the nodes are not obstacles if (startNode.walkable && targetNode.walkable) { //Nodes to evaluate Heap <Node> openList = new Heap <Node>((int)(AStarGrid.g.gridSize.x * AStarGrid.g.gridSize.y)); //Already evaluated nodes List <Node> closedList = new List <Node>(); startNode.gCost = 0; openList.Add(startNode); int safetyCheck = 0; //Loop through open list while (openList.Count > 0) { //Debug.Log("Looping through the open list..."); safetyCheck++; //Can only search in a 20 by 20 space so max 400 if (safetyCheck > 400) { break; } //Remove current node from the open list because it is being evaluated Node currentNode = openList.RemoveFirstItem(); // -- Very slow, optimised using the heap -- // /*Node lowestFCostNode = null; * foreach(Node n in openList) * { * if ((lowestFCostNode != null && lowestFCostNode.fCost > n.fCost) || (n.fCost == lowestFCostNode.fCost && n.hCost < lowestFCostNode.hCost)) || lowestFCostNode = n; ||} ||Node currentNode = lowestFCostNode;*/ //Add the current node to the closed (evaluated) list closedList.Add(currentNode); //Path found if (currentNode == targetNode) { sw.Stop(); pathSuccess = true; Statistics.instance.SavePathfindingTime(sw.ElapsedMilliseconds); //Debug.Log("Path found in " + sw.ElapsedMilliseconds + " ms"); break; } foreach (Node neighbour in AStarGrid.g.GetNodeNeighbours(currentNode)) { //if the neighbour is an obstacle or has already been evaluated //or is in a building if (!neighbour.walkable || closedList.Contains(neighbour)) { continue; } int movementCost = currentNode.gCost + GetDistanceBetweenNodes(currentNode, neighbour); if (movementCost < neighbour.gCost || !openList.Contains(neighbour)) { //set f cost of neighbour (g cost + h cost) neighbour.gCost = movementCost; neighbour.hCost = GetDistanceBetweenNodes(neighbour, targetNode); //set parent of neighbour to current neighbour.parentNode = currentNode; if (!openList.Contains(neighbour)) { openList.Add(neighbour); } else { openList.UpdateItem(neighbour); } } } //yield return null; } } if (pathSuccess) { waypoints = DefinePath(startNode, targetNode); } //else //{ // Debug.LogWarning("Path not found!"); //} prm.FinishedProcessingPath(waypoints, pathSuccess); yield return(null); }
//! Returns List<Node> public List <Node> FindPath(Node startNode, Node targetNode, ref NodeGrid grid, bool isPathSimplified = false, bool isPathFromStartToTarget = true, bool hasDiagonalMovement = true) { System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); sw.Start(); bool pathSuccess = false; //Node startNode = grid.NodeFromWorldPosition(startPos); //Node targetNode = grid.NodeFromWorldPosition(targetPos); if (startNode.isWalkable && targetNode.isWalkable) { //Heap<Node> openSet = new Heap<Node>(grid.MaxHeapSize); Heap <Node> openSet = new Heap <Node>(grid.MaxHeapSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.Remove(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); pathSuccess = true; Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms"); break; } foreach (var neighbourNode in grid.GetNeighbours(currentNode, hasDiagonalMovement)) { if (!neighbourNode.isWalkable || closedSet.Contains(neighbourNode)) { continue; } //count distance for each neighbour at grid generation => create set/array/list of neighbours? int movementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode) + neighbourNode.movementPenalty; bool isInOpenSet = openSet.Contains(neighbourNode); if (movementCostToNeighbour < neighbourNode.gCost || !isInOpenSet) { neighbourNode.gCost = movementCostToNeighbour; neighbourNode.hCost = GetDistance(neighbourNode, targetNode); neighbourNode.parent = currentNode; if (!isInOpenSet) { openSet.Add(neighbourNode); } else { openSet.UpdateItem(neighbourNode); } } } } } if (pathSuccess) { List <Node> path = RetracePath(startNode, targetNode); if (isPathSimplified) { SimplifyPathReturnAsNodes(ref path); pathSuccess = path.Count > 0; } if (isPathFromStartToTarget) { ReversePath(ref path); } if (pathSuccess) { return(path); } } return(null); }
List <Node> FindPath(Vector3 startPos, Node targetNode) { //Stopwatch sw = new Stopwatch (); //sw.Start (); Node startNode = grid.NodeFromWorldPoint(startPos); //Node targetNode = grid.NodeFromWorldPoint (targetPos); // OBSELETE RaycastHit2D hit = Physics2D.Linecast(startNode.worldPosition, targetNode.worldPosition, 8); // Base case -- saves performance? //Debug.DrawLine(startNode.worldPosition,targetNode.worldPosition,Color.red); if (hit == null) { List <Node> ShortList = new List <Node> (); ShortList.Add(startNode); ShortList.Add(targetNode); return(ShortList); } Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { //sw.Stop (); //print ("Path Found " + sw.ElapsedMilliseconds + "ms"); return(RetracePath(startNode, targetNode)); } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbor); if (newMovementCostToNeighbour < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbour; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } } } } return(null); }
private Vector3[] FindPath(Vector3 start, Vector3 end) { Node startNode = grid.pathGridData.NodeFromWorldPoint(start); Node endNode = grid.pathGridData.NodeFromWorldPoint(end); Node recentWalk = startNode; int minDistance = int.MaxValue; Heap <Node> openSet = new Heap <Node>(grid.pathGridData.MaxSize); HashSet <Node> closeSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closeSet.Add(currentNode); if (currentNode == endNode) { // 找到了 break; } // 获取相邻节点 List <Node> neighbours = grid.pathGridData.GetNeighbours(currentNode); foreach (Node n in neighbours) { if (!n.walkable || closeSet.Contains(n)) { continue; } int newCostToNeighbours = currentNode.gCost + GetDistence(currentNode, endNode); if (newCostToNeighbours < n.gCost || !openSet.Contains(n)) { // 更新 gCost n.gCost = newCostToNeighbours; int hCost = GetDistence(n, endNode); n.hCost = hCost; n.parent = currentNode; if (!openSet.Contains(n)) { openSet.Add(n); // 更新最近的节点 if (hCost < minDistance) { minDistance = hCost; recentWalk = n; } } else { openSet.UpdateItem(n); } } } } // 获取路径 if (recentWalk == startNode) { //在起始点没动 return(null); } else { return(RetarcePath(startNode, recentWalk)); } }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.GetNodeFromWorld(startPos); Node targetNode = grid.GetNodeFromWorld(targetPos); //Debug.Log(startNode.walkable); if (true) //startNode.walkable targetNode.walkable { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); //for (int i = 1; i < openSet.Count; i++) { // if (currentNode.fCost > openSet[i].fCost || (currentNode.fCost == openSet[i].fCost && // currentNode.hCost > openSet[i].hCost)) // { // currentNode = openSet[i]; // } //} //openSet.Remove(currentNode); closedSet.Add(currentNode); if (currentNode == targetNode) { //Debug.Log("find"); pathSuccess = true; break; } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; if (newCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } } yield return(null); if (pathSuccess) { wayPoints = RetracePath(startNode, targetNode); } pathRequestManager.FinishProcessingPath(wayPoints, pathSuccess); }
/// <summary> /// Finds the shortest paths to all other vertices from the specified source vertex. /// </summary> /// <param name="weightedGraph"> The weighted graph. </param> /// <param name="fromVertex"> The source vertex. </param> /// <returns> A graph representing the shortest paths from the source node to all other nodes in the graph. </returns> public static Graph <T> FindShortestPaths(Graph <T> weightedGraph, Vertex <T> fromVertex) { #region Parameter Checks if (weightedGraph == null) { throw new ArgumentNullException("weightedGraph"); } if (fromVertex == null) { throw new ArgumentNullException("fromVertex"); } if (!weightedGraph.ContainsVertex(fromVertex)) { throw new ArgumentException(Properties.Resources.VertexCouldNotBeFound); } #endregion var heap = new Heap <Association <double, Vertex <T> > >( HeapType.MinHeap, new AssociationKeyComparer <double, Vertex <T> >()); var vertexStatus = new Dictionary <Vertex <T>, VertexInfo <T> >(); // Initialise the vertex distances to using (var verticeEnumerator = weightedGraph.Vertices) while (verticeEnumerator.MoveNext()) { vertexStatus.Add(verticeEnumerator.Current, new VertexInfo <T>(double.MaxValue, null, false)); } vertexStatus[fromVertex].Distance = 0; // Add the source vertex to the heap - we'll be branching out from it. heap.Add(new Association <double, Vertex <T> >(0, fromVertex)); while (heap.Count > 0) { var item = heap.RemoveRoot(); var vertexInfo = vertexStatus[item.Value]; if (!vertexInfo.IsFinalised) { var edges = item.Value.EmanatingEdgeList; vertexStatus[item.Value].IsFinalised = true; // Enumerate through all the edges emanating from this node foreach (var edge in edges) { var partnerVertex = edge.GetPartnerVertex(item.Value); // Calculate the new distance to this distance var distance = vertexInfo.Distance + edge.Weight; var newVertexInfo = vertexStatus[partnerVertex]; // Found a better path, update the vertex status and add the // vertex to the heap for further analysis if (distance < newVertexInfo.Distance) { newVertexInfo.EdgeFollowed = edge; newVertexInfo.Distance = distance; heap.Add(new Association <double, Vertex <T> >(distance, partnerVertex)); } } } } // Now build the new graph var newGraph = new Graph <T>(weightedGraph.IsDirected); var enumerator = vertexStatus.GetEnumerator(); // This dictionary is used for mapping between the old vertices and the new vertices put into the graph var vertexMap = new Dictionary <Vertex <T>, Vertex <T> >(vertexStatus.Count); var newVertices = new Vertex <T> [vertexStatus.Count]; while (enumerator.MoveNext()) { var newVertex = new Vertex <T>( enumerator.Current.Key.Data, enumerator.Current.Value.Distance ); vertexMap.Add(enumerator.Current.Key, newVertex); newGraph.AddVertex(newVertex); } enumerator = vertexStatus.GetEnumerator(); while (enumerator.MoveNext()) { var info = enumerator.Current.Value; // Check if an edge has been included to this vertex if ((info.EdgeFollowed != null) && (enumerator.Current.Key != fromVertex)) { newGraph.AddEdge( vertexMap[info.EdgeFollowed.GetPartnerVertex(enumerator.Current.Key)], vertexMap[enumerator.Current.Key], info.EdgeFollowed.Weight); } } return(newGraph); }