/** Updates position, walkability and penalty for the node. * Assumes that collision.Initialize (...) has been called before this function */ public virtual void UpdateNodePositionCollision (GridNode node, int x, int z, bool resetPenalty = true) { // Set the node's initial position with a y-offset of zero node.position = GraphPointToWorld (x, z, 0); RaycastHit hit; bool walkable; // Calculate the actual position using physics raycasting (if enabled) // walkable will be set to false if no ground was found (unless that setting has been disabled) Vector3 position = collision.CheckHeight ((Vector3)node.position, out hit, out walkable); node.position = (Int3)position; if (resetPenalty) { node.Penalty = initialPenalty; // Calculate a penalty based on the y coordinate of the node if (penaltyPosition) { node.Penalty += (uint)Mathf.RoundToInt ((node.position.y-penaltyPositionOffset)*penaltyPositionFactor); } } // Check if the node is on a slope steeper than permitted if (walkable && useRaycastNormal && collision.heightCheck) { if (hit.normal != Vector3.zero) { // Take the dot product to find out the cosinus of the angle it has (faster than Vector3.Angle) float angle = Vector3.Dot (hit.normal.normalized,collision.up); // Enqueue penalty based on normal if (penaltyAngle && resetPenalty) { node.Penalty += (uint)Mathf.RoundToInt ((1F-Mathf.Pow(angle, penaltyAnglePower))*penaltyAngleFactor); } // Cosinus of the max slope float cosAngle = Mathf.Cos (maxSlope*Mathf.Deg2Rad); // Check if the ground is flat enough to stand on if (angle < cosAngle) { walkable = false; } } } // If the walkable flag has already been set to false, there is no point in checking for it again // Check for obstacles node.Walkable = walkable && collision.Check ((Vector3)node.position); // Store walkability before erosion is applied // Used for graph updating node.WalkableErosion = node.Walkable; }
public void SetNodeConnection (GridNode node, int dir, bool value) { int index = node.NodeInGridIndex; int z = index/Width; int x = index - z*Width; SetNodeConnection (index, x, z, dir, value); }
public override void ScanInternal (OnScanStatus statusCallback) { AstarPath.OnPostScan += new OnScanDelegate (OnPostScan); if (nodeSize <= 0) { return; } // Make sure the matrix is up to date GenerateMatrix (); if (width > 1024 || depth > 1024) { Debug.LogError ("One of the grid's sides is longer than 1024 nodes"); return; } #if !ASTAR_JPS if (this.useJumpPointSearch) { Debug.LogError ("Trying to use Jump Point Search, but support for it is not enabled. Please enable it in the inspector (Grid Graph settings)."); } #endif SetUpOffsetsAndCosts (); // Get the graph index of this graph int graphIndex = AstarPath.active.astarData.GetGraphIndex(this); // Set a global reference to this graph so that nodes can find it GridNode.SetGridGraph (graphIndex,this); // Create all nodes nodes = new GridNode[width*depth]; for (int i=0;i<nodes.Length;i++) { nodes[i] = new GridNode(active); nodes[i].GraphIndex = (uint)graphIndex; } // Create and initialize the collision class if (collision == null) { collision = new GraphCollision (); } collision.Initialize (matrix,nodeSize); textureData.Initialize (); for (int z = 0; z < depth; z ++) { for (int x = 0; x < width; x++) { var node = nodes[z*width+x]; node.NodeInGridIndex = z*width+x; // Updates the position of the node // and a bunch of other things UpdateNodePositionCollision (node,x,z); // Apply texture data if necessary textureData.Apply (node,x,z); } } for (int z = 0; z < depth; z ++) { for (int x = 0; x < width; x++) { var node = nodes[z*width+x]; // Recalculate connections to other nodes CalculateConnections (nodes,x,z,node); } } // Apply erosion ErodeWalkableArea (); }
public GridNode GetNodeConnection (GridNode node, int dir) { if (!node.GetConnectionInternal(dir)) return null; if (!node.EdgeNode) { return nodes[node.NodeInGridIndex + neighbourOffsets[dir]]; } else { int index = node.NodeInGridIndex; //int z = Math.DivRem (index,Width, out x); int z = index/Width; int x = index - z*Width; return GetNodeConnection (index, x, z, dir); } }
public bool HasNodeConnection (GridNode node, int dir) { if (!node.GetConnectionInternal(dir)) return false; if (!node.EdgeNode) { return true; } else { int index = node.NodeInGridIndex; int z = index/Width; int x = index - z*Width; return HasNodeConnection (index, x, z, dir); } }
/** Applies the texture to the node */ public void Apply (GridNode node, int x, int z) { if (enabled && data != null && x < source.width && z < source.height) { Color32 col = data[z*source.width+x]; if (channels[0] != ChannelUse.None) { ApplyChannel (node,x,z,col.r,channels[0],factors[0]); } if (channels[1] != ChannelUse.None) { ApplyChannel (node,x,z,col.g,channels[1],factors[1]); } if (channels[2] != ChannelUse.None) { ApplyChannel (node,x,z,col.b,channels[2],factors[2]); } } }
/** Applies a value to the node using the specified ChannelUse */ void ApplyChannel (GridNode node, int x, int z, int value, ChannelUse channelUse, float factor) { switch (channelUse) { case ChannelUse.Penalty: node.Penalty += (uint)Mathf.RoundToInt (value*factor); break; case ChannelUse.Position: node.position = GridNode.GetGridGraph(node.GraphIndex).GraphPointToWorld (x, z, value); break; case ChannelUse.WalkablePenalty: if (value == 0) { node.Walkable = false; } else { node.Penalty += (uint)Mathf.RoundToInt ((value-1)*factor); } break; } }
/** Returns if \a node is connected to it's neighbour in the specified direction. * This will also return true if #neighbours = NumNeighbours.Four, the direction is diagonal and one can move through one of the adjacent nodes * to the targeted node. * * \see neighbourOffsets */ public bool CheckConnection (GridNode node, int dir) { if (neighbours == NumNeighbours.Eight || neighbours == NumNeighbours.Six || dir < 4) { return HasNodeConnection (node, dir); } else { int dir1 = (dir-4-1) & 0x3; int dir2 = (dir-4+1) & 0x3; if (!HasNodeConnection (node, dir1) || !HasNodeConnection (node, dir2)) { return false; } else { GridNode n1 = nodes[node.NodeInGridIndex+neighbourOffsets[dir1]]; GridNode n2 = nodes[node.NodeInGridIndex+neighbourOffsets[dir2]]; if (!n1.Walkable || !n2.Walkable) { return false; } if (!HasNodeConnection (n2, dir1) || !HasNodeConnection (n1, dir2)) { return false; } } return true; } }
public override void DeserializeExtraInfo (GraphSerializationContext ctx) { int count = ctx.reader.ReadInt32(); if (count == -1) { nodes = null; return; } nodes = new GridNode[count]; for (int i=0;i<nodes.Length;i++) { nodes[i] = new GridNode (active); nodes[i].DeserializeNode(ctx); } }
/** Calculates the grid connections for a single node */ public virtual void CalculateConnections (GridNode[] nodes, int x, int z, GridNode node) { //Reset all connections // This makes the node have NO connections to any neighbour nodes node.ResetConnectionsInternal (); //All connections are disabled if the node is not walkable if (!node.Walkable) { return; } // Internal index of where in the graph the node is int index = node.NodeInGridIndex; if (neighbours == NumNeighbours.Four || neighbours == NumNeighbours.Eight) { // Reset the buffer if (corners == null) { corners = new int[4]; } else { for (int i = 0;i<4;i++) { corners[i] = 0; } } // Loop through axis aligned neighbours (up, down, right, left) for (int i=0, j = 3; i<4; j = i, i++) { int nx = x + neighbourXOffsets[i]; int nz = z + neighbourZOffsets[i]; if (nx < 0 || nz < 0 || nx >= width || nz >= depth) { continue; } var other = nodes[index+neighbourOffsets[i]]; if (IsValidConnection (node, other)) { node.SetConnectionInternal (i, true); // Mark the diagonal/corner adjacent to this connection as used corners[i]++; corners[j]++; } else { node.SetConnectionInternal (i, false); } } // Enqueue in the diagonal connections if (neighbours == NumNeighbours.Eight) { if (cutCorners) { for (int i=0; i<4; i++) { // If at least one axis aligned connection // is adjacent to this diagonal, then we can add a connection if (corners[i] >= 1) { int nx = x + neighbourXOffsets[i+4]; int nz = z + neighbourZOffsets[i+4]; if (nx < 0 || nz < 0 || nx >= width || nz >= depth) { continue; } GridNode other = nodes[index+neighbourOffsets[i+4]]; node.SetConnectionInternal (i+4, IsValidConnection (node,other)); } } } else { for (int i=0; i<4; i++) { // If exactly 2 axis aligned connections is adjacent to this connection // then we can add the connection //We don't need to check if it is out of bounds because if both of the other neighbours are inside the bounds this one must be too if (corners[i] == 2) { GridNode other = nodes[index+neighbourOffsets[i+4]]; node.SetConnectionInternal (i+4, IsValidConnection (node,other)); } } } } } else { // Hexagon layout // Loop through all possible neighbours and try to connect to them for (int j = 0; j < hexagonNeighbourIndices.Length; j++) { var i = hexagonNeighbourIndices[j]; int nx = x + neighbourXOffsets[i]; int nz = z + neighbourZOffsets[i]; if (nx < 0 || nz < 0 || nx >= width || nz >= depth) { continue; } var other = nodes[index+neighbourOffsets[i]]; node.SetConnectionInternal (i, IsValidConnection (node, other)); } } }
/** Calculates the grid connections for a single node. * Convenience function, it's faster to use CalculateConnections(GridNode[],int,int,node) * but that will only show when calculating for a large number of nodes. * \todo Test this function, should work ok, but you never know */ public static void CalculateConnections (GridNode node) { var gg = AstarData.GetGraph (node) as GridGraph; if (gg != null) { int index = node.NodeInGridIndex; int x = index % gg.width; int z = index / gg.width; gg.CalculateConnections (gg.nodes,x,z,node); } }
/** Returns true if a connection between the adjacent nodes \a n1 and \a n2 is valid. * Also takes into account if the nodes are walkable * * This method may be overriden if you want to customize what connections are valid. * It must however hold that IsValidConnection(a,b) == IsValidConnection(b,a) */ public virtual bool IsValidConnection (GridNode n1, GridNode n2) { if (!n1.Walkable || !n2.Walkable) { return false; } if (maxClimb > 0 && Mathf.Abs (n1.position[maxClimbAxis] - n2.position[maxClimbAxis]) > maxClimb*Int3.Precision) { return false; } return true; }
/** True if the node has any blocked connections. * For 4 and 8 neighbours the 4 axis aligned connections will be checked. * For 6 neighbours all 6 neighbours will be checked. */ bool ErosionAnyFalseConnections ( GridNode node ) { if (neighbours == NumNeighbours.Six) { // Check the 6 hexagonal connections for (int i=0;i<6;i++) { if (!HasNodeConnection (node,hexagonNeighbourIndices[i])) { return true; } } } else { // Check the four axis aligned connections for (int i=0;i<4;i++) { if (!HasNodeConnection (node,i)) { return true; } } } return false; }
/** 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; }