public override Node[] CreateNodes (int number) {
/*if (nodes != null && graphNodes != null && nodes.Length == number && graphNodes.Length == number) {
Debug.Log ("Caching");
return nodes;
}*/
GridNode[] tmp = new GridNode[number];
for (int i=0;i<number;i++) {
tmp[i] = new GridNode ();
}
return tmp as Node[];
}
/** Calculates the grid connections for a single node.
* Convenience function, it's faster to use CalculateConnections(int,int,GridNode)
* 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(x, z, node);
}
}
/** Helper method to set PathNode.flag1 to a specific value for all nodes adjacent to a grid node */
void SetFlag1OnSurroundingGridNodes (GridNode gridNode, bool flag1State) {
// Loop through all adjacent grid nodes
var gridGraph = GridNode.GetGridGraph(gridNode.GraphIndex);
// Number of neighbours as an int
int mxnum = gridGraph.neighbours == NumNeighbours.Four ? 4 : (gridGraph.neighbours == NumNeighbours.Eight ? 8 : 6);
// Calculate the coordinates of the node
var x = gridNode.NodeInGridIndex % gridGraph.width;
var z = gridNode.NodeInGridIndex / gridGraph.width;
for (int i = 0; i < mxnum; i++) {
int nx, nz;
if (gridGraph.neighbours == NumNeighbours.Six) {
// Hexagon graph
nx = x + gridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[i]];
nz = z + gridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[i]];
} else {
nx = x + gridGraph.neighbourXOffsets[i];
nz = z + gridGraph.neighbourZOffsets[i];
}
// Check if the position is still inside the grid
if (nx >= 0 && nz >= 0 && nx < gridGraph.width && nz < gridGraph.depth) {
var adjacentNode = gridGraph.nodes[nz*gridGraph.width + nx];
pathHandler.GetPathNode(adjacentNode).flag1 = flag1State;
}
}
}
/** Reset all values to their default values.
* All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths.
* Call this base function in inheriting types with base.Reset ();
*/
public override void Reset () {
base.Reset ();
startNode = null;
endNode = null;
startHint = null;
endHint = null;
originalStartPoint = Vector3.zero;
originalEndPoint = Vector3.zero;
startPoint = Vector3.zero;
endPoint = Vector3.zero;
calculatePartial = false;
partialBestTarget = null;
startIntPoint = new Int3();
hTarget = new Int3();
endNodeCosts = null;
#if !ASTAR_NO_GRID_GRAPH
gridSpecialCaseNode = null;
#endif
}
/** 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);
}
}
// Add 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));
}
}
}
/** 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;
}
}
/** 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;
}
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);
}
}
public override bool GetPortal(GraphNode other, List <Vector3> left, List <Vector3> right, bool backwards)
{
if (backwards)
{
return(true);
}
GridGraph gg = GetGridGraph(GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
GridNode[] nodes = gg.nodes;
for (int i = 0; i < 4; i++)
{
if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]])
{
Vector3 middle = ((Vector3)(position + other.position)) * 0.5f;
Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position));
cross.Normalize();
cross *= gg.nodeSize * 0.5f;
left.Add(middle - cross);
right.Add(middle + cross);
return(true);
}
}
for (int i = 4; i < 8; i++)
{
if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]])
{
bool rClear = false;
bool lClear = false;
if (GetConnectionInternal(i - 4))
{
GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[i - 4]];
if (n2.Walkable && n2.GetConnectionInternal((i - 4 + 1) % 4))
{
rClear = true;
}
}
if (GetConnectionInternal((i - 4 + 1) % 4))
{
GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[(i - 4 + 1) % 4]];
if (n2.Walkable && n2.GetConnectionInternal(i - 4))
{
lClear = true;
}
}
Vector3 middle = ((Vector3)(position + other.position)) * 0.5f;
Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position));
cross.Normalize();
cross *= gg.nodeSize * 1.4142f;
left.Add(middle - (lClear ? cross : Vector3.zero));
right.Add(middle + (rClear ? cross : Vector3.zero));
return(true);
}
}
return(false);
}
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 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);
}
}
}
}
}
/** Returns randomly selected points on the specified nodes with each point being separated by \a clearanceRadius from each other.
* Selecting points ON the nodes only works for TriangleMeshNode (used by Recast Graph and Navmesh Graph) and GridNode (used by GridGraph).
* For other node types, only the positions of the nodes will be used.
*
* clearanceRadius will be reduced if no valid points can be found.
*/
public static List <Vector3> GetPointsOnNodes(List <GraphNode> nodes, int count, float clearanceRadius = 0)
{
if (nodes == null)
{
throw new System.ArgumentNullException("nodes");
}
if (nodes.Count == 0)
{
throw new System.ArgumentException("no nodes passed");
}
System.Random rnd = new System.Random();
List <Vector3> pts = Pathfinding.Util.ListPool <Vector3> .Claim(count);
// Square
clearanceRadius *= clearanceRadius;
if (nodes[0] is TriangleMeshNode || nodes[0] is GridNode)
{
//Assume all nodes are triangle nodes or grid nodes
List <float> accs = Pathfinding.Util.ListPool <float> .Claim(nodes.Count);
float tot = 0;
for (int i = 0; i < nodes.Count; i++)
{
TriangleMeshNode tnode = nodes[i] as TriangleMeshNode;
if (tnode != null)
{
float a = System.Math.Abs(Polygon.TriangleArea(tnode.GetVertex(0), tnode.GetVertex(1), tnode.GetVertex(2)));
tot += a;
accs.Add(tot);
}
#if !ASTAR_NO_GRID_GRAPH
else
{
GridNode gnode = nodes[i] as GridNode;
if (gnode != null)
{
GridGraph gg = GridNode.GetGridGraph(gnode.GraphIndex);
float a = gg.nodeSize * gg.nodeSize;
tot += a;
accs.Add(tot);
}
else
{
accs.Add(tot);
}
}
#endif
}
for (int i = 0; i < count; i++)
{
//Pick point
int testCount = 0;
int testLimit = 10;
bool worked = false;
while (!worked)
{
worked = true;
//If no valid points can be found, progressively lower the clearance radius until such a point is found
if (testCount >= testLimit)
{
clearanceRadius *= 0.8f;
testLimit += 10;
if (testLimit > 100)
{
clearanceRadius = 0;
}
}
float tg = (float)rnd.NextDouble() * tot;
int v = accs.BinarySearch(tg);
if (v < 0)
{
v = ~v;
}
if (v >= nodes.Count)
{
// This shouldn't happen, due to NextDouble being smaller than 1... but I don't trust floating point arithmetic.
worked = false;
continue;
}
TriangleMeshNode node = nodes[v] as TriangleMeshNode;
Vector3 p;
if (node != null)
{
// Find a random point inside the triangle
float v1;
float v2;
do
{
v1 = (float)rnd.NextDouble();
v2 = (float)rnd.NextDouble();
} while (v1 + v2 > 1);
p = ((Vector3)(node.GetVertex(1) - node.GetVertex(0))) * v1 + ((Vector3)(node.GetVertex(2) - node.GetVertex(0))) * v2 + (Vector3)node.GetVertex(0);
}
else
{
#if !ASTAR_NO_GRID_GRAPH
GridNode gnode = nodes[v] as GridNode;
if (gnode != null)
{
GridGraph gg = GridNode.GetGridGraph(gnode.GraphIndex);
float v1 = (float)rnd.NextDouble();
float v2 = (float)rnd.NextDouble();
p = (Vector3)gnode.position + new Vector3(v1 - 0.5f, 0, v2 - 0.5f) * gg.nodeSize;
}
else
#endif
{
//Point nodes have no area, so we break directly instead
pts.Add((Vector3)nodes[v].position);
break;
}
}
// Test if it is some distance away from the other points
if (clearanceRadius > 0)
{
for (int j = 0; j < pts.Count; j++)
{
if ((pts[j] - p).sqrMagnitude < clearanceRadius)
{
worked = false;
break;
}
}
}
if (worked)
{
pts.Add(p);
break;
}
else
{
testCount++;
}
}
}
Pathfinding.Util.ListPool <float> .Release(accs);
}
else
{
for (int i = 0; i < count; i++)
{
pts.Add((Vector3)nodes[rnd.Next(nodes.Count)].position);
}
}
return(pts);
}
protected virtual bool EndPointGridGraphSpecialCase(GraphNode closestWalkableEndNode)
{
GridNode gridNode = closestWalkableEndNode as GridNode;
if (gridNode != null)
{
GridGraph gridGraph = GridNode.GetGridGraph(gridNode.GraphIndex);
GridNode gridNode2 = AstarPath.active.GetNearest(this.originalEndPoint, NNConstraint.None).node as GridNode;
if (gridNode != gridNode2 && gridNode2 != null && gridNode.GraphIndex == gridNode2.GraphIndex)
{
int num = gridNode.NodeInGridIndex % gridGraph.width;
int num2 = gridNode.NodeInGridIndex / gridGraph.width;
int num3 = gridNode2.NodeInGridIndex % gridGraph.width;
int num4 = gridNode2.NodeInGridIndex / gridGraph.width;
bool flag = false;
switch (gridGraph.neighbours)
{
case NumNeighbours.Four:
if ((num == num3 && Math.Abs(num2 - num4) == 1) || (num2 == num4 && Math.Abs(num - num3) == 1))
{
flag = true;
}
break;
case NumNeighbours.Eight:
if (Math.Abs(num - num3) <= 1 && Math.Abs(num2 - num4) <= 1)
{
flag = true;
}
break;
case NumNeighbours.Six:
for (int i = 0; i < 6; i++)
{
int num5 = num3 + gridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[i]];
int num6 = num4 + gridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[i]];
if (num == num5 && num2 == num6)
{
flag = true;
break;
}
}
break;
default:
throw new Exception("Unhandled NumNeighbours");
}
if (flag)
{
this.SetFlagOnSurroundingGridNodes(gridNode2, 1, true);
this.endPoint = (Vector3)gridNode2.position;
this.hTarget = gridNode2.position;
this.endNode = gridNode2;
this.hTargetNode = this.endNode;
this.gridSpecialCaseNode = gridNode2;
return(true);
}
}
}
return(false);
}
public override float SurfaceArea()
{
GridGraph gg = GridNode.GetGridGraph(GraphIndex);
return(gg.nodeSize * gg.nodeSize);
}
/** Calculates the grid connections for a single node.
* The x and z parameters are assumed to be the grid coordinates of the node.
*
* \see CalculateConnections(GridNode)
*/
public virtual void CalculateConnections (int x, int z, GridNode node) {
// All connections are disabled if the node is not walkable
if (!node.Walkable) {
// Reset all connections
// This makes the node have NO connections to any neighbour nodes
node.ResetConnectionsInternal();
return;
}
// Internal index of where in the graph the node is
int index = node.NodeInGridIndex;
if (neighbours == NumNeighbours.Four || neighbours == NumNeighbours.Eight) {
// Bitpacked connections
// bit 0 is set if connection 0 is enabled
// bit 1 is set if connection 1 is enabled etc.
int conns = 0;
// Loop through axis aligned neighbours (down, right, up, left) or (-Z, +X, +Z, -X)
for (int i = 0; i < 4; i++) {
int nx = x + neighbourXOffsets[i];
int nz = z + neighbourZOffsets[i];
// Check if the new position is inside the grid
// Bitwise AND (&) is measurably faster than &&
// (not much, but this code is hot)
if (nx >= 0 & nz >= 0 & nx < width & nz < depth) {
var other = nodes[index+neighbourOffsets[i]];
if (IsValidConnection(node, other)) {
// Enable connection i
conns |= 1 << i;
}
}
}
// Bitpacked diagonal connections
int diagConns = 0;
// Add 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.
// Bitshifting is a lot faster than calling node.GetConnectionInternal.
// We need to check if connection i and i+1 are enabled
// but i+1 may overflow 4 and in that case need to be wrapped around
// (so 3+1 = 4 goes to 0). We do that by checking both connection i+1
// and i+1-4 at the same time. Either i+1 or i+1-4 will be in the range
// from 0 to 4 (exclusive)
if (((conns >> i | conns >> (i+1) | conns >> (i+1-4)) & 1) != 0) {
int directionIndex = i+4;
int nx = x + neighbourXOffsets[directionIndex];
int nz = z + neighbourZOffsets[directionIndex];
if (nx >= 0 & nz >= 0 & nx < width & nz < depth) {
GridNode other = nodes[index+neighbourOffsets[directionIndex]];
if (IsValidConnection(node, other)) {
diagConns |= 1 << directionIndex;
}
}
}
}
} 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 ((conns >> i & 1) != 0 && ((conns >> (i+1) | conns >> (i+1-4)) & 1) != 0) {
GridNode other = nodes[index+neighbourOffsets[i+4]];
if (IsValidConnection(node, other)) {
diagConns |= 1 << (i+4);
}
}
}
}
}
// Set all connections at the same time
node.SetAllConnectionInternal(conns | diagConns);
} else {
// Hexagon layout
// Reset all connections
// This makes the node have NO connections to any neighbour nodes
node.ResetConnectionsInternal();
// 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) {
var other = nodes[index+neighbourOffsets[i]];
node.SetConnectionInternal(i, IsValidConnection(node, other));
}
}
}
}
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);
}
}
/** 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 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;
}
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);
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);
}
}
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(x, z, node);
}
}
// Apply erosion
ErodeWalkableArea();
}
/** 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 static List <Vector3> GetPointsOnNodes(List <GraphNode> nodes, int count, float clearanceRadius = 0f)
{
if (nodes == null)
{
throw new ArgumentNullException("nodes");
}
if (nodes.Count == 0)
{
throw new ArgumentException("no nodes passed");
}
System.Random random = new System.Random();
List <Vector3> list = ListPool <Vector3> .Claim(count);
clearanceRadius *= clearanceRadius;
if (nodes[0] is TriangleMeshNode || nodes[0] is GridNode)
{
List <float> list2 = ListPool <float> .Claim(nodes.Count);
float num = 0f;
for (int i = 0; i < nodes.Count; i++)
{
TriangleMeshNode triangleMeshNode = nodes[i] as TriangleMeshNode;
if (triangleMeshNode != null)
{
float num2 = (float)Math.Abs(Polygon.TriangleArea2(triangleMeshNode.GetVertex(0), triangleMeshNode.GetVertex(1), triangleMeshNode.GetVertex(2)));
num += num2;
list2.Add(num);
}
else
{
GridNode gridNode = nodes[i] as GridNode;
if (gridNode != null)
{
GridGraph gridGraph = GridNode.GetGridGraph(gridNode.GraphIndex);
float num3 = gridGraph.nodeSize * gridGraph.nodeSize;
num += num3;
list2.Add(num);
}
else
{
list2.Add(num);
}
}
}
for (int j = 0; j < count; j++)
{
int num4 = 0;
int num5 = 10;
bool flag = false;
while (!flag)
{
flag = true;
if (num4 >= num5)
{
clearanceRadius *= 0.8f;
num5 += 10;
if (num5 > 100)
{
clearanceRadius = 0f;
}
}
float item = (float)random.NextDouble() * num;
int num6 = list2.BinarySearch(item);
if (num6 < 0)
{
num6 = ~num6;
}
if (num6 >= nodes.Count)
{
flag = false;
}
else
{
TriangleMeshNode triangleMeshNode2 = nodes[num6] as TriangleMeshNode;
Vector3 vector;
if (triangleMeshNode2 != null)
{
float num7;
float num8;
do
{
num7 = (float)random.NextDouble();
num8 = (float)random.NextDouble();
}while (num7 + num8 > 1f);
vector = (Vector3)(triangleMeshNode2.GetVertex(1) - triangleMeshNode2.GetVertex(0)) * num7 + (Vector3)(triangleMeshNode2.GetVertex(2) - triangleMeshNode2.GetVertex(0)) * num8 + (Vector3)triangleMeshNode2.GetVertex(0);
}
else
{
GridNode gridNode2 = nodes[num6] as GridNode;
if (gridNode2 == null)
{
list.Add((Vector3)nodes[num6].position);
break;
}
GridGraph gridGraph2 = GridNode.GetGridGraph(gridNode2.GraphIndex);
float num9 = (float)random.NextDouble();
float num10 = (float)random.NextDouble();
vector = (Vector3)gridNode2.position + new Vector3(num9 - 0.5f, 0f, num10 - 0.5f) * gridGraph2.nodeSize;
}
if (clearanceRadius > 0f)
{
for (int k = 0; k < list.Count; k++)
{
if ((list[k] - vector).sqrMagnitude < clearanceRadius)
{
flag = false;
break;
}
}
}
if (flag)
{
list.Add(vector);
break;
}
num4++;
}
}
}
ListPool <float> .Release(list2);
}
else
{
for (int l = 0; l < count; l++)
{
list.Add((Vector3)nodes[random.Next(nodes.Count)].position);
}
}
return(list);
}
/** 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
}
}
/** 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;
}
/** Applies a special case for grid nodes.
*
* Assume the closest walkable node is a grid node.
* We will now apply a special case only for grid graphs.
* In tile based games, an obstacle often occupies a whole
* node. When a path is requested to the position of an obstacle
* (single unwalkable node) the closest walkable node will be
* one of the 8 nodes surrounding that unwalkable node
* but that node is not neccessarily the one that is most
* optimal to walk to so in this special case
* we mark all nodes around the unwalkable node as targets
* and when we search and find any one of them we simply exit
* and set that first node we found to be the 'real' end node
* because that will be the optimal node (this does not apply
* in general unless the heuristic is set to None, but
* for a single unwalkable node it does).
* This also applies if the nearest node cannot be traversed for
* some other reason like restricted tags.
*
* \returns True if the workaround was applied. If this happens the
* endPoint, endNode, hTarget and hTargetNode fields will be modified.
*
* Image below shows paths when this special case is applied. The path goes from the white sphere to the orange box.
* \shadowimage{abpath_grid_special.gif}
*
* Image below shows paths when this special case has been disabled
* \shadowimage{abpath_grid_not_special.gif}
*/
protected virtual bool EndPointGridGraphSpecialCase(GraphNode closestWalkableEndNode)
{
var gridNode = closestWalkableEndNode as GridNode;
if (gridNode != null)
{
var gridGraph = GridNode.GetGridGraph(gridNode.GraphIndex);
// Find the closest node, not neccessarily walkable
var endNNInfo2 = AstarPath.active.GetNearest(originalEndPoint, NNConstraint.None);
var gridNode2 = endNNInfo2.node as GridNode;
if (gridNode != gridNode2 && gridNode2 != null && gridNode.GraphIndex == gridNode2.GraphIndex)
{
// Calculate the coordinates of the nodes
var x1 = gridNode.NodeInGridIndex % gridGraph.width;
var z1 = gridNode.NodeInGridIndex / gridGraph.width;
var x2 = gridNode2.NodeInGridIndex % gridGraph.width;
var z2 = gridNode2.NodeInGridIndex / gridGraph.width;
bool wasClose = false;
switch (gridGraph.neighbours)
{
case NumNeighbours.Four:
if ((x1 == x2 && System.Math.Abs(z1 - z2) == 1) || (z1 == z2 && System.Math.Abs(x1 - x2) == 1))
{
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x
// x O x
// x
wasClose = true;
}
break;
case NumNeighbours.Eight:
if (System.Math.Abs(x1 - x2) <= 1 && System.Math.Abs(z1 - z2) <= 1)
{
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x x x
// x O x
// x x x
wasClose = true;
}
break;
case NumNeighbours.Six:
// Hexagon graph
for (int i = 0; i < 6; i++)
{
var nx = x2 + gridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[i]];
var nz = z2 + gridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[i]];
if (x1 == nx && z1 == nz)
{
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x x
// x O x
// x x
wasClose = true;
break;
}
}
break;
default:
// Should not happen unless NumNeighbours is modified in the future
throw new System.Exception("Unhandled NumNeighbours");
}
if (wasClose)
{
// We now need to find all nodes marked with an x to be able to mark them as targets
SetFlagOnSurroundingGridNodes(gridNode2, 1, true);
// Note, other methods assume hTarget is (Int3)endPoint
endPoint = (Vector3)gridNode2.position;
hTarget = gridNode2.position;
endNode = gridNode2;
// hTargetNode is used for heuristic optimizations
// (also known as euclidean embedding).
// Even though the endNode is not walkable
// we can use it for better heuristics since
// there is a workaround added (EuclideanEmbedding.ApplyGridGraphEndpointSpecialCase)
// which is there to support this case.
hTargetNode = endNode;
// We need to save this node
// so that we can reset flag1 on all nodes later
gridSpecialCaseNode = gridNode2;
return(true);
}
}
}
return(false);
}
public override IEnumerable<Progress> ScanInternal () {
AstarPath.OnPostScan += new OnScanDelegate (OnPostScan);
if (nodeSize <= 0) {
yield break;
}
// 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");
yield break;
}
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);
yield return new Progress(0.05f, "Creating nodes");
// 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);
int progressCounter = 0;
const int YieldEveryNNodes = 1000;
for (int z = 0; z < depth; z ++) {
// Yield with a progress value at most every N nodes
if (progressCounter >= YieldEveryNNodes) {
progressCounter = 0;
yield return new Progress(Mathf.Lerp(0.1f,0.7f,z/(float)depth), "Calculating positions");
}
progressCounter += width;
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);
}
}
for (int z = 0; z < depth; z++) {
// Yield with a progress value at most every N nodes
if (progressCounter >= YieldEveryNNodes) {
progressCounter = 0;
yield return new Progress(Mathf.Lerp(0.1f,0.7f,z/(float)depth), "Calculating connections");
}
for (int x = 0; x < width; x++) {
var node = nodes[z*width + x];
// Recalculate connections to other nodes
CalculateConnections(x, z, node);
}
}
yield return new Progress(0.95f, "Calculating erosion");
// Apply erosion
ErodeWalkableArea ();
}
public override void ScanInternal (OnScanStatus statusCallback) {
AstarPath.OnPostScan += new OnScanDelegate (OnPostScan);
scans++;
if (nodeSize <= 0) {
return;
}
GenerateMatrix ();
if (width > 1024 || depth > 1024) {
Debug.LogError ("One of the grid's sides is longer than 1024 nodes");
return;
}
SetUpOffsetsAndCosts ();
var graphIndex = AstarPath.active.astarData.GetGraphIndex(this);
GridNode.SetGridGraph (graphIndex,this);
nodes = new GridNode[width*depth];
for (var i=0;i<nodes.Length;i++) {
nodes[i] = new GridNode(active);
nodes[i].GraphIndex = (uint)graphIndex;
}
if (collision == null) {
collision = new GraphCollision ();
}
collision.Initialize (matrix,nodeSize);
for (var z = 0; z < depth; z ++) {
for (var x = 0; x < width; x++) {
var node = nodes[z*width+x];
node.NodeInGridIndex = z*width+x;
UpdateNodePositionCollision (node,x,z);
}
}
for (var z = 0; z < depth; z ++) {
for (var x = 0; x < width; x++) {
var node = nodes[z*width+x];
CalculateConnections (nodes,x,z,node);
}
}
ErodeWalkableArea ();
}
/** Applies a special case for grid nodes.
*
* Assume the closest walkable node is a grid node.
* We will now apply a special case only for grid graphs.
* In tile based games, an obstacle often occupies a whole
* node. When a path is requested to the position of an obstacle
* (single unwalkable node) the closest walkable node will be
* one of the 8 nodes surrounding that unwalkable node
* but that node is not neccessarily the one that is most
* optimal to walk to so in this special case
* we mark all nodes around the unwalkable node as targets
* and when we search and find any one of them we simply exit
* and set that first node we found to be the 'real' end node
* because that will be the optimal node (this does not apply
* in general unless the heuristic is set to None, but
* for a single unwalkable node it does).
* This also applies if the nearest node cannot be traversed for
* some other reason like restricted tags.
*
* \returns True if the workaround was applied. If this happens the
* endPoint, endNode, hTarget and hTargetNode fields will be modified.
*
* Image below shows paths when this special case is applied. The path goes from the white sphere to the blue orange box.
* \shadowimage{abpath_grid_special.gif}
*
* Image below shows paths when this special case has been disabled
* \shadowimage{abpath_grid_not_special.gif}
*/
protected virtual bool EndPointGridGraphSpecialCase (GraphNode closestWalkableEndNode) {
var gridNode = closestWalkableEndNode as GridNode;
if (gridNode != null) {
var gridGraph = GridNode.GetGridGraph(gridNode.GraphIndex);
// Find the closest node, not neccessarily walkable
NNInfo endNNInfo2 = AstarPath.active.GetNearest(originalEndPoint, NNConstraint.None, endHint);
var gridNode2 = endNNInfo2.node as GridNode;
if (gridNode != gridNode2 && gridNode2 != null && gridNode.GraphIndex == gridNode2.GraphIndex) {
// Calculate the coordinates of the nodes
var x1 = gridNode.NodeInGridIndex % gridGraph.width;
var z1 = gridNode.NodeInGridIndex / gridGraph.width;
var x2 = gridNode2.NodeInGridIndex % gridGraph.width;
var z2 = gridNode2.NodeInGridIndex / gridGraph.width;
bool wasClose = false;
switch (gridGraph.neighbours) {
case NumNeighbours.Four:
if ((x1 == x2 && System.Math.Abs(z1-z2) == 1) || (z1 == z2 && System.Math.Abs(x1-x2) == 1)) {
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x
// x O x
// x
wasClose = true;
}
break;
case NumNeighbours.Eight:
if (System.Math.Abs(x1-x2) <= 1 && System.Math.Abs(z1-z2) <= 1) {
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x x x
// x O x
// x x x
wasClose = true;
}
break;
case NumNeighbours.Six:
// Hexagon graph
for (int i = 0; i < 6; i++) {
var nx = x2 + gridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[i]];
var nz = z2 + gridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[i]];
if (x1 == nx && z1 == nz) {
// If 'O' is gridNode2, then gridNode is one of the nodes marked with an 'x'
// x x
// x O x
// x x
wasClose = true;
break;
}
}
break;
default:
// Should not happen unless NumNeighbours is modified in the future
throw new System.Exception("Unhandled NumNeighbours");
}
if (wasClose) {
// We now need to find all nodes marked with an x to be able to mark them as targets
SetFlag1OnSurroundingGridNodes(gridNode2, true);
// Note, other methods assume hTarget is (Int3)endPoint
endPoint = (Vector3)gridNode2.position;
hTarget = gridNode2.position;
endNode = gridNode2;
// hTargetNode is used for heuristic optimizations
// (also known as euclidean embedding).
// Even though the endNode is not walkable
// we can use it for better heuristics since
// there is a workaround added (EuclideanEmbedding.ApplyGridGraphEndpointSpecialCase)
// which is there to support this case.
hTargetNode = endNode;
// We need to save this node
// so that we can reset flag1 on all nodes later
gridSpecialCaseNode = gridNode2;
return true;
}
}
}
return false;
}
/** 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) {
node.position = GetNodePosition ( node.NodeInGridIndex, 0 );//0;// = (Int3)matrix.MultiplyPoint3x4 (new Vector3 (x+0.5F,0,z+0.5F));
RaycastHit hit;
var walkable = true;
var position = collision.CheckHeight ((Vector3)node.position, out hit, out walkable);
node.position = (Int3)position;
if (resetPenalty)
node.Penalty = initialPenalty;
if (penaltyPosition && resetPenalty) {
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)
var angle = Vector3.Dot (hit.normal.normalized,collision.up);
if ( Mathf.Abs ( 1 - collision.up.magnitude ) > 0.1f ) {
Debug.Log ("HI");
}
//Add penalty based on normal
if (penaltyAngle && resetPenalty) {
node.Penalty += (uint)Mathf.RoundToInt ((1F-Mathf.Pow(angle, penaltyAnglePower))*penaltyAngleFactor);
}
//Max slope in cosinus
var cosAngle = Mathf.Cos (maxSlope*Mathf.Deg2Rad);
//Check if the slope 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
if (walkable)
node.Walkable = collision.Check ((Vector3)node.position);
else
node.Walkable = walkable;
node.WalkableErosion = node.Walkable;
}
/** 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).
*
* This is used for calculating the connections when the graph is scanned or updated.
*
* \see CalculateConnections
*/
public virtual bool IsValidConnection (GridNode n1, GridNode n2) {
if (!n1.Walkable || !n2.Walkable) {
return false;
}
return maxClimb <= 0 || System.Math.Abs(n1.position[maxClimbAxis] - n2.position[maxClimbAxis]) <= maxClimb*Int3.Precision;
}
/** 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
var index = node.NodeInGridIndex;
if (corners == null) {
corners = new int[4];
} else {
for (var 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++) {
var nx = x + neighbourXOffsets[i];
var nz = z + neighbourZOffsets[i];
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) {
continue;
}
var other = nodes[index+neighbourOffsets[i]] as GridNode;
if (IsValidConnection (node, other)) {
node.SetConnectionInternal (i, true);
corners[i]++;
corners[j]++;
} else {
node.SetConnectionInternal (i, false);
}
}
// Add in the diagonal connections
if (neighbours == NumNeighbours.Eight) {
if (cutCorners) {
for (var i=0; i<4; i++) {
if (corners[i] >= 1) {
var nx = x + neighbourXOffsets[i+4];
var nz = z + neighbourZOffsets[i+4];
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) {
continue;
}
var other = nodes[index+neighbourOffsets[i+4]];
node.SetConnectionInternal (i+4, IsValidConnection (node,other));
}
}
} else {
for (var i=0; i<4; i++) {
//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) {
var other = nodes[index+neighbourOffsets[i+4]];
node.SetConnectionInternal (i+4, IsValidConnection (node,other));
}
}
}
}
}
public virtual void CalculateConnections (GridNode[] nodes, int x, int z, GridNode node) {
CalculateConnections(x, z, node);
}
public void AddPortal (GridNode n1, GridNode n2, List<Vector3> left, List<Vector3> right) {
if (n1 == n2) {
return;
}
int i1 = n1.GetIndex ();
int i2 = n2.GetIndex ();
int x1 = i1 % width;
int x2 = i2 % width;
int z1 = i1 / width;
int z2 = i2 / width;
Vector3 n1p = n1.position;
Vector3 n2p = n2.position;
int diffx = Mathf.Abs (x1-x2);
int diffz = Mathf.Abs (z1-z2);
if (diffx > 1 || diffz > 1) {
//If the nodes are not adjacent to each other
left.Add (n1p);
right.Add (n1p);
left.Add (n2p);
right.Add (n2p);
} else if ((diffx+diffz) <= 1){
//If it is not a diagonal move
Vector3 dir = n2p - n1p;
dir = dir.normalized * nodeSize * 0.5F;
Vector3 tangent = Vector3.Cross (dir, Vector3.up);
tangent = tangent.normalized * nodeSize * 0.5F;
left.Add (n1p + dir - tangent);
right.Add (n1p + dir + tangent);
} else {
//Diagonal move
Node t1 = nodes[z1 * width + x2];
Node t2 = nodes[z2 * width + x1];
Node target = null;
if (t1.walkable) {
target = t1;
} else if (t2.walkable) {
target = t2;
}
if (target == null) {
Vector3 avg = (n1p + n2p) * 0.5F;
left.Add (avg);
right.Add (avg);
} else {
AddPortal (n1,(GridNode)target,left,right);
AddPortal ((GridNode)target,n2,left,right);
}
}
}
/** 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;
}
}
//END IFunnelGraph Implementation
public bool CheckConnection (GridNode node, int dir) {
if (neighbours == NumNeighbours.Eight) {
return node.GetConnection (dir);
} else {
int dir1 = (dir-4-1) & 0x3;
int dir2 = (dir-4+1) & 0x3;
if (!node.GetConnection (dir1) || !node.GetConnection (dir2)) {
return false;
} else {
GridNode n1 = nodes[node.GetIndex ()+neighbourOffsets[dir1]] as GridNode;
GridNode n2 = nodes[node.GetIndex ()+neighbourOffsets[dir2]] as GridNode;
if (!n1.walkable || !n2.walkable) {
return false;
}
if (!n2.GetConnection (dir1) || !n1.GetConnection (dir2)) {
return false;
}
}
return true;
}
}
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);
}
}
/** 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) {
GridGraph gg = AstarData.GetGraph (node) as GridGraph;
if (gg != null) {
int index = node.GetIndex ();
int x = index % gg.width;
int z = index / gg.width;
gg.CalculateConnections (gg.graphNodes,x,z,node);
}
}
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);
}
/** Calculates the grid connections for a single node */
public virtual void CalculateConnections (GridNode[] graphNodes, int x, int z, GridNode node) {
//Reset all connections
node.flags = node.flags & -256;
//All connections are disabled if the node is not walkable
if (!node.walkable) {
return;
}
int index = node.GetIndex ();
if (corners == null) {
corners = new int[4];
} else {
for (int i = 0;i<4;i++) {
corners[i] = 0;
}
}
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;
}
GridNode other = graphNodes[index+neighbourOffsets[i]];
if (IsValidConnection (node, other)) {
node.SetConnectionRaw (i,1);
corners[i]++;
corners[j]++;
}
}
if (neighbours == NumNeighbours.Eight) {
if (cutCorners) {
for (int i=0; i<4; i++) {
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 = graphNodes[index+neighbourOffsets[i+4]];
if (IsValidConnection (node,other)) {
node.SetConnectionRaw (i+4,1);
}
}
}
} else {
for (int i=0; i<4; i++) {
//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 = graphNodes[index+neighbourOffsets[i+4]];
if (IsValidConnection (node,other)) {
node.SetConnectionRaw (i+4,1);
}
}
}
}
}
}
/** 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);
// Add 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;
}
/** 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]);
}
}
}
/** 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;
}
private void ApplyGridGraphEndpointSpecialCase()
{
NavGraph[] graphs = AstarPath.active.graphs;
for (int i = 0; i < graphs.Length; i++)
{
GridGraph gridGraph = graphs[i] as GridGraph;
if (gridGraph != null)
{
GridNode[] nodes = gridGraph.nodes;
int num = (gridGraph.neighbours != NumNeighbours.Four) ? ((gridGraph.neighbours != NumNeighbours.Eight) ? 6 : 8) : 4;
for (int j = 0; j < gridGraph.depth; j++)
{
for (int k = 0; k < gridGraph.width; k++)
{
GridNode gridNode = nodes[j * gridGraph.width + k];
if (!gridNode.Walkable)
{
int num2 = gridNode.NodeIndex * this.pivotCount;
for (int l = 0; l < this.pivotCount; l++)
{
this.costs[num2 + l] = uint.MaxValue;
}
for (int m = 0; m < num; m++)
{
int num3;
int num4;
if (gridGraph.neighbours == NumNeighbours.Six)
{
num3 = k + gridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[m]];
num4 = j + gridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[m]];
}
else
{
num3 = k + gridGraph.neighbourXOffsets[m];
num4 = j + gridGraph.neighbourZOffsets[m];
}
if (num3 >= 0 && num4 >= 0 && num3 < gridGraph.width && num4 < gridGraph.depth)
{
GridNode gridNode2 = gridGraph.nodes[num4 * gridGraph.width + num3];
if (gridNode2.Walkable)
{
for (int n = 0; n < this.pivotCount; n++)
{
uint val = this.costs[gridNode2.NodeIndex * this.pivotCount + n] + gridGraph.neighbourCosts[m];
this.costs[num2 + n] = Math.Min(this.costs[num2 + n], val);
Debug.DrawLine((Vector3)gridNode.position, (Vector3)gridNode2.position, Color.blue, 1f);
}
}
}
}
for (int num5 = 0; num5 < this.pivotCount; num5++)
{
if (this.costs[num2 + num5] == 4294967295u)
{
this.costs[num2 + num5] = 0u;
}
}
}
}
}
}
}
}
