public GetPathNode ( |
||
node | ||
Résultat | PathNode |
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) { UpdateG (path,pathNode); handler.PushNode (pathNode); for (int i=0;i<connections.Length;i++) { GraphNode other = connections[i]; PathNode otherPN = handler.GetPathNode (other); if (otherPN.parent == pathNode && otherPN.pathID == handler.PathID) { other.UpdateRecursiveG (path, otherPN,handler); } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { if (this.connections == null) { return; } for (int i = 0; i < this.connections.Length; i++) { GraphNode graphNode = this.connections[i]; if (path.CanTraverse(graphNode)) { PathNode pathNode2 = handler.GetPathNode(graphNode); if (pathNode2.pathID != handler.PathID) { pathNode2.node = graphNode; pathNode2.parent = pathNode; pathNode2.pathID = handler.PathID; pathNode2.cost = this.connectionCosts[i]; pathNode2.H = path.CalculateHScore(graphNode); graphNode.UpdateG(path, pathNode2); handler.PushNode(pathNode2); } else { uint num = this.connectionCosts[i]; if (pathNode.G + num + path.GetTraversalCost(graphNode) < pathNode2.G) { pathNode2.cost = num; pathNode2.parent = pathNode; graphNode.UpdateRecursiveG(path, pathNode2, handler); } else if (pathNode2.G + num + path.GetTraversalCost(this) < pathNode.G && graphNode.ContainsConnection(this)) { pathNode.parent = pathNode2; pathNode.cost = num; this.UpdateRecursiveG(path, pathNode, handler); } } } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); ushort pid = handler.PathID; #if ASTAR_JPS if (gg.useJumpPointSearch && !path.FloodingPath) { JPSOpen(path, pathNode, handler); } else #endif { int[] neighbourOffsets = gg.neighbourOffsets; uint[] neighbourCosts = gg.neighbourCosts; GridNode[] nodes = gg.nodes; 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); uint tmpCost = neighbourCosts[i]; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used #if ASTAR_NO_TRAVERSAL_COST if (pathNode.G+tmpCost < otherPN.G) #else if (pathNode.G+tmpCost+path.GetTraversalCost(other) < otherPN.G) #endif { //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 } #if ASTAR_NO_TRAVERSAL_COST else if (otherPN.G+tmpCost < pathNode.G) #else else if (otherPN.G+tmpCost+path.GetTraversalCost(this) < pathNode.G) #endif { //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_NO_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); uint tmpCost = connectionCosts[i]; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used #if ASTAR_NO_TRAVERSAL_COST if (pathNode.G+tmpCost < otherPN.G) #else if (pathNode.G+tmpCost+path.GetTraversalCost(other) < otherPN.G) #endif { //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 } #if ASTAR_NO_TRAVERSAL_COST else if (otherPN.G+tmpCost < pathNode.G && other.ContainsConnection(this)) #else else if (otherPN.G+tmpCost+path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this)) #endif { //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 override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) { //BaseUpdateAllG (nodeR, nodeRunData); handler.PushNode (pathNode); UpdateG (path, pathNode); LayerGridGraph graph = GetGridGraph (GraphIndex); int[] neighbourOffsets = graph.neighbourOffsets; LevelGridNode[] nodes = graph.nodes; int index = NodeInGridIndex; for (int i=0;i<4;i++) { int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF; if (conn != LevelGridNode.NoConnection) { LevelGridNode other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; PathNode otherPN = handler.GetPathNode (other); if (otherPN != null && otherPN.parent == pathNode && otherPN.pathID == handler.PathID) { other.UpdateRecursiveG (path, otherPN,handler); } } } }
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; UpdateG(path, pathNode); handler.PushNode(pathNode); ushort pid = handler.PathID; for (int i = 0; i < 8; i++) { if (GetConnectionInternal(i)) { GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]]; PathNode otherPN = handler.GetPathNode(other); if (otherPN.parent == pathNode && otherPN.pathID == pid) other.UpdateRecursiveG(path, otherPN, handler); } } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS if (connections != null) for (int i = 0; i < connections.Length; i++) { GraphNode other = connections[i]; PathNode otherPN = handler.GetPathNode(other); if (otherPN.parent == pathNode && otherPN.pathID == pid) other.UpdateRecursiveG(path, otherPN, handler); } #endif }
/** Executes a diagonal jump search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ GridNode JPSJumpDiagonal (Path path, PathHandler handler, int parentDir, int depth = 0) { // Indexing into the cache arrays from multiple threads like this should cause // a lot of false sharing and cache trashing, but after profiling it seems // that this is not a major concern int threadID = handler.threadID; int threadOffset = 8*handler.threadID; // This is needed to make sure different threads don't overwrite each others results // It doesn't matter if we throw away some caching done by other threads as this will only // happen during the first few path requests if (JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount) { lock (this) { // Check again in case another thread has already created the array if (JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount) { JPSCache = new GridNode[8*handler.totalThreadCount]; JPSDead = new byte[handler.totalThreadCount]; JPSLastCacheID = new ushort[handler.totalThreadCount]; } } } if (JPSLastCacheID[threadID] != path.pathID) { for (int i = 0; i < 8; i++) JPSCache[i + threadOffset] = null; JPSLastCacheID[threadID] = path.pathID; JPSDead[threadID] = 0; } // Cache earlier results, major optimization // It is important to read from it once and then return the same result, // if we read from it twice, we might get different results due to other threads clearing the array sometimes GridNode cachedResult = JPSCache[parentDir + threadOffset]; if (cachedResult != null) { //return cachedResult; } //if ( ((JPSDead[threadID] >> parentDir)&1) != 0 ) return null; // Special node (e.g end node), take care of if (handler.GetPathNode(this).flag2) { //Debug.Log ("Found end Node!"); //Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green); JPSCache[parentDir + threadOffset] = this; return this; } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS // Special node which has custom connections, take care of if (connections != null && connections.Length > 0) { JPSCache[parentDir] = this; return this; } #endif int noncyclic = gridFlags;//We don't actually need to & with this because we don't use the other bits. & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) cyclic |= ((noncyclic >> i)&0x1) << JPSCyclic[i]; int forced = 0; int cyclicParentDir = JPSCyclic[parentDir]; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; int natural; for (int i = 0; i < 8; i++) if (((cyclic >> i)&1) == 0) forced |= JPSForcedDiagonal[i]; natural = JPSNaturalDiagonalNeighbours; /* * if ( ((Vector3)position - new Vector3(1.5f,0,-1.5f)).magnitude < 0.5f ) { * Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir); * Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2)); * }*/ // Don't force nodes we cannot reach anyway forced &= cyclic; natural &= cyclic; if ((forced & (~natural)) != 0) { // Some of the neighbour nodes are forced JPSCache[parentDir+threadOffset] = this; return this; } int forwardDir; GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; { // Rotate 180 degrees - 1 node forwardDir = 3; if (((cyclic >> forwardDir)&1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.black); GridNode v; if (oi < 4) { v = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(cyclicParentDir-1+8)%8], depth+1); } else { v = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(cyclicParentDir-1+8)%8], depth+1); } if (v != null) { JPSCache[parentDir+threadOffset] = this; return this; } } // Rotate 180 degrees + 1 node forwardDir = 5; if (((cyclic >> forwardDir)&1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.grey); GridNode v; if (oi < 4) { v = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(cyclicParentDir+1+8)%8], depth+1); } else { v = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(cyclicParentDir+1+8)%8], depth+1); } if (v != null) { JPSCache[parentDir+threadOffset] = this; return this; } } } // Rotate 180 degrees forwardDir = 4; if (((cyclic >> forwardDir)&1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta); var v = other.JPSJumpDiagonal(path, handler, parentDir, depth+1); if (v != null) { JPSCache[parentDir+threadOffset] = v; return v; } } JPSDead[threadID] |= (byte)(1 << parentDir); return null; }
public override void OnDrawGizmos(bool drawNodes) { if (!drawNodes) { return; } Matrix4x4 preMatrix = matrix; GenerateMatrix(); if (nodes == null) { //Scan (); } if (nodes == null) { return; } if (bbTree != null) { bbTree.OnDrawGizmos(); } if (preMatrix != matrix) { //Debug.Log ("Relocating Nodes"); RelocateNodes(preMatrix, matrix); } PathHandler debugData = AstarPath.active.debugPathData; for (int i = 0; i < nodes.Length; i++) { TriangleMeshNode node = (TriangleMeshNode)nodes[i]; Gizmos.color = NodeColor(node, AstarPath.active.debugPathData); if (node.Walkable) { if (AstarPath.active.showSearchTree && debugData != null && debugData.GetPathNode(node).parent != null) { Gizmos.DrawLine((Vector3)node.position, (Vector3)debugData.GetPathNode(node).parent.node.position); } else { for (int q = 0; q < node.connections.Length; q++) { Gizmos.DrawLine((Vector3)node.position, Vector3.Lerp((Vector3)node.position, (Vector3)node.connections[q].position, 0.45f)); } } Gizmos.color = AstarColor.MeshEdgeColor; } else { Gizmos.color = Color.red; } Gizmos.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1]); Gizmos.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2]); Gizmos.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0]); } }
/* Color to use for gizmos. * Returns a color to be used for the specified node with the current debug settings (editor only). * * \version Since 3.6.1 this method will not handle null nodes */ public virtual Color NodeColor (GraphNode node, PathHandler data) { Color c = AstarColor.NodeConnection; switch (AstarPath.active.debugMode) { case GraphDebugMode.Areas: c = AstarColor.GetAreaColor (node.Area); break; case GraphDebugMode.Penalty: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)node.Penalty-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; case GraphDebugMode.Tags: c = AstarMath.IntToColor ((int)node.Tag,0.5F); break; default: if (data == null) return AstarColor.NodeConnection; PathNode nodeR = data.GetPathNode (node); switch (AstarPath.active.debugMode) { case GraphDebugMode.G: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.G-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; case GraphDebugMode.H: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.H-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; case GraphDebugMode.F: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.F-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; } break; } c.a *= 0.5F; return c; }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); ushort pid = handler.PathID; #if ASTAR_JPS if (gg.useJumpPointSearch && !path.FloodingPath) { JPSOpen(path, pathNode, handler); } else #endif { int[] neighbourOffsets = gg.neighbourOffsets; uint[] neighbourCosts = gg.neighbourCosts; GridNode[] nodes = gg.nodes; var index = NodeInGridIndex; for (int i = 0; i < 8; i++) { if (HasConnectionInDirection(i)) { GridNode other = nodes[index + neighbourOffsets[i]]; if (!path.CanTraverse(other)) { continue; } PathNode otherPN = handler.GetPathNode(other); uint tmpCost = neighbourCosts[i]; // Check if the other node has not yet been visited by this path if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; otherPN.H = path.CalculateHScore(other); otherPN.UpdateG(path); handler.heap.Add(otherPN); } else { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used #if ASTAR_NO_TRAVERSAL_COST if (pathNode.G + tmpCost < otherPN.G) #else if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) #endif { //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); } } } } } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS base.Open(path, pathNode, handler); #endif }
public virtual Color NodeColor(GraphNode node, PathHandler data) { Color result = AstarColor.NodeConnection; GraphDebugMode debugMode = AstarPath.active.debugMode; switch (debugMode) { case GraphDebugMode.Penalty: result = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (node.Penalty - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); goto IL_18C; case GraphDebugMode.Connections: { IL_25: if (debugMode == GraphDebugMode.Areas) { result = AstarColor.GetAreaColor(node.Area); goto IL_18C; } if (data == null) { return AstarColor.NodeConnection; } PathNode pathNode = data.GetPathNode(node); switch (AstarPath.active.debugMode) { case GraphDebugMode.G: result = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (pathNode.G - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; case GraphDebugMode.H: result = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (pathNode.H - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; case GraphDebugMode.F: result = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (pathNode.F - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; } goto IL_18C; } case GraphDebugMode.Tags: result = AstarMath.IntToColor((int)node.Tag, 0.5f); goto IL_18C; } goto IL_25; IL_18C: result.a *= 0.5f; return result; }
/** Executes a diagonal jump search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ GridNode JPSJumpDiagonal(Path path, PathHandler handler, int parentDir, int depth = 0) { // Indexing into the cache arrays from multiple threads like this should cause // a lot of false sharing and cache trashing, but after profiling it seems // that this is not a major concern int threadID = handler.threadID; int threadOffset = 8 * handler.threadID; // This is needed to make sure different threads don't overwrite each others results // It doesn't matter if we throw away some caching done by other threads as this will only // happen during the first few path requests if (JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount) { lock (this) { // Check again in case another thread has already created the array if (JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount) { JPSCache = new GridNode[8 * handler.totalThreadCount]; JPSDead = new byte[handler.totalThreadCount]; JPSLastCacheID = new ushort[handler.totalThreadCount]; } } } if (JPSLastCacheID[threadID] != path.pathID) { for (int i = 0; i < 8; i++) { JPSCache[i + threadOffset] = null; } JPSLastCacheID[threadID] = path.pathID; JPSDead[threadID] = 0; } // Cache earlier results, major optimization // It is important to read from it once and then return the same result, // if we read from it twice, we might get different results due to other threads clearing the array sometimes GridNode cachedResult = JPSCache[parentDir + threadOffset]; if (cachedResult != null) { //return cachedResult; } //if ( ((JPSDead[threadID] >> parentDir)&1) != 0 ) return null; // Special node (e.g end node), take care of if (handler.GetPathNode(this).flag2) { //Debug.Log ("Found end Node!"); //Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green); JPSCache[parentDir + threadOffset] = this; return(this); } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS // Special node which has custom connections, take care of if (connections != null && connections.Length > 0) { JPSCache[parentDir] = this; return(this); } #endif int noncyclic = gridFlags; //We don't actually need to & with this because we don't use the other bits. & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) { cyclic |= ((noncyclic >> i) & 0x1) << JPSCyclic[i]; } int forced = 0; int cyclicParentDir = JPSCyclic[parentDir]; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; int natural; for (int i = 0; i < 8; i++) { if (((cyclic >> i) & 1) == 0) { forced |= JPSForcedDiagonal[i]; } } natural = JPSNaturalDiagonalNeighbours; /* * if ( ((Vector3)position - new Vector3(1.5f,0,-1.5f)).magnitude < 0.5f ) { * Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir); * Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2)); * }*/ // Don't force nodes we cannot reach anyway forced &= cyclic; natural &= cyclic; if ((forced & (~natural)) != 0) { // Some of the neighbour nodes are forced JPSCache[parentDir + threadOffset] = this; return(this); } int forwardDir; GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; { // Rotate 180 degrees - 1 node forwardDir = 3; if (((cyclic >> forwardDir) & 1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.black); GridNode v; if (oi < 4) { v = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(cyclicParentDir - 1 + 8) % 8], depth + 1); } else { v = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(cyclicParentDir - 1 + 8) % 8], depth + 1); } if (v != null) { JPSCache[parentDir + threadOffset] = this; return(this); } } // Rotate 180 degrees + 1 node forwardDir = 5; if (((cyclic >> forwardDir) & 1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.grey); GridNode v; if (oi < 4) { v = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(cyclicParentDir + 1 + 8) % 8], depth + 1); } else { v = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(cyclicParentDir + 1 + 8) % 8], depth + 1); } if (v != null) { JPSCache[parentDir + threadOffset] = this; return(this); } } } // Rotate 180 degrees forwardDir = 4; if (((cyclic >> forwardDir) & 1) != 0) { int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta); var v = other.JPSJumpDiagonal(path, handler, parentDir, depth + 1); if (v != null) { JPSCache[parentDir + threadOffset] = v; return(v); } } JPSDead[threadID] |= (byte)(1 << parentDir); return(null); }
/** Opens a node using Jump Point Search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ public void JPSOpen(Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; ushort pid = handler.PathID; int noncyclic = gridFlags & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) { cyclic |= ((noncyclic >> i) & 0x1) << JPSCyclic[i]; } var parent = pathNode.parent != null ? pathNode.parent.node as GridNode : null; int parentDir = -1; if (parent != null) { int diff = parent != null ? parent.nodeInGridIndex - nodeInGridIndex : 0; int x2 = nodeInGridIndex % gg.width; int x1 = parent.nodeInGridIndex % gg.width; if (diff < 0) { if (x1 == x2) { parentDir = 0; } else if (x1 < x2) { parentDir = 7; } else { parentDir = 4; } } else { if (x1 == x2) { parentDir = 1; } else if (x1 < x2) { parentDir = 6; } else { parentDir = 5; } } } int cyclicParentDir = 0; // Check for -1 int forced = 0; if (parentDir != -1) { cyclicParentDir = JPSCyclic[parentDir]; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; } else { forced = 0xFF; //parentDir = 0; } bool diagonal = parentDir >= 4; int natural; if (diagonal) { for (int i = 0; i < 8; i++) { if (((cyclic >> i) & 1) == 0) { forced |= JPSForcedDiagonal[i]; } } natural = JPSNaturalDiagonalNeighbours; } else { for (int i = 0; i < 8; i++) { if (((cyclic >> i) & 1) == 0) { forced |= JPSForced[i]; } } natural = JPSNaturalStraightNeighbours; } // Don't force nodes we cannot reach anyway forced &= cyclic; natural &= cyclic; int nb = forced | natural; /*if ( ((Vector3)position - new Vector3(0.5f,0,3.5f)).magnitude < 0.5f ) { * Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir); * Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2)); * }*/ for (int i = 0; i < 8; i++) { if (((nb >> i) & 1) != 0) { int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; #if ASTARDEBUG if (((forced >> i) & 1) != 0) { Debug.DrawLine((Vector3)position, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f), Color.red); } if (((natural >> i) & 1) != 0) { Debug.DrawLine((Vector3)position + Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) + Vector3.up * 0.2f, Color.green); } #endif if (oi < 4) { other = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]); } else { other = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]); } if (other != null) { //Debug.DrawLine ( (Vector3)position + Vector3.up*0.0f, (Vector3)other.position + Vector3.up*0.3f, Color.cyan); //Debug.DrawRay ( (Vector3)other.position, Vector3.up, Color.cyan); //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 = (uint)(other.position - position).costMagnitude; //neighbourCosts[i]; otherPN.H = path.CalculateHScore(other); otherPN.UpdateG(path); //Debug.Log ("G " + otherPN.G + " F " + otherPN.F); handler.heap.Add(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 = (uint)(other.position - position).costMagnitude; //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); } } } } #if ASTARDEBUG if (i == 0 && parentDir != -1 && this.nodeInGridIndex > 10) { int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8]; if (nodeInGridIndex + neighbourOffsets[oi] < 0 || nodeInGridIndex + neighbourOffsets[oi] >= nodes.Length) { //Debug.LogError ("ERR: " + (nodeInGridIndex + neighbourOffsets[oi]) + " " + cyclicParentDir + " " + parentDir + " Reverted " + oi); //Debug.DrawRay ((Vector3)position, Vector3.up, Color.red); } else { GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; Debug.DrawLine((Vector3)position - Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) - Vector3.up * 0.2f, Color.blue); } } #endif } }
/** Executes a straight jump search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ static GridNode JPSJumpStraight(GridNode node, Path path, PathHandler handler, int parentDir, int depth = 0) { GridGraph gg = GetGridGraph(node.GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; GridNode origin = node; // Indexing into the cache arrays from multiple threads like this should cause // a lot of false sharing and cache trashing, but after profiling it seems // that this is not a major concern int threadID = handler.threadID; int threadOffset = 8 * handler.threadID; int cyclicParentDir = JPSCyclic[parentDir]; GridNode result = null; // Rotate 180 degrees const int forwardDir = 4; int forwardOffset = neighbourOffsets[JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]]; // Move forwards in the same direction // until a node is encountered which we either // * know the result for (memoization) // * is a special node (flag2 set) // * has custom connections // * the node has a forced neighbour // Then break out of the loop // and start another loop which goes through the same nodes and sets the // memoization caches to avoid expensive calls in the future while (true) { // This is needed to make sure different threads don't overwrite each others results // It doesn't matter if we throw away some caching done by other threads as this will only // happen during the first few path requests if (node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount) { lock (node) { // Check again in case another thread has already created the array if (node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount) { node.JPSCache = new GridNode[8 * handler.totalThreadCount]; node.JPSDead = new byte[handler.totalThreadCount]; node.JPSLastCacheID = new ushort[handler.totalThreadCount]; } } } if (node.JPSLastCacheID[threadID] != path.pathID) { for (int i = 0; i < 8; i++) { node.JPSCache[i + threadOffset] = null; } node.JPSLastCacheID[threadID] = path.pathID; node.JPSDead[threadID] = 0; } // Cache earlier results, major optimization // It is important to read from it once and then return the same result, // if we read from it twice, we might get different results due to other threads clearing the array sometimes GridNode cachedResult = node.JPSCache[parentDir + threadOffset]; if (cachedResult != null) { result = cachedResult; break; } if (((node.JPSDead[threadID] >> parentDir) & 1) != 0) { return(null); } // Special node (e.g end node), take care of if (handler.GetPathNode(node).flag2) { //Debug.Log ("Found end Node!"); //Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green); result = node; break; } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS // Special node which has custom connections, take care of if (node.connections != null && node.connections.Length > 0) { result = node; break; } #endif // These are the nodes this node is connected to, one bit for each of the 8 directions int noncyclic = node.gridFlags; //We don't actually need to & with this because we don't use the other bits. & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) { cyclic |= ((noncyclic >> i) & 0x1) << JPSCyclic[i]; } int forced = 0; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; //for ( int i = 0; i < 8; i++ ) if ( ((cyclic >> i)&1) == 0 ) forced |= JPSForced[i]; if ((cyclic & (1 << 2)) == 0) { forced |= (1 << 3); } if ((cyclic & (1 << 6)) == 0) { forced |= (1 << 5); } int natural = JPSNaturalStraightNeighbours; // Check if there are any forced neighbours which we can reach that are not natural neighbours //if ( ((forced & cyclic) & (~(natural & cyclic))) != 0 ) { if ((forced & (~natural) & cyclic) != 0) { // Some of the neighbour nodes are forced result = node; break; } // Make sure we can reach the next node if ((cyclic & (1 << forwardDir)) != 0) { node = nodes[node.nodeInGridIndex + forwardOffset]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta); } else { result = null; break; } } if (result == null) { while (origin != node) { origin.JPSDead[threadID] |= (byte)(1 << parentDir); origin = nodes[origin.nodeInGridIndex + forwardOffset]; } } else { while (origin != node) { origin.JPSCache[parentDir + threadOffset] = result; origin = nodes[origin.nodeInGridIndex + forwardOffset]; } } return(result); }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { if (connections == null) { return; } // Flag2 indicates if this node needs special treatment // with regard to connection costs var flag2 = pathNode.flag2; // Loop through all connections for (var i = connections.Length - 1; i >= 0; i--) { var conn = connections[i]; var other = conn.node; // Make sure we can traverse the neighbour if (path.CanTraverse(conn.node)) { var pathOther = handler.GetPathNode(conn.node); // Fast path out, worth it for triangle mesh nodes since they usually have degree 2 or 3 if (pathOther == pathNode.parent) { continue; } var cost = conn.cost; if (flag2 || pathOther.flag2) // Get special connection cost from the path // This is used by the start and end nodes { cost = path.GetConnectionSpecialCost(this, conn.node, cost); } // Test if we have seen the other node before if (pathOther.pathID != handler.PathID) { // We have not seen the other node before // So the path from the start through this node to the other node // must be the shortest one so far // Might not be assigned pathOther.node = conn.node; pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = cost; pathOther.H = path.CalculateHScore(other); pathOther.UpdateG(path); handler.heap.Add(pathOther); } else { // If not we can test if the path from this node to the other one is a better one than the one already used if (pathNode.G + cost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = cost; pathOther.parent = pathNode; other.UpdateRecursiveG(path, pathOther, handler); } } } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { if (connections == null) { return; } for (int i = 0; i < connections.Length; i++) { GraphNode other = connections[i]; if (path.CanTraverse(other)) { PathNode pathOther = handler.GetPathNode(other); if (pathOther.pathID != handler.PathID) { //Might not be assigned pathOther.node = other; pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = connectionCosts[i]; pathOther.H = path.CalculateHScore(other); other.UpdateG(path, pathOther); handler.PushNode(pathOther); } else { //If not we can test if the path from this node to the other one is a better one then the one already used uint tmpCost = connectionCosts[i]; if (pathNode.G + tmpCost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = tmpCost; pathOther.parent = pathNode; //other.UpdateAllG (pathOther,handler); other.UpdateRecursiveG(path, pathOther, handler); //handler.PushNode (pathOther); } else if (pathOther.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this)) { //Or if the path from the other node to this one is better pathNode.parent = pathOther; pathNode.cost = tmpCost; //UpdateAllG (pathNode,handler); UpdateRecursiveG(path, pathNode, handler); //handler.PushNode (pathNode); } } } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gridGraph = GridNode.GetGridGraph(base.GraphIndex); ushort pathID = handler.PathID; int[] neighbourOffsets = gridGraph.neighbourOffsets; uint[] neighbourCosts = gridGraph.neighbourCosts; GridNode[] nodes = gridGraph.nodes; for (int i = 0; i < 8; i++) { if (this.GetConnectionInternal(i)) { GridNode gridNode = nodes[this.nodeInGridIndex + neighbourOffsets[i]]; if (path.CanTraverse(gridNode)) { PathNode pathNode2 = handler.GetPathNode(gridNode); uint num = neighbourCosts[i]; if (pathNode2.pathID != pathID) { pathNode2.parent = pathNode; pathNode2.pathID = pathID; pathNode2.cost = num; pathNode2.H = path.CalculateHScore(gridNode); gridNode.UpdateG(path, pathNode2); handler.PushNode(pathNode2); } else if (pathNode.G + num + path.GetTraversalCost(gridNode) < pathNode2.G) { pathNode2.cost = num; pathNode2.parent = pathNode; gridNode.UpdateRecursiveG(path, pathNode2, handler); } else if (pathNode2.G + num + path.GetTraversalCost(this) < pathNode.G) { pathNode.parent = pathNode2; pathNode.cost = num; this.UpdateRecursiveG(path, pathNode, handler); } } } } if (this.connections != null) { for (int j = 0; j < this.connections.Length; j++) { GraphNode graphNode = this.connections[j]; if (path.CanTraverse(graphNode)) { PathNode pathNode3 = handler.GetPathNode(graphNode); uint num2 = this.connectionCosts[j]; if (pathNode3.pathID != pathID) { pathNode3.parent = pathNode; pathNode3.pathID = pathID; pathNode3.cost = num2; pathNode3.H = path.CalculateHScore(graphNode); graphNode.UpdateG(path, pathNode3); handler.PushNode(pathNode3); } else if (pathNode.G + num2 + path.GetTraversalCost(graphNode) < pathNode3.G) { pathNode3.cost = num2; pathNode3.parent = pathNode; graphNode.UpdateRecursiveG(path, pathNode3, handler); } else if (pathNode3.G + num2 + path.GetTraversalCost(this) < pathNode.G && graphNode.ContainsConnection(this)) { pathNode.parent = pathNode3; pathNode.cost = num2; this.UpdateRecursiveG(path, pathNode, handler); } } } } }
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph (GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; UpdateG (path,pathNode); handler.PushNode (pathNode); ushort pid = handler.PathID; for (int i=0;i<8;i++) { if (GetConnectionInternal(i)) { GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]]; PathNode otherPN = handler.GetPathNode (other); if (otherPN.parent == pathNode && otherPN.pathID == pid) other.UpdateRecursiveG (path, otherPN,handler); } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { if (connections == null) return; for (int i=0;i<connections.Length;i++) { GraphNode other = connections[i]; if (path.CanTraverse (other)) { PathNode pathOther = handler.GetPathNode (other); if (pathOther.pathID != handler.PathID) { pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = connectionCosts[i]; pathOther.H = path.CalculateHScore (other); other.UpdateG (path, pathOther); handler.PushNode (pathOther); } else { //If not we can test if the path from this node to the other one is a better one then the one already used uint tmpCost = connectionCosts[i]; if (pathNode.G + tmpCost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = tmpCost; pathOther.parent = pathNode; other.UpdateRecursiveG (path, pathOther,handler); //handler.PushNode (pathOther); } else if (pathOther.G+tmpCost+path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection (this)) { //Or if the path from the other node to this one is better pathNode.parent = pathOther; pathNode.cost = tmpCost; UpdateRecursiveG (path, pathNode,handler); //handler.PushNode (pathNode); } } } } }
public override void UpdateRecursiveG(Path path, PathNode pathNode, PathHandler handler) { base.UpdateG(path, pathNode); handler.PushNode(pathNode); for (int i = 0; i < this.connections.Length; i++) { GraphNode graphNode = this.connections[i]; PathNode pathNode2 = handler.GetPathNode(graphNode); if (pathNode2.parent == pathNode && pathNode2.pathID == handler.PathID) { graphNode.UpdateRecursiveG(path, pathNode2, handler); } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { ushort pid = handler.PathID; if (connections != null) { for (int i = 0; i < connections.Length; i++) { GraphNode other = connections[i].node; if (!path.CanTraverse(other)) { continue; } PathNode otherPN = handler.GetPathNode(other); uint tmpCost = connections[i].cost; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; otherPN.H = path.CalculateHScore(other); other.UpdateG(path, otherPN); //Debug.Log ("G " + otherPN.G + " F " + otherPN.F); handler.heap.Add(otherPN); //Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue); } else { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used #if ASTAR_NO_TRAVERSAL_COST if (pathNode.G + tmpCost < otherPN.G) #else if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) #endif { //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 } #if ASTAR_NO_TRAVERSAL_COST else if (otherPN.G + tmpCost < pathNode.G && other.ContainsConnection(this)) #else else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this)) #endif { //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); } } } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); ushort pid = handler.PathID; { int[] neighbourOffsets = gg.neighbourOffsets; uint[] neighbourCosts = gg.neighbourCosts; GridNode[] nodes = gg.nodes; 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); uint tmpCost = neighbourCosts[i]; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used #if ASTAR_NO_TRAVERSAL_COST if (pathNode.G + tmpCost < otherPN.G) #else if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) #endif { //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 } #if ASTAR_NO_TRAVERSAL_COST else if (otherPN.G + tmpCost < pathNode.G) #else else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G) #endif { //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_NO_CUSTOM_CONNECTIONS base.Open(path, pathNode, handler); #endif }
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); } } } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { if (this.connections == null) { return; } bool flag = pathNode.flag2; for (int i = this.connections.Length - 1; i >= 0; i--) { GraphNode graphNode = this.connections[i]; if (path.CanTraverse(graphNode)) { PathNode pathNode2 = handler.GetPathNode(graphNode); if (pathNode2 != pathNode.parent) { uint num = this.connectionCosts[i]; if (flag || pathNode2.flag2) { num = path.GetConnectionSpecialCost(this, graphNode, num); } if (pathNode2.pathID != handler.PathID) { pathNode2.node = graphNode; pathNode2.parent = pathNode; pathNode2.pathID = handler.PathID; pathNode2.cost = num; pathNode2.H = path.CalculateHScore(graphNode); graphNode.UpdateG(path, pathNode2); handler.PushNode(pathNode2); } else if (pathNode.G + num + path.GetTraversalCost(graphNode) < pathNode2.G) { pathNode2.cost = num; pathNode2.parent = pathNode; graphNode.UpdateRecursiveG(path, pathNode2, handler); } else if (pathNode2.G + num + path.GetTraversalCost(this) < pathNode.G && graphNode.ContainsConnection(this)) { pathNode.parent = pathNode2; pathNode.cost = num; this.UpdateRecursiveG(path, pathNode, handler); } } } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { if (connections == null) return; // Flag2 indicates if this node needs special treatment // with regard to connection costs bool flag2 = pathNode.flag2; // Loop through all connections for (int i=connections.Length-1;i >= 0;i--) { GraphNode other = connections[i]; // Make sure we can traverse the neighbour if (path.CanTraverse (other)) { PathNode pathOther = handler.GetPathNode (other); // Fast path out, worth it for triangle mesh nodes since they usually have degree 2 or 3 if (pathOther == pathNode.parent) { continue; } uint cost = connectionCosts[i]; if (flag2 || pathOther.flag2) { // Get special connection cost from the path // This is used by the start and end nodes cost = path.GetConnectionSpecialCost (this,other,cost); } // Test if we have seen the other node before if (pathOther.pathID != handler.PathID) { // We have not seen the other node before // So the path from the start through this node to the other node // must be the shortest one so far // Might not be assigned pathOther.node = other; pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = cost; pathOther.H = path.CalculateHScore (other); other.UpdateG (path, pathOther); handler.PushNode (pathOther); } else { // If not we can test if the path from this node to the other one is a better one than the one already used if (pathNode.G + cost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = cost; pathOther.parent = pathNode; other.UpdateRecursiveG (path, pathOther,handler); } else if (pathOther.G+cost+path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection (this)) { // Or if the path from the other node to this one is better pathNode.parent = pathOther; pathNode.cost = cost; UpdateRecursiveG (path, pathNode,handler); } } } } }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); ushort pid = handler.PathID; { int[] neighbourOffsets = gg.neighbourOffsets; uint[] neighbourCosts = gg.neighbourCosts; GridNode[] nodes = gg.nodes; 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); // Multiply the connection cost with 1 + the average of the traversal costs for the two nodes uint tmpCost = (neighbourCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other))) / 128; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used if (pathNode.G + tmpCost < 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 < 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 (connections != null) { for (int i = 0; i < connections.Length; i++) { GraphNode other = connections[i]; if (!path.CanTraverse(other)) { continue; } PathNode otherPN = handler.GetPathNode(other); uint tmpCost = (connectionCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other))) / 128; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used if (pathNode.G + tmpCost < 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 < 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); } } } } }
/* 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) { var c = AstarColor.NodeConnection; var 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-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); 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; var 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-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; case GraphDebugMode.H: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.H-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; case GraphDebugMode.F: c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.F-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor)); break; } } c.a *= 0.5F; return c; }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { GridGraph gridGraph = GridNode.GetGridGraph(base.GraphIndex); ushort pathID = handler.PathID; int[] neighbourOffsets = gridGraph.neighbourOffsets; uint[] neighbourCosts = gridGraph.neighbourCosts; GridNode[] nodes = gridGraph.nodes; for (int i = 0; i < 8; i++) { int num = this.nodeInGridIndex + neighbourOffsets[i]; if (num >= 0 && num < nodes.Length) { GridNode gridNode = nodes[this.nodeInGridIndex + neighbourOffsets[i]]; if (path.CanTraverse(gridNode)) { PathNode pathNode2 = handler.GetPathNode(gridNode); uint num2 = neighbourCosts[i]; if (pathNode2.pathID != pathID) { pathNode2.parent = pathNode; pathNode2.pathID = pathID; pathNode2.cost = num2; pathNode2.H = path.CalculateHScore(gridNode); gridNode.UpdateG(path, pathNode2); handler.PushNode(pathNode2); } else if (pathNode.G + num2 + path.GetTraversalCost(gridNode) < pathNode2.G) { pathNode2.cost = num2; pathNode2.parent = pathNode; gridNode.UpdateRecursiveG(path, pathNode2, handler); } else if (pathNode2.G + num2 + path.GetTraversalCost(this) < pathNode.G) { pathNode.parent = pathNode2; pathNode.cost = num2; this.UpdateRecursiveG(path, pathNode, handler); } } } } if (this.connections != null) { for (int j = 0; j < this.connections.Length; j++) { GraphNode graphNode = this.connections[j]; if (path.CanTraverse(graphNode)) { PathNode pathNode3 = handler.GetPathNode(graphNode); uint num3 = this.connectionCosts[j]; if (pathNode3.pathID != pathID) { pathNode3.parent = pathNode; pathNode3.pathID = pathID; pathNode3.cost = num3; pathNode3.H = path.CalculateHScore(graphNode); graphNode.UpdateG(path, pathNode3); handler.PushNode(pathNode3); } else if (pathNode.G + num3 + path.GetTraversalCost(graphNode) < pathNode3.G) { pathNode3.cost = num3; pathNode3.parent = pathNode; graphNode.UpdateRecursiveG(path, pathNode3, handler); } else if (pathNode3.G + num3 + path.GetTraversalCost(this) < pathNode.G && graphNode.ContainsConnection(this)) { pathNode.parent = pathNode3; pathNode.cost = num3; this.UpdateRecursiveG(path, pathNode, handler); } } } } }
/** Executes a straight jump search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ static GridNode JPSJumpStraight (GridNode node, Path path, PathHandler handler, int parentDir, int depth = 0) { GridGraph gg = GetGridGraph(node.GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; GridNode origin = node; // Indexing into the cache arrays from multiple threads like this should cause // a lot of false sharing and cache trashing, but after profiling it seems // that this is not a major concern int threadID = handler.threadID; int threadOffset = 8*handler.threadID; int cyclicParentDir = JPSCyclic[parentDir]; GridNode result = null; // Rotate 180 degrees const int forwardDir = 4; int forwardOffset = neighbourOffsets[JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]]; // Move forwards in the same direction // until a node is encountered which we either // * know the result for (memoization) // * is a special node (flag2 set) // * has custom connections // * the node has a forced neighbour // Then break out of the loop // and start another loop which goes through the same nodes and sets the // memoization caches to avoid expensive calls in the future while (true) { // This is needed to make sure different threads don't overwrite each others results // It doesn't matter if we throw away some caching done by other threads as this will only // happen during the first few path requests if (node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount) { lock (node) { // Check again in case another thread has already created the array if (node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount) { node.JPSCache = new GridNode[8*handler.totalThreadCount]; node.JPSDead = new byte[handler.totalThreadCount]; node.JPSLastCacheID = new ushort[handler.totalThreadCount]; } } } if (node.JPSLastCacheID[threadID] != path.pathID) { for (int i = 0; i < 8; i++) node.JPSCache[i + threadOffset] = null; node.JPSLastCacheID[threadID] = path.pathID; node.JPSDead[threadID] = 0; } // Cache earlier results, major optimization // It is important to read from it once and then return the same result, // if we read from it twice, we might get different results due to other threads clearing the array sometimes GridNode cachedResult = node.JPSCache[parentDir + threadOffset]; if (cachedResult != null) { result = cachedResult; break; } if (((node.JPSDead[threadID] >> parentDir)&1) != 0) return null; // Special node (e.g end node), take care of if (handler.GetPathNode(node).flag2) { //Debug.Log ("Found end Node!"); //Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green); result = node; break; } #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS // Special node which has custom connections, take care of if (node.connections != null && node.connections.Length > 0) { result = node; break; } #endif // These are the nodes this node is connected to, one bit for each of the 8 directions int noncyclic = node.gridFlags;//We don't actually need to & with this because we don't use the other bits. & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) cyclic |= ((noncyclic >> i)&0x1) << JPSCyclic[i]; int forced = 0; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; //for ( int i = 0; i < 8; i++ ) if ( ((cyclic >> i)&1) == 0 ) forced |= JPSForced[i]; if ((cyclic & (1 << 2)) == 0) forced |= (1<<3); if ((cyclic & (1 << 6)) == 0) forced |= (1<<5); int natural = JPSNaturalStraightNeighbours; // Check if there are any forced neighbours which we can reach that are not natural neighbours //if ( ((forced & cyclic) & (~(natural & cyclic))) != 0 ) { if ((forced & (~natural) & cyclic) != 0) { // Some of the neighbour nodes are forced result = node; break; } // Make sure we can reach the next node if ((cyclic & (1 << forwardDir)) != 0) { node = nodes[node.nodeInGridIndex + forwardOffset]; //Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta); } else { result = null; break; } } if (result == null) { while (origin != node) { origin.JPSDead[threadID] |= (byte)(1 << parentDir); origin = nodes[origin.nodeInGridIndex + forwardOffset]; } } else { while (origin != node) { origin.JPSCache[parentDir + threadOffset] = result; origin = nodes[origin.nodeInGridIndex + forwardOffset]; } } return result; }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { if (connections == null) { return; } bool flag2 = pathNode.flag2; for (int i = connections.Length - 1; i >= 0; i--) { GraphNode other = connections[i]; if (path.CanTraverse(other)) { PathNode pathOther = handler.GetPathNode(other); //Fast path out, worth it for triangle mesh nodes since they usually have degree 2 or 3 if (pathOther == pathNode.parent) { continue; } uint cost = connectionCosts[i]; if (flag2 || pathOther.flag2) { cost = path.GetConnectionSpecialCost(this, other, cost); } if (pathOther.pathID != handler.PathID) { //Might not be assigned pathOther.node = other; pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = cost; pathOther.H = path.CalculateHScore(other); other.UpdateG(path, pathOther); handler.PushNode(pathOther); } else { //If not we can test if the path from this node to the other one is a better one than the one already used if (pathNode.G + cost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = cost; pathOther.parent = pathNode; other.UpdateRecursiveG(path, pathOther, handler); //handler.PushNode (pathOther); } else if (pathOther.G + cost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this)) { //Or if the path from the other node to this one is better pathNode.parent = pathOther; pathNode.cost = cost; UpdateRecursiveG(path, pathNode, handler); //handler.PushNode (pathNode); } } } } }
/** Opens a node using Jump Point Search. * \see http://en.wikipedia.org/wiki/Jump_point_search */ public void JPSOpen (Path path, PathNode pathNode, PathHandler handler) { GridGraph gg = GetGridGraph(GraphIndex); int[] neighbourOffsets = gg.neighbourOffsets; GridNode[] nodes = gg.nodes; ushort pid = handler.PathID; int noncyclic = gridFlags & 0xFF; int cyclic = 0; for (int i = 0; i < 8; i++) cyclic |= ((noncyclic >> i)&0x1) << JPSCyclic[i]; var parent = pathNode.parent != null ? pathNode.parent.node as GridNode : null; int parentDir = -1; if (parent != null) { int diff = parent != null ? parent.nodeInGridIndex - nodeInGridIndex : 0; int x2 = nodeInGridIndex % gg.width; int x1 = parent.nodeInGridIndex % gg.width; if (diff < 0) { if (x1 == x2) { parentDir = 0; } else if (x1 < x2) { parentDir = 7; } else { parentDir = 4; } } else { if (x1 == x2) { parentDir = 1; } else if (x1 < x2) { parentDir = 6; } else { parentDir = 5; } } } int cyclicParentDir = 0; // Check for -1 int forced = 0; if (parentDir != -1) { cyclicParentDir = JPSCyclic[parentDir]; // Loop around to be able to assume -X is where we came from cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF; } else { forced = 0xFF; //parentDir = 0; } bool diagonal = parentDir >= 4; int natural; if (diagonal) { for (int i = 0; i < 8; i++) if (((cyclic >> i)&1) == 0) forced |= JPSForcedDiagonal[i]; natural = JPSNaturalDiagonalNeighbours; } else { for (int i = 0; i < 8; i++) if (((cyclic >> i)&1) == 0) forced |= JPSForced[i]; natural = JPSNaturalStraightNeighbours; } // Don't force nodes we cannot reach anyway forced &= cyclic; natural &= cyclic; int nb = forced | natural; /*if ( ((Vector3)position - new Vector3(0.5f,0,3.5f)).magnitude < 0.5f ) { * Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir); * Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2)); * }*/ for (int i = 0; i < 8; i++) { if (((nb >> i)&1) != 0) { int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8]; GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; #if ASTARDEBUG if (((forced >> i)&1) != 0) { Debug.DrawLine((Vector3)position, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f), Color.red); } if (((natural >> i)&1) != 0) { Debug.DrawLine((Vector3)position + Vector3.up*0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) + Vector3.up*0.2f, Color.green); } #endif if (oi < 4) { other = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]); } else { other = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]); } if (other != null) { //Debug.DrawLine ( (Vector3)position + Vector3.up*0.0f, (Vector3)other.position + Vector3.up*0.3f, Color.cyan); //Debug.DrawRay ( (Vector3)other.position, Vector3.up, Color.cyan); //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 = (uint)(other.position - position).costMagnitude;//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 = (uint)(other.position - position).costMagnitude;//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 ASTARDEBUG if (i == 0 && parentDir != -1 && this.nodeInGridIndex > 10) { int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8]; if (nodeInGridIndex + neighbourOffsets[oi] < 0 || nodeInGridIndex + neighbourOffsets[oi] >= nodes.Length) { //Debug.LogError ("ERR: " + (nodeInGridIndex + neighbourOffsets[oi]) + " " + cyclicParentDir + " " + parentDir + " Reverted " + oi); //Debug.DrawRay ((Vector3)position, Vector3.up, Color.red); } else { GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]]; Debug.DrawLine((Vector3)position - Vector3.up*0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) - Vector3.up*0.2f, Color.blue); } } #endif } }
/* 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) { #if !PhotonImplementation 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 - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); 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 - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; case GraphDebugMode.H: c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, ((float)nodeR.H - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; case GraphDebugMode.F: c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, ((float)nodeR.F - AstarPath.active.debugFloor) / (AstarPath.active.debugRoof - AstarPath.active.debugFloor)); break; } } c.a *= 0.5F; return(c); #else return(new Color(1, 1, 1)); #endif }
public virtual void UpdateRecursiveG(Path path, PathNode pathNode, PathHandler handler) { //Simple but slow default implementation UpdateG (path,pathNode); handler.PushNode (pathNode); GetConnections (delegate (GraphNode other) { PathNode otherPN = handler.GetPathNode (other); if (otherPN.parent == pathNode && otherPN.pathID == handler.PathID) other.UpdateRecursiveG (path, otherPN,handler); }); }
public override void Open(Path path, PathNode pathNode, PathHandler handler) { LayerGridGraph gridGraph = LevelGridNode.GetGridGraph(base.GraphIndex); int[] neighbourOffsets = gridGraph.neighbourOffsets; uint[] neighbourCosts = gridGraph.neighbourCosts; LevelGridNode[] nodes = gridGraph.nodes; int nodeInGridIndex = base.NodeInGridIndex; for (int i = 0; i < 4; i++) { int connectionValue = this.GetConnectionValue(i); if (connectionValue != 255) { GraphNode graphNode = nodes[nodeInGridIndex + neighbourOffsets[i] + gridGraph.lastScannedWidth * gridGraph.lastScannedDepth * connectionValue]; if (path.CanTraverse(graphNode)) { PathNode pathNode2 = handler.GetPathNode(graphNode); if (pathNode2.pathID != handler.PathID) { pathNode2.parent = pathNode; pathNode2.pathID = handler.PathID; pathNode2.cost = neighbourCosts[i]; pathNode2.H = path.CalculateHScore(graphNode); graphNode.UpdateG(path, pathNode2); handler.PushNode(pathNode2); } else { uint num = neighbourCosts[i]; if (pathNode.G + num + path.GetTraversalCost(graphNode) < pathNode2.G) { pathNode2.cost = num; pathNode2.parent = pathNode; graphNode.UpdateRecursiveG(path, pathNode2, handler); } else if (pathNode2.G + num + path.GetTraversalCost(this) < pathNode.G) { pathNode.parent = pathNode2; pathNode.cost = num; this.UpdateRecursiveG(path, pathNode, handler); } } } } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { //BaseOpen (nodeRunData, nodeR, targetPosition, path); LayerGridGraph graph = GetGridGraph(GraphIndex); int[] neighbourOffsets = graph.neighbourOffsets; uint[] neighbourCosts = graph.neighbourCosts; LevelGridNode[] nodes = graph.nodes; int index = NodeInGridIndex;//indices & 0xFFFFFF; for (int i=0;i<4;i++) { int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF; if (conn != LevelGridNode.NoConnection) { GraphNode other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; if (!path.CanTraverse (other)) { continue; } PathNode otherPN = handler.GetPathNode (other); if (otherPN.pathID != handler.PathID) { otherPN.parent = pathNode; otherPN.pathID = handler.PathID; otherPN.cost = neighbourCosts[i]; otherPN.H = path.CalculateHScore (other); other.UpdateG (path, otherPN); handler.PushNode (otherPN); } 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 ASTAR_NO_TRAVERSAL_COST if (pathNode.G + tmpCost < otherPN.G) #else if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G) #endif { 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 #if ASTAR_NO_TRAVERSAL_COST else if (otherPN.G+tmpCost < pathNode.G) #else else if (otherPN.G+tmpCost+path.GetTraversalCost(this) < pathNode.G) #endif { pathNode.parent = otherPN; pathNode.cost = tmpCost; UpdateRecursiveG(path, pathNode, handler); } } } } }
public override void UpdateRecursiveG(Path path, PathNode pathNode, PathHandler handler) { handler.PushNode(pathNode); base.UpdateG(path, pathNode); LayerGridGraph gridGraph = LevelGridNode.GetGridGraph(base.GraphIndex); int[] neighbourOffsets = gridGraph.neighbourOffsets; LevelGridNode[] nodes = gridGraph.nodes; int nodeInGridIndex = base.NodeInGridIndex; for (int i = 0; i < 4; i++) { int connectionValue = this.GetConnectionValue(i); if (connectionValue != 255) { LevelGridNode levelGridNode = nodes[nodeInGridIndex + neighbourOffsets[i] + gridGraph.lastScannedWidth * gridGraph.lastScannedDepth * connectionValue]; PathNode pathNode2 = handler.GetPathNode(levelGridNode); if (pathNode2 != null && pathNode2.parent == pathNode && pathNode2.pathID == handler.PathID) { levelGridNode.UpdateRecursiveG(path, pathNode2, handler); } } } }
public override void UpdateRecursiveG(Path path, PathNode pathNode, PathHandler handler) { GridGraph gridGraph = GridNode.GetGridGraph(base.GraphIndex); int[] neighbourOffsets = gridGraph.neighbourOffsets; GridNode[] nodes = gridGraph.nodes; base.UpdateG(path, pathNode); handler.PushNode(pathNode); ushort pathID = handler.PathID; for (int i = 0; i < 8; i++) { if (this.GetConnectionInternal(i)) { GridNode gridNode = nodes[this.nodeInGridIndex + neighbourOffsets[i]]; PathNode pathNode2 = handler.GetPathNode(gridNode); if (pathNode2.parent == pathNode && pathNode2.pathID == pathID) { gridNode.UpdateRecursiveG(path, pathNode2, handler); } } } if (this.connections != null) { for (int j = 0; j < this.connections.Length; j++) { GraphNode graphNode = this.connections[j]; PathNode pathNode3 = handler.GetPathNode(graphNode); if (pathNode3.parent == pathNode && pathNode3.pathID == pathID) { graphNode.UpdateRecursiveG(path, pathNode3, handler); } } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { if (connections == null) return; bool flag2 = pathNode.flag2; for (int i=connections.Length-1;i >= 0;i--) { GraphNode other = connections[i]; if (path.CanTraverse (other)) { PathNode pathOther = handler.GetPathNode (other); //Fast path out, worth it for triangle mesh nodes since they usually have degree 2 or 3 if (pathOther == pathNode.parent) { continue; } uint cost = connectionCosts[i]; if (flag2 || pathOther.flag2) { cost = path.GetConnectionSpecialCost (this,other,cost); } if (pathOther.pathID != handler.PathID) { //Might not be assigned pathOther.node = other; pathOther.parent = pathNode; pathOther.pathID = handler.PathID; pathOther.cost = cost; pathOther.H = path.CalculateHScore (other); other.UpdateG (path, pathOther); handler.PushNode (pathOther); } else { //If not we can test if the path from this node to the other one is a better one than the one already used if (pathNode.G + cost + path.GetTraversalCost(other) < pathOther.G) { pathOther.cost = cost; pathOther.parent = pathNode; other.UpdateRecursiveG (path, pathOther,handler); //handler.PushNode (pathOther); } else if (pathOther.G+cost+path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection (this)) { //Or if the path from the other node to this one is better pathNode.parent = pathOther; pathNode.cost = cost; UpdateRecursiveG (path, pathNode,handler); //handler.PushNode (pathNode); } } } } }
public override void Open (Path path, PathNode pathNode, PathHandler handler) { var gg = GetGridGraph (GraphIndex); var pid = handler.PathID; { var neighbourOffsets = gg.neighbourOffsets; var neighbourCosts = gg.neighbourCosts; var nodes = gg.nodes; for (var i=0;i<8;i++) { if (GetConnectionInternal(i)) { var other = nodes[nodeInGridIndex + neighbourOffsets[i]]; if (!path.CanTraverse (other)) continue; var otherPN = handler.GetPathNode (other); // Multiply the connection cost with 1 + the average of the traversal costs for the two nodes var tmpCost = (neighbourCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other)))/128; if (otherPN.pathID != pid) { otherPN.parent = pathNode; otherPN.pathID = pid; otherPN.cost = tmpCost; 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 { // Sorry for the huge number of #ifs //If not we can test if the path from the current node to this one is a better one then the one already used if (pathNode.G+tmpCost < 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 < 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); } } } } } }
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) { UpdateG (path,pathNode); handler.PushNode (pathNode); if (connections == null) return; for (var i=0;i<connections.Length;i++) { var other = connections[i]; var otherPN = handler.GetPathNode (other); if (otherPN.parent == pathNode && otherPN.pathID == handler.PathID) other.UpdateRecursiveG (path, otherPN,handler); } }