/** 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;
}
