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