// Update is called once per frame void LateUpdate () { if (prevNode == null) { NNInfo nninfo = AstarPath.active.GetNearest(transform.position); prevNode = nninfo.node; prevPos = transform.position; } if (prevNode == null) { return; } if (prevNode != null) { var graph = AstarData.GetGraph(prevNode) as IRaycastableGraph; if (graph != null) { GraphHitInfo hit; if (graph.Linecast(prevPos, transform.position, prevNode, out hit)) { hit.point.y = transform.position.y; Vector3 closest = VectorMath.ClosestPointOnLine(hit.tangentOrigin, hit.tangentOrigin+hit.tangent, transform.position); Vector3 ohit = hit.point; ohit = ohit + Vector3.ClampMagnitude((Vector3)hit.node.position-ohit, 0.008f); if (graph.Linecast(ohit, closest, hit.node, out hit)) { hit.point.y = transform.position.y; transform.position = hit.point; } else { closest.y = transform.position.y; transform.position = closest; } } prevNode = hit.node; } } prevPos = transform.position; }
public GraphNode GetOther ( GraphNode a ) { if ( this.connections.Length < 2 ) return null; if ( this.connections.Length != 2 ) throw new System.Exception ("Invalid NodeLink3Node. Expected 2 connections, found " + this.connections.Length); return a == connections[0] ? (connections[1] as NodeLink3Node).GetOtherInternal(this) : (connections[0] as NodeLink3Node).GetOtherInternal(this); }
public override bool GetPortal (GraphNode other, List<Vector3> left, List<Vector3> right, bool backwards) { if ( this.connections.Length < 2 ) return false; if ( this.connections.Length != 2 ) throw new System.Exception ("Invalid NodeLink3Node. Expected 2 connections, found " + this.connections.Length); //if ( other != connections[0] || other != connections[1] ) return false; if ( left != null ) { //Debug.DrawLine ( portalA, portalB, Color.red); left.Add ( portalA ); right.Add ( portalB ); /* Vector3 normal = link.transform.forward; Vector3 tangent = Vector3.Dot (normal, (Vector3)(other.Position - this.Position) ) > 0 ? link.transform.right*0.5f : -link.transform.right*0.5f; Debug.DrawLine ( link.transform.position -tangent * link.portalWidth, link.transform.position +tangent * link.portalWidth, Color.red); Debug.DrawRay ( link.transform.position -tangent * link.portalWidth, Vector3.up*5, Color.red); Debug.Break (); left.Add ( link.transform.position -tangent * link.portalWidth ); right.Add (link.transform.position +tangent * link.portalWidth );*/ } return true; }
public static FloodPath Construct (GraphNode start, OnPathDelegate callback = null) { if (start == null) throw new ArgumentNullException("start"); var p = PathPool.GetPath<FloodPath>(); p.Setup(start, callback); return p; }
public override void AddConnection(GraphNode node, uint cost) { if (this.connections != null) { for (int i = 0; i < this.connections.Length; i++) { if (this.connections[i] == node) { this.connectionCosts[i] = cost; return; } } } int num = (this.connections == null) ? 0 : this.connections.Length; GraphNode[] array = new GraphNode[num + 1]; uint[] array2 = new uint[num + 1]; for (int j = 0; j < num; j++) { array[j] = this.connections[j]; array2[j] = this.connectionCosts[j]; } array[num] = node; array2[num] = cost; this.connections = array; this.connectionCosts = array2; }
public override void Execute() { if(Path.path.Count >= Index) { return; } Node = Path.path[Index]; }
/** Returns all nodes reachable from the seed node. * This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from * the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node. * In the editor areas are displayed as different colors of the nodes. * The only case where it will not be so is when there is a one way path from some part of the area to the seed node * but no path from the seed node to that part of the graph. * * The returned list is sorted by node distance from the seed node * i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through. * Note that the distance measurement does not take heuristics, penalties or tag penalties. * * Depending on the number of reachable nodes, this function can take quite some time to calculate * so don't use it too often or it might affect the framerate of your game. * * \param seed The node to start the search from * \param tagMask Optional mask for tags. This is a bitmask. * * \returns A List<Node> containing all nodes reachable from the seed node. * For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool */ public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) { var stack = StackPool<GraphNode>.Claim (); var list = ListPool<GraphNode>.Claim (); /** \todo Pool */ var map = new HashSet<GraphNode>(); GraphNodeDelegate callback; if (tagMask == -1) { callback = delegate (GraphNode node) { if (node.Walkable && map.Add (node)) { list.Add (node); stack.Push (node); } }; } else { callback = delegate (GraphNode node) { if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) { list.Add (node); stack.Push (node); } }; } callback (seed); while (stack.Count > 0) { stack.Pop ().GetConnections (callback); } StackPool<GraphNode>.Release (stack); return list; }
/** Add a connection from this node to the specified node. * If the connection already exists, the cost will simply be updated and * no extra connection added. * * \note Only adds a one-way connection. Consider calling the same function on the other node * to get a two-way connection. */ public override void AddConnection (GraphNode node, uint cost) { if (connections != null) { for (int i=0;i<connections.Length;i++) { if (connections[i] == node) { connectionCosts[i] = cost; return; } } } int connLength = connections != null ? connections.Length : 0; GraphNode[] newconns = new GraphNode[connLength+1]; uint[] newconncosts = new uint[connLength+1]; for (int i=0;i<connLength;i++) { newconns[i] = connections[i]; newconncosts[i] = connectionCosts[i]; } newconns[connLength] = node; newconncosts[connLength] = cost; connections = newconns; connectionCosts = newconncosts; }
public override void Execute() { var closestResult = AstarPath.active.GetNearest(Position); if (closestResult.node == null) { return; } NearestNode = closestResult.node; }
/** Returns all nodes reachable from the seed node. * This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from * the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node. * In the editor areas are displayed as different colors of the nodes. * The only case where it will not be so is when there is a one way path from some part of the area to the seed node * but no path from the seed node to that part of the graph. * * The returned list is sorted by node distance from the seed node * i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through. * Note that the distance measurement does not take heuristics, penalties or tag penalties. * * Depending on the number of reachable nodes, this function can take quite some time to calculate * so don't use it too often or it might affect the framerate of your game. * * \param seed The node to start the search from * \param tagMask Optional mask for tags. This is a bitmask. * * \returns A List<Node> containing all nodes reachable from the seed node. * For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool */ public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) { Stack<GraphNode> stack = Pathfinding.Util.StackPool<GraphNode>.Claim (); List<GraphNode> list = Pathfinding.Util.ListPool<GraphNode>.Claim (); HashSet<GraphNode> map = new HashSet<GraphNode>(); GraphNodeDelegate callback; if (tagMask == -1) { callback = delegate (GraphNode node) { if (node.Walkable && map.Add (node)) { list.Add (node); stack.Push (node); } }; } else { callback = delegate (GraphNode node) { if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) { list.Add (node); stack.Push (node); } }; } callback (seed); while (stack.Count > 0) { stack.Pop ().GetConnections (callback); } Pathfinding.Util.StackPool<GraphNode>.Release (stack); return list; }
/** Updates graphs and checks if all nodes are still reachable from each other. * Graphs are updated, then a check is made to see if the nodes are still reachable from each other. * If they are not, the graphs are reverted to before the update and \a false is returned.\n * This is slower than a normal graph update. * All queued graph updates and thread safe callbacks will be flushed during this function. * * \note This might return true for small areas even if there is no possible path if AstarPath.minAreaSize is greater than zero (0). * So when using this, it is recommended to set AstarPath.minAreaSize to 0 (A* Inspector -> Settings -> Pathfinding) * * \param guo The GraphUpdateObject to update the graphs with * \param node1 Node which should have a valid path to \a node2. All nodes should be walkable or \a false will be returned. * \param node2 Node which should have a valid path to \a node1. All nodes should be walkable or \a false will be returned. * \param alwaysRevert If true, reverts the graphs to the old state even if no blocking ocurred * * \returns True if the given nodes are still reachable from each other after the \a guo has been applied. False otherwise. * \code var guo = new GraphUpdateObject (tower.GetComponent<Collider>.bounds); var spawnPointNode = AstarPath.active.GetNearest (spawnPoint.position).node; var goalNode = AstarPath.active.GetNearest (goalNode.position).node; if (GraphUpdateUtilities.UpdateGraphsNoBlock (guo, spawnPointNode, goalNode, false)) { // Valid tower position // Since the last parameter (which is called "alwaysRevert") in the method call was false // The graph is now updated and the game can just continue } else { // Invalid tower position. It blocks the path between the spawn point and the goal // The effect on the graph has been reverted Destroy (tower); } \endcode */ public static bool UpdateGraphsNoBlock (GraphUpdateObject guo, GraphNode node1, GraphNode node2, bool alwaysRevert = false) { List<GraphNode> buffer = ListPool<GraphNode>.Claim (); buffer.Add (node1); buffer.Add (node2); bool worked = UpdateGraphsNoBlock (guo, buffer, alwaysRevert); ListPool<GraphNode>.Release (buffer); return worked; }
public GraphHitInfo(Vector3 point) { this.tangentOrigin = Vector3.zero; this.origin = Vector3.zero; this.point = point; this.node = null; this.tangent = Vector3.zero; }
/** Adds a node to the heap */ public void Add(GraphNode node) { if (node == null) { Debug.Log ("Sending null node to BinaryHeap"); return; } if (numberOfItems == binaryHeap.Length) { Debug.Log ("Forced to discard nodes because of binary heap size limit, please consider increasing the size ("+numberOfItems +" "+binaryHeap.Length+")"); numberOfItems--; } binaryHeap[numberOfItems] = node; //node.heapIndex = numberOfItems;//Heap index int bubbleIndex = numberOfItems; uint nodeF = node.f; while (bubbleIndex != 1) { int parentIndex = bubbleIndex / 2; /*if (binaryHeap[parentIndex] == null) { Debug.Log ("WUT!!"); return; }*/ if (nodeF <= binaryHeap[parentIndex].f) { //binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /** \todo Wouldn't it be more efficient with '<' instead of '<=' ? * / //Node tmpValue = binaryHeap[parentIndex]; //tmpValue.heapIndex = bubbleIndex;//HeapIndex binaryHeap[bubbleIndex] = binaryHeap[parentIndex]; binaryHeap[parentIndex] = node;//binaryHeap[bubbleIndex]; //binaryHeap[bubbleIndex].heapIndex = bubbleIndex; //Heap index //binaryHeap[parentIndex].heapIndex = parentIndex; //Heap index bubbleIndex = parentIndex; } else { /*if (binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /** \todo Wouldn't it be more efficient with '<' instead of '<=' ? * Node tmpValue = binaryHeap[parentIndex]; //tmpValue.heapIndex = bubbleIndex;//HeapIndex binaryHeap[parentIndex] = binaryHeap[bubbleIndex]; binaryHeap[bubbleIndex] = tmpValue; bubbleIndex = parentIndex; } else {*/ break; } } numberOfItems++; }
public static bool InSearchTree(GraphNode node, Path path) { if (path == null || path.pathHandler == null) { return true; } PathNode pathNode = path.pathHandler.GetPathNode(node); return pathNode.pathID == path.pathID; }
public override bool ContainsConnection(GraphNode node) { for (int i = 0; i < this.connections.Length; i++) { if (this.connections[i] == node) { return true; } } return false; }
public override void Execute() { if(Path.vectorPath.Count == 0) { return; } var closestNode = Path.FindClosestNodeTo(PositionToCheck); OutputNode = closestNode; if (DistanceToClosest > 0 || DistanceToClosest < 0) { var nodePosition = (Vector3) closestNode.position; DistanceToClosest = Vector3.Distance(nodePosition, PositionToCheck); } }
/** Returns all nodes up to a given node-distance from the seed node. * This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from * the seed node. In almost all cases when \a depth is large enough this will be identical to returning all nodes which have the same area as the seed node. * In the editor areas are displayed as different colors of the nodes. * The only case where it will not be so is when there is a one way path from some part of the area to the seed node * but no path from the seed node to that part of the graph. * * The returned list is sorted by node distance from the seed node * i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through. * Note that the distance measurement does not take heuristics, penalties or tag penalties. * * Depending on the number of nodes, this function can take quite some time to calculate * so don't use it too often or it might affect the framerate of your game. * * \param seed The node to start the search from. * \param depth The maximum node-distance from the seed node. * \param tagMask Optional mask for tags. This is a bitmask. * * \returns A List<Node> containing all nodes reachable up to a specified node distance from the seed node. * For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool * * \warning This method is not thread safe. Only use it from the Unity thread (i.e normal game code). */ public static List<GraphNode> BFS(GraphNode seed, int depth, int tagMask = -1) { BFSQueue = BFSQueue ?? new Queue<GraphNode>(); var que = BFSQueue; BFSMap = BFSMap ?? new Dictionary<GraphNode,int>(); var map = BFSMap; // Even though we clear at the end of this function, it is good to // do it here as well in case the previous invocation of the method // threw an exception for some reason // and didn't clear the que and map que.Clear (); map.Clear (); List<GraphNode> result = ListPool<GraphNode>.Claim (); int currentDist = -1; GraphNodeDelegate callback; if (tagMask == -1) { callback = node => { if (node.Walkable && !map.ContainsKey (node)) { map.Add (node, currentDist+1); result.Add (node); que.Enqueue (node); } }; } else { callback = node => { if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey (node)) { map.Add (node, currentDist+1); result.Add (node); que.Enqueue (node); } }; } callback (seed); while (que.Count > 0 ) { GraphNode n = que.Dequeue (); currentDist = map[n]; if (currentDist >= depth) break; n.GetConnections (callback); } que.Clear (); map.Clear (); return result; }
static int CanTraverse(IntPtr L) { try { ToLua.CheckArgsCount(L, 2); Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1); Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2); bool o = obj.CanTraverse(arg0); LuaDLL.lua_pushboolean(L, o); return(1); } catch (Exception e) { return(LuaDLL.toluaL_exception(L, e)); } }
static int CalculateHScore(IntPtr L) { try { ToLua.CheckArgsCount(L, 2); Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1); Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2); uint o = obj.CalculateHScore(arg0); LuaDLL.lua_pushnumber(L, o); return(1); } catch (Exception e) { return(LuaDLL.toluaL_exception(L, e)); } }
public void Trace(GraphNode from) { GraphNode graphNode = from; int num = 0; while (graphNode != null) { this.path.Add(graphNode); this.vectorPath.Add((Vector3)graphNode.position); graphNode = this.flood.GetParent(graphNode); num++; if (num > 1024) { Debug.LogWarning("Inifinity loop? >1024 node path. Remove this message if you really have that long paths (FloodPathTracer.cs, Trace function)"); break; } } }
public RichSpecial Initialize(NodeLink2 nodeLink, GraphNode first) { this.nodeLink = nodeLink; if (first == nodeLink.StartNode) { this.first = nodeLink.StartTransform; this.second = nodeLink.EndTransform; this.reverse = false; } else { this.first = nodeLink.EndTransform; this.second = nodeLink.StartTransform; this.reverse = true; } return this; }
public Pathfinding.GraphNode GetClosestNode(Vector3 position) { Pathfinding.GraphNode closestNode = null; float minDistance = float.MaxValue; for (int nodeIndex = 0; nodeIndex < _nodes.Count; ++nodeIndex) { Pathfinding.GraphNode node = _nodes[nodeIndex]; float distance = Vector3.Distance((Vector3)node.position, position); if (minDistance > distance) { closestNode = node; minDistance = distance; } } return(closestNode); }
static int GetConnectionSpecialCost(IntPtr L) { try { ToLua.CheckArgsCount(L, 4); Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1); Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2); Pathfinding.GraphNode arg1 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 3); uint arg2 = (uint)LuaDLL.luaL_checknumber(L, 4); uint o = obj.GetConnectionSpecialCost(arg0, arg1, arg2); LuaDLL.lua_pushnumber(L, o); return(1); } catch (Exception e) { return(LuaDLL.toluaL_exception(L, e)); } }
/** Returns all nodes reachable from the seed node. * This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from * the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node. * In the editor areas are displayed as different colors of the nodes. * The only case where it will not be so is when there is a one way path from some part of the area to the seed node * but no path from the seed node to that part of the graph. * * The returned list is sorted by node distance from the seed node * i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through. * Note that the distance measurement does not take heuristics, penalties or tag penalties. * * Depending on the number of reachable nodes, this function can take quite some time to calculate * so don't use it too often or it might affect the framerate of your game. * * \param seed The node to start the search from * \param tagMask Optional mask for tags. This is a bitmask. * * \returns A List<Node> containing all nodes reachable from the seed node. * For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool */ public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) { #if ASTAR_PROFILE System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch(); watch.Start (); #endif Stack<GraphNode> stack = Pathfinding.Util.StackPool<GraphNode>.Claim (); List<GraphNode> list = Pathfinding.Util.ListPool<GraphNode>.Claim (); HashSet<GraphNode> map = new HashSet<GraphNode>(); GraphNodeDelegate callback; if (tagMask == -1) { callback = delegate (GraphNode node) { if (node.Walkable && map.Add (node)) { list.Add (node); stack.Push (node); } }; } else { callback = delegate (GraphNode node) { if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) { list.Add (node); stack.Push (node); } }; } callback (seed); while (stack.Count > 0) { stack.Pop ().GetConnections (callback); } Pathfinding.Util.StackPool<GraphNode>.Release (stack); #if ASTAR_PROFILE watch.Stop (); Debug.Log ((1000*watch.Elapsed.TotalSeconds).ToString("0.0 ms")); #endif return list; }
void selectTargetNode() { selectTargetTransform(); if (_targetTransform == null) { return; } TargetNodes targetNodes = _targetTransform.GetComponent <TargetNodes>(); Pathfinding.GraphNode targetNode = targetNodes.GetClosestNode(_transform.position); if (targetNode == null) { return; } _aiPath.destination = (Vector3)targetNode.position; }
public bool Contains (GraphNode node) { Vector3 point = (Vector3)node.position; //Debug.DrawRay (node.position,-Vector3.up*2,Color.magenta); if (convex) { if (_convexPoints == null) return false; for (int i=0,j=_convexPoints.Length-1;i<_convexPoints.Length;j=i,i++) { if (Polygon.Left (_convexPoints[i],_convexPoints[j],point)) return false; } } else { if (_points == null) return false; return Polygon.ContainsPoint (_points,point); } //Debug.DrawRay (node.position,Vector3.up*2,Color.blue); return true; }
public PathNode GetPathNode(GraphNode node) { int nodeIndex = node.NodeIndex; return(this.nodes[nodeIndex >> 10][nodeIndex & 1023]); }
public override bool ContainsConnection (GraphNode node) { for (int i=0;i<connections.Length;i++) if (connections[i] == node) return true; return false; }
public void AddPortal(GraphNode n1, GraphNode n2, List <Vector3> left, List <Vector3> right) { }
/** Returns if there is an obstacle between \a origin and \a end on the graph. * \param [in] _a Point to linecast from * \param [in] _b Point to linecast to * \param [out] hit Contains info on what was hit, see GraphHitInfo * \param [in] hint If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups * \param trace If a list is passed, then it will be filled with all nodes the linecast traverses * This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. * \astarpro */ public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) { hit = new GraphHitInfo (); // //Node n2 = GetNearest (_b,NNConstraint.None); _a = inverseMatrix.MultiplyPoint3x4 (_a); _a.x -= 0.5F; _a.z -= 0.5F; _b = inverseMatrix.MultiplyPoint3x4 (_b); _b.x -= 0.5F; _b.z -= 0.5F; //Grid coordinates //Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z)); //Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z)); //Clamping is needed if (_a.x < -0.5F || _a.z < -0.5F || _a.x >= width-0.5F || _a.z >= depth-0.5F || _b.x < -0.5F || _b.z < -0.5F || _b.x >= width-0.5F || _b.z >= depth-0.5F) { //Bounding points of the grid Vector3 p1 = new Vector3 (-0.5F ,0, -0.5F); Vector3 p2 = new Vector3 (-0.5F ,0, depth-0.5F); Vector3 p3 = new Vector3 (width-0.5F,0, depth-0.5F); Vector3 p4 = new Vector3 (width-0.5F,0, -0.5F); int intersectCount = 0; bool intersect = false; Vector3 intersection = Polygon.SegmentIntersectionPoint (p1,p2,_a,_b, out intersect); if (intersect) { //Debug.Log ("Intersection with p1 and p2 "+_a+" "+_b+" - Intersection: "+intersection); intersectCount++; if (!Polygon.Left (p1,p2,_a)) { _a = intersection; } else { _b = intersection; } } intersection = Polygon.SegmentIntersectionPoint (p2,p3,_a,_b, out intersect); if (intersect) { //Debug.Log ("Intersection with p2 and p3 "+_a+" "+_b+" - Intersection: "+intersection); intersectCount++; if (!Polygon.Left (p2,p3,_a)) { _a = intersection; } else { _b = intersection; } } intersection = Polygon.SegmentIntersectionPoint (p3,p4,_a,_b, out intersect); if (intersect) { //Debug.Log ("Intersection with p3 and p4 "+_a+" "+_b+" - Intersection: "+intersection); intersectCount++; if (!Polygon.Left (p3,p4,_a)) { _a = intersection; } else { _b = intersection; } } intersection = Polygon.SegmentIntersectionPoint (p4,p1,_a,_b, out intersect); if (intersect) { //Debug.Log ("Intersection with p4 and p1 "+_a+" "+_b+" - Intersection: "+intersection); intersectCount++; if (!Polygon.Left (p4,p1,_a)) { _a = intersection; } else { _b = intersection; } } if (intersectCount == 0) { //The line does not intersect with the grid return false; } } Vector3 dir = _b-_a; float magn = dir.magnitude; if (magn == 0) { //Zero length line return false; } float sampleLength = 0.2F; float newMagn = nodeSize * sampleLength; newMagn -= nodeSize * 0.02F; dir = (dir / magn) * newMagn; //Floor to int, number of samples on the line int its = (int)(magn / newMagn); //Debug.Log ("Num Its: "+its+" "+dir); Vector3 originOffset = _a + dir * nodeSize * 0.01F; GraphNode prevNode = null; for (int i=0;i <= its;i++) { Vector3 p = originOffset + dir * i; int x = Mathf.RoundToInt (p.x); int z = Mathf.RoundToInt (p.z); x = x < 0 ? 0 : (x >= width ? width-1 : x); z = z < 0 ? 0 : (z >= depth ? depth-1 : z); /*if (x < 0 || z < 0 || x >= width || z >= depth) { Debug.LogError ("Point Out Of Bounds "+"("+x+", "+z+") "+p+" in iteration "+i+" of "+its+" With a direction magn "+dir.magnitude+"\nA: "+_a+"\nB: "+_b); throw new System.IndexOutOfRangeException ("The point "+x+","+z+ " is outside the bounds of the grid"); break; }*/ GraphNode node = nodes[z*width+x]; if (node == prevNode) continue; if (!node.Walkable) { if (i > 0) { hit.point = matrix.MultiplyPoint3x4 (originOffset + dir * (i-1)+new Vector3 (0.5F,0,0.5F)); } else { hit.point = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F)); } hit.origin = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F)); hit.node = node; return true; } if (i > its-1) { if (Mathf.Abs (p.x-_b.x) <= 0.50001F || Mathf.Abs (p.z - _b.z) <= 0.50001F) { return false; } } if (trace != null) trace.Add (node); prevNode = node; } return false; }
//public void GenerateBounds () { //bounds.center = offset+new Vector3 (0,height*0.5F,0); //bounds.size = new Vector3 (width*scale,height,depth*scale); //} /** \todo Set clamped position for Grid Graph */ public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) { if (nodes == null || depth*width != nodes.Length) { return new NNInfo (); } position = inverseMatrix.MultiplyPoint3x4 (position); float xf = position.x-0.5F; float zf = position.z-0.5f; int x = Mathf.Clamp (Mathf.RoundToInt (xf) , 0, width-1); int z = Mathf.Clamp (Mathf.RoundToInt (zf) , 0, depth-1); NNInfo nn = new NNInfo(nodes[z*width+x]); float y = inverseMatrix.MultiplyPoint3x4((Vector3)nodes[z*width+x].position).y; nn.clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f,y,Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f)); //Set clamped position //nn.clampedPosition = new Vector3(Mathf.Clamp (xf,x-0.5f,x+0.5f),position.y,Mathf.Clamp (zf,z-0.5f,z+0.5f)); //nn.clampedPosition = matrix.MultiplyPoint3x4 (nn.clampedPosition); return nn; }
public override string DebugString(PathLog logMode) { if (logMode == PathLog.None || (!error && logMode == PathLog.OnlyErrors)) { return(""); } System.Text.StringBuilder text = pathHandler.DebugStringBuilder; text.Length = 0; text.Append(error ? "Path Failed : " : "Path Completed : "); text.Append("Computation Time "); text.Append((duration).ToString(logMode == PathLog.Heavy ? "0.000" : "0.00")); text.Append(" ms Searched Nodes "); text.Append(searchedNodes); if (!error) { text.Append("\nLast Found Path Length "); text.Append(path == null ? "Null" : path.Count.ToString()); if (logMode == PathLog.Heavy) { text.Append("\nSearch Iterations " + searchIterations); text.Append("\nPaths (").Append(targetsFound.Length).Append("):"); for (int i = 0; i < targetsFound.Length; i++) { text.Append("\n\n Path "+ i).Append(" Found: ").Append(targetsFound[i]); GraphNode node = nodePaths[i] == null ? null : nodePaths[i][nodePaths[i].Count - 1]; text.Append("\n Length: "); text.Append(nodePaths[i].Count); if (node != null) { PathNode nodeR = pathHandler.GetPathNode(endNode); if (nodeR != null) { text.Append("\n End Node"); text.Append("\n G: "); text.Append(nodeR.G); text.Append("\n H: "); text.Append(nodeR.H); text.Append("\n F: "); text.Append(nodeR.F); text.Append("\n Point: "); text.Append(((Vector3)endPoint).ToString()); text.Append("\n Graph: "); text.Append(endNode.GraphIndex); } else { text.Append("\n End Node: Null"); } } } text.Append("\nStart Node"); text.Append("\n Point: "); text.Append(((Vector3)endPoint).ToString()); text.Append("\n Graph: "); text.Append(startNode.GraphIndex); text.Append("\nBinary Heap size at completion: "); text.AppendLine(pathHandler.GetHeap() == null ? "Null" : (pathHandler.GetHeap().numberOfItems - 2).ToString()); // -2 because numberOfItems includes the next item to be added and item zero is not used } } if (error) { text.Append("\nError: "); text.Append(errorLog); text.AppendLine(); } if (logMode == PathLog.Heavy && !AstarPath.IsUsingMultithreading) { text.Append("\nCallback references "); if (callback != null) { text.Append(callback.Target.GetType().FullName).AppendLine(); } else { text.AppendLine("NULL"); } } text.Append("\nPath Number "); text.Append(pathID); return(text.ToString()); }
public void FloodFill(GraphNode seed) { this.FloodFill(seed, this.lastUniqueAreaIndex + 1u); this.lastUniqueAreaIndex += 1u; }
public TowerPrototype _towerToPlace = null; // If null, none selected. //private Vector3 _outOfTheWay = new Vector3(20, 0, 0); // Use this for initialization void Start() { Active = this; _spawnPoint = AstarPath.active.GetNearest(SpawnPointObject.transform.position).node; _goalPoint = AstarPath.active.GetNearest(GoalNodeObject.transform.position).node; }
private void RemoveConnections(GraphNode node) { node.ClearConnections(true); }
public override void RemoveConnection(GraphNode node) { throw new NotImplementedException(); }
public override void AddConnection(GraphNode node, uint cost) { throw new NotImplementedException(); }
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint) { return(GetNearest(this, nodes, position, constraint, accurateNearestNode)); }
void RemoveConnections(GraphNode node) { //TODO, might be better to replace connection node.ClearConnections(true); }
public override void ReturnPath() { if (error) { if (callbacks != null) { for (int i = 0; i < callbacks.Length; i++) { if (callbacks[i] != null) { callbacks[i] (this); } } } if (callback != null) { callback(this); } return; } bool anySucceded = false; Vector3 _originalStartPoint = originalStartPoint; Vector3 _startPoint = startPoint; GraphNode _startNode = startNode; for (int i = 0; i < nodePaths.Length; i++) { path = nodePaths[i]; if (path != null) { CompleteState = PathCompleteState.Complete; anySucceded = true; } else { CompleteState = PathCompleteState.Error; } if (callbacks != null && callbacks[i] != null) { vectorPath = vectorPaths[i]; //=== SEGMENT - should be identical to the one a few rows below (except "i") if (inverted) { endPoint = _startPoint; endNode = _startNode; //path[0]; startNode = targetNodes[i]; //path[path.Length-1]; startPoint = targetPoints[i]; originalEndPoint = _originalStartPoint; originalStartPoint = originalTargetPoints[i]; } else { endPoint = targetPoints[i]; originalEndPoint = originalTargetPoints[i]; endNode = targetNodes[i]; //path[path.Length-1]; } callbacks[i] (this); //In case a modifier changed the vectorPath, update the array of all vectorPaths vectorPaths[i] = vectorPath; } } if (anySucceded) { CompleteState = PathCompleteState.Complete; if (!pathsForAll) { path = nodePaths[chosenTarget]; vectorPath = vectorPaths[chosenTarget]; //=== SEGMENT - should be identical to the one a few rows above (except "chosenTarget") if (inverted) { endPoint = _startPoint; endNode = _startNode; startNode = targetNodes[chosenTarget]; startPoint = targetPoints[chosenTarget]; originalEndPoint = _originalStartPoint; originalStartPoint = originalTargetPoints[chosenTarget]; } else { endPoint = targetPoints[chosenTarget]; originalEndPoint = originalTargetPoints[chosenTarget]; endNode = targetNodes[chosenTarget]; } } } else { CompleteState = PathCompleteState.Error; } if (callback != null) { callback(this); } }
public void RecalculateCosts() { if (this.pivots == null) { this.RecalculatePivots(); } if (this.mode == HeuristicOptimizationMode.None) { return; } this.pivotCount = 0; for (int i = 0; i < this.pivots.Length; i++) { if (this.pivots[i] != null && (this.pivots[i].Destroyed || !this.pivots[i].Walkable)) { throw new Exception("Invalid pivot nodes (destroyed or unwalkable)"); } } if (this.mode != HeuristicOptimizationMode.RandomSpreadOut) { for (int j = 0; j < this.pivots.Length; j++) { if (this.pivots[j] == null) { throw new Exception("Invalid pivot nodes (null)"); } } } Debug.Log("Recalculating costs..."); this.pivotCount = this.pivots.Length; Action <int> startCostCalculation = null; int numComplete = 0; OnPathDelegate onComplete = delegate(Path path) { numComplete++; if (numComplete == this.pivotCount) { Debug.Log("Grid graph special case!"); this.ApplyGridGraphEndpointSpecialCase(); } }; startCostCalculation = delegate(int k) { GraphNode pivot = this.pivots[k]; FloodPath fp = null; fp = FloodPath.Construct(pivot, onComplete); fp.immediateCallback = delegate(Path _p) { _p.Claim(this); MeshNode meshNode = pivot as MeshNode; uint costOffset = 0u; int k; if (meshNode != null && meshNode.connectionCosts != null) { for (k = 0; k < meshNode.connectionCosts.Length; k++) { costOffset = Math.Max(costOffset, meshNode.connectionCosts[k]); } } NavGraph[] graphs = AstarPath.active.graphs; for (int m = graphs.Length - 1; m >= 0; m--) { graphs[m].GetNodes(delegate(GraphNode node) { int num6 = node.NodeIndex * this.pivotCount + k; this.EnsureCapacity(num6); PathNode pathNode = fp.pathHandler.GetPathNode(node); if (costOffset > 0u) { this.costs[num6] = ((pathNode.pathID != fp.pathID || pathNode.parent == null) ? 0u : Math.Max(pathNode.parent.G - costOffset, 0u)); } else { this.costs[num6] = ((pathNode.pathID != fp.pathID) ? 0u : pathNode.G); } return(true); }); } if (this.mode == HeuristicOptimizationMode.RandomSpreadOut && k < this.pivots.Length - 1) { if (this.pivots[k + 1] == null) { int num = -1; uint num2 = 0u; int num3 = this.maxNodeIndex / this.pivotCount; for (int n = 1; n < num3; n++) { uint num4 = 1073741824u; for (int num5 = 0; num5 <= k; num5++) { num4 = Math.Min(num4, this.costs[n * this.pivotCount + num5]); } GraphNode node2 = fp.pathHandler.GetPathNode(n).node; if ((num4 > num2 || num == -1) && node2 != null && !node2.Destroyed && node2.Walkable) { num = n; num2 = num4; } } if (num == -1) { Debug.LogError("Failed generating random pivot points for heuristic optimizations"); return; } this.pivots[k + 1] = fp.pathHandler.GetPathNode(num).node; } startCostCalculation(k + 1); } _p.Release(this, false); }; AstarPath.StartPath(fp, true); }; if (this.mode != HeuristicOptimizationMode.RandomSpreadOut) { for (int l = 0; l < this.pivots.Length; l++) { startCostCalculation(l); } } else { startCostCalculation(0); } this.dirty = false; }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public override void Prepare() { AstarProfiler.StartProfile("Get Nearest"); //Initialize the NNConstraint nnConstraint.tags = enabledTags; NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart(startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; //If it is declared that this path type has an end point //Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; // Note, other methods assume hTarget is (Int3)endPoint hTarget = (Int3)endPoint; endNode = endNNInfo.node; } AstarProfiler.EndProfile(); if (startNode == null && (hasEndPoint && endNode == null)) { Error(); LogError("Couldn't find close nodes to the start point or the end point"); return; } if (startNode == null) { Error(); LogError("Couldn't find a close node to the start point"); return; } if (endNode == null && hasEndPoint) { Error(); LogError("Couldn't find a close node to the end point"); return; } if (!startNode.Walkable) { Error(); LogError("The node closest to the start point is not walkable"); return; } if (hasEndPoint && !endNode.Walkable) { Error(); LogError("The node closest to the end point is not walkable"); return; } if (hasEndPoint && startNode.Area != endNode.Area) { Error(); LogError("There is no valid path to the target (start area: " + startNode.Area + ", target area: " + endNode.Area + ")"); return; } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; uint[] neighbourCosts = gg.neighbourCosts; GridNode[] nodes = gg.nodes; ushort pid = handler.PathID; for (int i = 0; i < 8; i++) { if (GetConnectionInternal(i)) { GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]]; if (!path.CanTraverse(other)) { continue; } PathNode otherPN = handler.GetPathNode(other); if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = neighbourCosts[i]; otherPN.H = path.CalculateHScore(other); other.UpdateG(path, otherPN); //Debug.Log ("G " + otherPN.G + " F " + otherPN.F); handler.PushNode(otherPN); //Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue); } else { //If not we can test if the path from the current node to this one is a better one then the one already used uint tmpCost = neighbourCosts[i]; if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) { //Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G); otherPN.cost = tmpCost; otherPN.parent = pathNode; other.UpdateRecursiveG(path, otherPN, handler); //Or if the path from this node ("other") to the current ("current") is better } else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G) { //Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G); pathNode.parent = otherPN; pathNode.cost = tmpCost; UpdateRecursiveG(path, pathNode, handler); } } } } #if ASTAR_GRID_CUSTOM_CONNECTIONS if (connections != null) { for (int i = 0; i < connections.Length; i++) { GraphNode other = connections[i]; if (!path.CanTraverse(other)) { continue; } PathNode otherPN = handler.GetPathNode(other); if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = connectionCosts[i]; otherPN.H = path.CalculateHScore(other); other.UpdateG(path, otherPN); //Debug.Log ("G " + otherPN.G + " F " + otherPN.F); handler.PushNode(otherPN); //Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue); } else { //If not we can test if the path from the current node to this one is a better one then the one already used uint tmpCost = connectionCosts[i]; if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) { //Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G); otherPN.cost = tmpCost; otherPN.parent = pathNode; other.UpdateRecursiveG(path, otherPN, handler); //Or if the path from this node ("other") to the current ("current") is better } else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this)) { //Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G); pathNode.parent = otherPN; pathNode.cost = tmpCost; UpdateRecursiveG(path, pathNode, handler); } } } } #endif }
public uint GetTraversalCost(Path path, GraphNode node) { // Same as default implementation return(path.GetTagPenalty((int)node.Tag) + node.Penalty); }
protected override bool EndPointGridGraphSpecialCase(GraphNode endNode) { return(false); }
public override void RemoveConnection(GraphNode node) { throw new System.NotImplementedException("GridNodes do not have support for adding manual connections"); }
public override bool GetPortal(GraphNode other, List <Vector3> left, List <Vector3> right, bool backwards) { if (backwards) { return(true); } GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; for (int i = 0; i < 4; i++) { if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]]) { Vector3 middle = ((Vector3)(position + other.position)) * 0.5f; Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position)); cross.Normalize(); cross *= gg.nodeSize * 0.5f; left.Add(middle - cross); right.Add(middle + cross); return(true); } } for (int i = 4; i < 8; i++) { if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]]) { bool rClear = false; bool lClear = false; if (GetConnectionInternal(i - 4)) { GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[i - 4]]; if (n2.Walkable && n2.GetConnectionInternal((i - 4 + 1) % 4)) { rClear = true; } } if (GetConnectionInternal((i - 4 + 1) % 4)) { GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[(i - 4 + 1) % 4]]; if (n2.Walkable && n2.GetConnectionInternal(i - 4)) { lClear = true; } } Vector3 middle = ((Vector3)(position + other.position)) * 0.5f; Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position)); cross.Normalize(); cross *= gg.nodeSize * 1.4142f; left.Add(middle - (lClear ? cross : Vector3.zero)); right.Add(middle + (rClear ? cross : Vector3.zero)); return(true); } } return(false); }
/** May be called by graph nodes to get a special cost for some connections. * Nodes may call it when PathNode.flag2 is set to true, for example mesh nodes, which have * a very large area can be marked on the start and end nodes, this method will be called * to get the actual cost for moving from the start position to its neighbours instead * of as would otherwise be the case, from the start node's position to its neighbours. * The position of a node and the actual start point on the node can vary quite a lot. * * The default behaviour of this method is to return the previous cost of the connection, * essentiall making no change at all. * * This method should return the same regardless of the order of a and b. * That is f(a,b) == f(b,a) should hold. * * \param a Moving from this node * \param b Moving to this node * \param currentCost The cost of moving between the nodes. Return this value if there is no meaningful special cost to return. */ internal virtual uint GetConnectionSpecialCost(GraphNode a, GraphNode b, uint currentCost) { return(currentCost); }
/** Returns if there is an obstacle between \a origin and \a end on the graph. * \param [in] _a Point to linecast from * \param [in] _b Point to linecast to * \param [out] hit Contains info on what was hit, see GraphHitInfo * \param [in] hint If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups * This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. * \astarpro */ public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) { return Linecast (_a, _b, hint, out hit, null); }
/** Internal method to clean up node data */ public void DestroyNode (GraphNode node) { PathNode pn = GetPathNode (node); //Clean up reference to help GC pn.node = null; pn.parent = null; }
/** Returns if \a _b is visible from \a _a on the graph. * This function is different from the other Linecast functions since it 1) snaps the start and end positions directly to the graph * and it uses Bresenham's line drawing algorithm as opposed to the others which use sampling at fixed intervals. * If you only care about if one \b node can see another \b node, then this function is great, but if you need more precision than one node, * use the normal linecast functions * * \param [in] _a Point to linecast from * \param [in] _b Point to linecast to * \param [out] hit Contains info on what was hit, see GraphHitInfo * \param [in] hint If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups * * This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. * \astarpro */ public bool SnappedLinecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) { hit = new GraphHitInfo (); //System.DateTime startTime = System.DateTime.UtcNow; GraphNode n1 = GetNearest (_a,NNConstraint.None).node; GraphNode n2 = GetNearest (_b,NNConstraint.None).node; _a = inverseMatrix.MultiplyPoint3x4 ((Vector3)n1.position); _a.x -= 0.5F; _a.z -= 0.5F; _b = inverseMatrix.MultiplyPoint3x4 ((Vector3)n2.position); _b.x -= 0.5F; _b.z -= 0.5F; Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z)); Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z)); hit.origin = (Vector3)a; //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (b*100),Color.yellow); if (!nodes[a.z*width+a.x].Walkable) { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } int dx = Mathf.Abs (a.x-b.x); int dz = Mathf.Abs (a.z-b.z); int sx = 0; int sz = 0; if (a.x < b.x) { sx = 1; } else { sx = -1; } if (a.z < b.z) { sz = 1; } else { sz = -1; } int err = dx-dz; while (true) { if (a.x == b.x && a.z == b.z) { //System.DateTime endTime2 = System.DateTime.UtcNow; //float theTime2 = (endTime2-startTime).Ticks*0.0001F; //Debug.Log ("Grid Linecast : Time "+theTime2.ToString ("0.00")); return false; } int e2 = err*2; int dir = 0; Int3 newPos = a; if (e2 > -dz) { err = err-dz; dir = sx; newPos.x += sx; } if (e2 < dx) { err = err+dx; dir += width*sz; newPos.z += sz; } if (dir == 0) { Debug.LogError ("Offset is zero, this should not happen"); return false; } for (int i=0;i<neighbourOffsets.Length;i++) { if (neighbourOffsets[i] == dir) { if (CheckConnection (nodes[a.z*width+a.x] as GridNode,i)) { if (!nodes[newPos.z*width+newPos.x].Walkable) { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100)); a = newPos; break; } else { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } } } } //Debug.DrawLine (_a,_b,Color.green); //hit.success = true; //return false; }
public PathNode GetPathNode ( GraphNode node ) { //Get the index of the node int ind = node.NodeIndex; return nodes[ind >> BucketSizeLog2][ind & BucketIndexMask]; }
/** Removes any connection from this node to the specified node. * If no such connection exists, nothing will be done. * * \note This only removes the connection from this node to the other node. * You may want to call the same function on the other node to remove its eventual connection * to this node. */ public override void RemoveConnection (GraphNode node) { if (connections == null) return; for (int i=0;i<connections.Length;i++) { if (connections[i] == node) { int connLength = connections.Length; GraphNode[] newconns = new GraphNode[connLength-1]; uint[] newconncosts = new uint[connLength-1]; for (int j=0;j<i;j++) { newconns[j] = connections[j]; newconncosts[j] = connectionCosts[j]; } for (int j=i+1;j<connLength;j++) { newconns[j-1] = connections[j]; newconncosts[j-1] = connectionCosts[j]; } connections = newconns; connectionCosts = newconncosts; return; } } }
/// <summary> /// Returns if there is a walkable path from node1 to node2. /// This method is extremely fast because it only uses precalculated information. /// /// <code> /// GraphNode node1 = AstarPath.active.GetNearest(point1, NNConstraint.Default).node; /// GraphNode node2 = AstarPath.active.GetNearest(point2, NNConstraint.Default).node; /// /// if (PathUtilities.IsPathPossible(node1, node2)) { /// // Yay, there is a path between those two nodes /// } /// </code> /// /// See: graph-updates (view in online documentation for working links) /// See: <see cref="AstarPath.GetNearest"/> /// </summary> public static bool IsPathPossible(GraphNode node1, GraphNode node2) { return(node1.Walkable && node2.Walkable && node1.Area == node2.Area); }
/// <summary> /// Check if a straight path between v1 and v2 is valid. /// If both n1 and n2 are supplied it is assumed that the line goes from the center of n1 to the center of n2 and a more optimized graph linecast may be done. /// </summary> protected bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2) { if (useRaycasting) { // Use raycasting to check if a straight path between v1 and v2 is valid if (use2DPhysics) { if (thickRaycast && thickRaycastRadius > 0 && Physics2D.CircleCast(v1 + raycastOffset, thickRaycastRadius, v2 - v1, (v2 - v1).magnitude, mask)) { return(false); } if (Physics2D.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask)) { return(false); } } else { // Perform a thick raycast (if enabled) if (thickRaycast && thickRaycastRadius > 0 && Physics.SphereCast( new Ray(v1 + raycastOffset, v2 - v1), thickRaycastRadius, (v2 - v1).magnitude, mask)) { return(false); } // Perform a normal raycast // This is done even if a thick raycast is also done because thick raycasts do not report collisions for // colliders that overlapped the (imaginary) sphere at the origin of the thick raycast. // If this raycast was not done then some obstacles could be missed. if (Physics.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask)) { return(false); } } } if (useGraphRaycasting) { if (n1 == null) { n1 = AstarPath.active.GetNearest(v1).node; } if (n2 == null) { n2 = AstarPath.active.GetNearest(v2).node; } if (n1 != null && n2 != null) { // Use graph raycasting to check if a straight path between v1 and v2 is valid var graph = n1.Graph; var graph2 = n2.Graph; if (graph != graph2) { return(false); } var rayGraph = graph as IRaycastableGraph; if (rayGraph != null) { return(!rayGraph.Linecast(v1, v2, n1)); } } } return(true); }
/** This performs a linear search through all polygons returning the closest one. * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node * complying with the NNConstraint. * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool) */ public static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { var pos = (Int3)position; float minDist = -1; GraphNode minNode = null; float minConstDist = -1; GraphNode minConstNode = null; float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; GraphNodeDelegateCancelable del = delegate(GraphNode _node) { var node = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 closest = node.ClosestPointOnNode(position); float dist = ((Vector3)pos - closest).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { if (!node.ContainsPoint((Int3)position)) { float dist = (node.position - pos).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { int dist = AstarMath.Abs(node.position.y - pos.y); if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } } return(true); }; graph.GetNodes(del); var nninfo = new NNInfo(minNode); //Find the point closest to the nearest triangle if (nninfo.node != null) { var node = nninfo.node as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minConstNode; if (nninfo.constrainedNode != null) { var node = nninfo.constrainedNode as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.constClampedPosition = clP; } return(nninfo); }
/** Internal method to initialize node data */ public void InitializeNode (GraphNode node) { //Get the index of the node int ind = node.NodeIndex; int bucketNumber = ind >> BucketSizeLog2; int bucketIndex = ind & BucketIndexMask; if (bucketNumber >= nodes.Length) { //At least increase the size to: //Current size * 1.5 //Current size + 2 or //bucketNumber PathNode[][] newNodes = new PathNode[System.Math.Max (System.Math.Max (nodes.Length*3 / 2,bucketNumber+1), nodes.Length+2)][]; for (int i=0;i<nodes.Length;i++) newNodes[i] = nodes[i]; //Debug.Log ("Resizing Bucket List from " + nodes.Length + " to " + newNodes.Length + " (bucketNumber="+bucketNumber+")"); bool[] newBucketNew = new bool[newNodes.Length]; for (int i=0;i<nodes.Length;i++) newBucketNew[i] = bucketNew[i]; bool[] newBucketCreated = new bool[newNodes.Length]; for (int i=0;i<nodes.Length;i++) newBucketCreated[i] = bucketCreated[i]; nodes = newNodes; bucketNew = newBucketNew; bucketCreated = newBucketCreated; } if (nodes[bucketNumber] == null) { PathNode[] ns; if (bucketCache.Count > 0) { ns = bucketCache.Pop(); } else { ns = new PathNode[BucketSize]; for (int i=0;i<BucketSize;i++) ns[i] = new PathNode (); } nodes[bucketNumber] = ns; if (!bucketCreated[bucketNumber]) { bucketNew[bucketNumber] = true; bucketCreated[bucketNumber] = true; } filledBuckets++; } PathNode pn = nodes[bucketNumber][bucketIndex]; pn.node = node; }
public NNInfo(NNInfoInternal internalInfo) { node = internalInfo.node; position = internalInfo.clampedPosition; }
/* Color to use for gizmos. * Returns a color to be used for the specified node with the current debug settings (editor only) */ public virtual Color NodeColor(GraphNode node, PathHandler data) { Color c = AstarColor.NodeConnection; bool colSet = false; if (node == null) { return(AstarColor.NodeConnection); } switch (AstarPath.active.debugMode) { case GraphDebugMode.Areas: c = AstarColor.GetAreaColor(node.Area); colSet = true; break; case GraphDebugMode.Penalty: c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)node.Penalty / (float)AstarPath.active.debugRoof); colSet = true; break; case GraphDebugMode.Tags: c = AstarMath.IntToColor((int)node.Tag, 0.5F); colSet = true; break; /* Wasn't really usefull * case GraphDebugMode.Position: * float r = Mathf.PingPong (node.position.x/10000F,1F) + Mathf.PingPong (node.position.x/300000F,1F); * float g = Mathf.PingPong (node.position.y/10000F,1F) + Mathf.PingPong (node.position.y/200000F,1F); * float b = Mathf.PingPong (node.position.z/10000F,1F) + Mathf.PingPong (node.position.z/100000F,1F); * * * c = new Color (r,g,b); * break; */ } if (!colSet) { if (data == null) { return(AstarColor.NodeConnection); } PathNode nodeR = data.GetPathNode(node); switch (AstarPath.active.debugMode) { case GraphDebugMode.G: //c = Mathfx.IntToColor (node.g,0.5F); c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.G / (float)AstarPath.active.debugRoof); break; case GraphDebugMode.H: c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.H / (float)AstarPath.active.debugRoof); break; case GraphDebugMode.F: c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.F / (float)AstarPath.active.debugRoof); break; } } c.a *= 0.5F; return(c); }
/// <summary> /// Returns all nodes up to a given node-distance from the seed node. /// This function performs a BFS (<a href="https://en.wikipedia.org/wiki/Breadth-first_search">breadth-first search</a>) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from /// the seed node. In almost all cases when depth is large enough this will be identical to returning all nodes which have the same area as the seed node. /// In the editor areas are displayed as different colors of the nodes. /// The only case where it will not be so is when there is a one way path from some part of the area to the seed node /// but no path from the seed node to that part of the graph. /// /// The returned list is sorted by node distance from the seed node /// i.e distance is measured in the number of nodes the shortest path from seed to that node would pass through. /// Note that the distance measurement does not take heuristics, penalties or tag penalties. /// /// Depending on the number of nodes, this function can take quite some time to calculate /// so don't use it too often or it might affect the framerate of your game. /// /// Returns: A List<GraphNode> containing all nodes reachable up to a specified node distance from the seed node. /// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool /// /// Warning: This method is not thread safe. Only use it from the Unity thread (i.e normal game code). /// /// The video below shows the BFS result with varying values of depth. Points are sampled on the nodes using <see cref="GetPointsOnNodes"/>. /// [Open online documentation to see videos] /// </summary> /// <param name="seed">The node to start the search from.</param> /// <param name="depth">The maximum node-distance from the seed node.</param> /// <param name="tagMask">Optional mask for tags. This is a bitmask.</param> /// <param name="filter">Optional filter for which nodes to search. You can combine this with depth = int.MaxValue and tagMask = -1 to make the filter determine everything. /// Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable.</param> public static List <GraphNode> BFS(GraphNode seed, int depth, int tagMask = -1, System.Func <GraphNode, bool> filter = null) { #if ASTAR_PROFILE System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch(); watch.Start(); #endif BFSQueue = BFSQueue ?? new Queue <GraphNode>(); var que = BFSQueue; BFSMap = BFSMap ?? new Dictionary <GraphNode, int>(); var map = BFSMap; // Even though we clear at the end of this function, it is good to // do it here as well in case the previous invocation of the method // threw an exception for some reason // and didn't clear the que and map que.Clear(); map.Clear(); List <GraphNode> result = ListPool <GraphNode> .Claim(); int currentDist = -1; System.Action <GraphNode> callback; if (tagMask == -1) { callback = node => { if (node.Walkable && !map.ContainsKey(node)) { if (filter != null && !filter(node)) { return; } map.Add(node, currentDist + 1); result.Add(node); que.Enqueue(node); } }; } else { callback = node => { if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey(node)) { if (filter != null && !filter(node)) { return; } map.Add(node, currentDist + 1); result.Add(node); que.Enqueue(node); } }; } callback(seed); while (que.Count > 0) { GraphNode n = que.Dequeue(); currentDist = map[n]; if (currentDist >= depth) { break; } n.GetConnections(callback); } que.Clear(); map.Clear(); #if ASTAR_PROFILE watch.Stop(); Debug.Log((1000 * watch.Elapsed.TotalSeconds).ToString("0.0 ms")); #endif return(result); }