public PathThreadInfo(int index, AstarPath astar, PathHandler runData)
{
this.threadIndex = index;
this.astar = astar;
this.runData = runData;
this.lockObject = new object();
}
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);
}
}
}
static int PrepareBase(IntPtr L)
{
try
{
ToLua.CheckArgsCount(L, 2);
Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1);
Pathfinding.PathHandler arg0 = (Pathfinding.PathHandler)ToLua.CheckObject <Pathfinding.PathHandler>(L, 2);
obj.PrepareBase(arg0);
return(0);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e));
}
}
static int get_pathHandler(IntPtr L)
{
object o = null;
try
{
o = ToLua.ToObject(L, 1);
Pathfinding.Path obj = (Pathfinding.Path)o;
Pathfinding.PathHandler ret = obj.pathHandler;
ToLua.PushObject(L, ret);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e, o, "attempt to index pathHandler on a nil value"));
}
}
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 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);
}
}
}
/** Reset all values to their default values.
*
* \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
* override this function, resetting ALL their variables to enable recycling of paths.
* If this is not done, trying to use that path type for pooling might result in weird behaviour.
* The best way is to reset to default values the variables declared in the extended path type and then
* call this base function in inheriting types with base.Reset ().
*
* \warning This function should not be called manually.
*/
public virtual void Reset () {
if (ReferenceEquals (AstarPath.active, null))
throw new NullReferenceException ("No AstarPath object found in the scene. " +
"Make sure there is one or do not create paths in Awake");
hasBeenReset = true;
state = (int)PathState.Created;
releasedNotSilent = false;
pathHandler = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = ListPool<GraphNode>.Claim();
vectorPath = ListPool<Vector3>.Claim();
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
//calltime
nnConstraint = PathNNConstraint.Default;
next = null;
radius = 0;
walkabilityMask = -1;
height = 0;
turnRadius = 0;
speed = 0;
//heuristic = (Heuristic)0;
//heuristicScale = 1F;
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
pathID = 0;
enabledTags = -1;
tagPenalties = null;
callTime = DateTime.UtcNow;
pathID = AstarPath.active.GetNextPathID ();
hTarget = Int3.zero;
hTargetNode = null;
}
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)
{
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 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);
});
}
/** 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 void PrepareBase(PathHandler pathHandler)
{
if (pathHandler.PathID > this.pathID)
{
pathHandler.ClearPathIDs();
}
this.pathHandler = pathHandler;
pathHandler.InitializeForPath(this);
if (this.internalTagPenalties == null || this.internalTagPenalties.Length != 32)
{
this.internalTagPenalties = Path.ZeroTagPenalties;
}
try
{
this.ErrorCheck();
}
catch (Exception ex)
{
this.ForceLogError(string.Concat(new object[]
{
"Exception in path ",
this.pathID,
"\n",
ex
}));
}
}
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++)
{
var node = 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 = AstarColor.UnwalkableNode;
}
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]);
}
}
/// <summary>
/// Main pathfinding method (multithreaded).
/// This method will calculate the paths in the pathfinding queue when multithreading is enabled.
///
/// See: CalculatePaths
/// See: StartPath
/// </summary>
void CalculatePathsThreaded(PathHandler pathHandler)
{
#if UNITY_2017_3_OR_NEWER
UnityEngine.Profiling.Profiler.BeginThreadProfiling("Pathfinding", "Pathfinding thread #" + (pathHandler.threadID + 1));
#endif
#if !ASTAR_FAST_BUT_NO_EXCEPTIONS
try {
#endif
// Max number of ticks we are allowed to continue working in one run.
// One tick is 1/10000 of a millisecond.
// We need to check once in a while if the thread should be stopped.
long maxTicks = (long)(10 * 10000);
long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
while (true)
{
// The path we are currently calculating
Path path = queue.Pop();
#if UNITY_2017_3_OR_NEWER
profilingSampler.Begin();
#endif
// Access the internal implementation methods
IPathInternals ipath = (IPathInternals)path;
AstarProfiler.StartFastProfile(0);
ipath.PrepareBase(pathHandler);
// Now processing the path
// Will advance to Processing
ipath.AdvanceState(PathState.Processing);
// Call some callbacks
if (OnPathPreSearch != null)
{
OnPathPreSearch(path);
}
// Tick for when the path started, used for calculating how long time the calculation took
long startTicks = System.DateTime.UtcNow.Ticks;
// Prepare the path
ipath.Prepare();
AstarProfiler.EndFastProfile(0);
if (path.CompleteState == PathCompleteState.NotCalculated)
{
// For visualization purposes, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
astar.debugPathData = ipath.PathHandler;
astar.debugPathID = path.pathID;
AstarProfiler.StartFastProfile(1);
// Initialize the path, now ready to begin search
ipath.Initialize();
AstarProfiler.EndFastProfile(1);
// Loop while the path has not been fully calculated
while (path.CompleteState == PathCompleteState.NotCalculated)
{
// Do some work on the path calculation.
// The function will return when it has taken too much time
// or when it has finished calculation
AstarProfiler.StartFastProfile(2);
ipath.CalculateStep(targetTick);
AstarProfiler.EndFastProfile(2);
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
// Cancel function (and thus the thread) if no more paths should be accepted.
// This is done when the A* object is about to be destroyed
// The path is returned and then this function will be terminated
if (queue.IsTerminating)
{
path.FailWithError("AstarPath object destroyed");
}
}
path.duration = (System.DateTime.UtcNow.Ticks - startTicks) * 0.0001F;
#if ProfileAstar
System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted);
System.Threading.Interlocked.Add(ref AstarPath.TotalSearchTime, System.DateTime.UtcNow.Ticks - startTicks);
#endif
}
// Cleans up node tagging and other things
ipath.Cleanup();
AstarProfiler.StartFastProfile(9);
if (path.immediateCallback != null)
{
path.immediateCallback(path);
}
if (OnPathPostSearch != null)
{
OnPathPostSearch(path);
}
// Push the path onto the return stack
// It will be detected by the main Unity thread and returned as fast as possible (the next late update hopefully)
returnQueue.Enqueue(path);
// Will advance to ReturnQueue
ipath.AdvanceState(PathState.ReturnQueue);
AstarProfiler.EndFastProfile(9);
#if UNITY_2017_3_OR_NEWER
profilingSampler.End();
#endif
}
#if !ASTAR_FAST_BUT_NO_EXCEPTIONS
}
catch (System.Exception e) {
#if !NETFX_CORE
if (e is ThreadAbortException || e is ThreadControlQueue.QueueTerminationException)
#else
if (e is ThreadControlQueue.QueueTerminationException)
#endif
{
if (astar.logPathResults == PathLog.Heavy)
{
Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID);
}
return;
}
Debug.LogException(e);
Debug.LogError("Unhandled exception during pathfinding. Terminating.");
// Unhandled exception, kill pathfinding
queue.TerminateReceivers();
} finally {
#if UNITY_2017_3_OR_NEWER
UnityEngine.Profiling.Profiler.EndThreadProfiling();
#endif
}
#endif
Debug.LogError("Error : This part should never be reached.");
queue.ReceiverTerminated();
}
void IPathInternals.PrepareBase(PathHandler handler)
{
PrepareBase(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;
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
}
private IEnumerator CalculatePaths(PathThreadInfo threadInfo)
{
int numPaths = 0;
PathHandler runData = threadInfo.runData;
if (runData.nodes == null)
{
throw new NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
}
long maxTicks = (long)(this.astar.maxFrameTime * 10000f);
long targetTick = DateTime.UtcNow.Ticks + maxTicks;
for (;;)
{
Path p = null;
bool blockedBefore = false;
while (p == null)
{
try
{
p = this.queue.PopNoBlock(blockedBefore);
blockedBefore |= (p == null);
}
catch (ThreadControlQueue.QueueTerminationException)
{
yield break;
}
if (p == null)
{
yield return(null);
}
}
IPathInternals ip = p;
maxTicks = (long)(this.astar.maxFrameTime * 10000f);
ip.PrepareBase(runData);
ip.AdvanceState(PathState.Processing);
Action <Path> tmpOnPathPreSearch = this.OnPathPreSearch;
if (tmpOnPathPreSearch != null)
{
tmpOnPathPreSearch(p);
}
numPaths++;
long startTicks = DateTime.UtcNow.Ticks;
long totalTicks = 0L;
ip.Prepare();
if (!p.IsDone())
{
this.astar.debugPathData = ip.PathHandler;
this.astar.debugPathID = p.pathID;
ip.Initialize();
while (!p.IsDone())
{
ip.CalculateStep(targetTick);
if (p.IsDone())
{
break;
}
totalTicks += DateTime.UtcNow.Ticks - startTicks;
yield return(null);
startTicks = DateTime.UtcNow.Ticks;
if (this.queue.IsTerminating)
{
p.Error();
}
targetTick = DateTime.UtcNow.Ticks + maxTicks;
}
totalTicks += DateTime.UtcNow.Ticks - startTicks;
p.duration = (float)totalTicks * 0.0001f;
}
ip.Cleanup();
OnPathDelegate tmpImmediateCallback = p.immediateCallback;
if (tmpImmediateCallback != null)
{
tmpImmediateCallback(p);
}
Action <Path> tmpOnPathPostSearch = this.OnPathPostSearch;
if (tmpOnPathPostSearch != null)
{
tmpOnPathPostSearch(p);
}
this.returnQueue.Enqueue(p);
ip.AdvanceState(PathState.ReturnQueue);
if (DateTime.UtcNow.Ticks > targetTick)
{
yield return(null);
targetTick = DateTime.UtcNow.Ticks + maxTicks;
numPaths = 0;
}
}
yield break;
}
private void CalculatePathsThreaded(PathThreadInfo threadInfo)
{
try
{
PathHandler runData = threadInfo.runData;
if (runData.nodes == null)
{
throw new NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
}
long num = (long)(this.astar.maxFrameTime * 10000f);
long num2 = DateTime.UtcNow.Ticks + num;
for (;;)
{
Path path = this.queue.Pop();
IPathInternals pathInternals = path;
num = (long)(this.astar.maxFrameTime * 10000f);
pathInternals.PrepareBase(runData);
pathInternals.AdvanceState(PathState.Processing);
if (this.OnPathPreSearch != null)
{
this.OnPathPreSearch(path);
}
long ticks = DateTime.UtcNow.Ticks;
long num3 = 0L;
pathInternals.Prepare();
if (!path.IsDone())
{
this.astar.debugPathData = pathInternals.PathHandler;
this.astar.debugPathID = path.pathID;
pathInternals.Initialize();
while (!path.IsDone())
{
pathInternals.CalculateStep(num2);
if (path.IsDone())
{
break;
}
num3 += DateTime.UtcNow.Ticks - ticks;
Thread.Sleep(0);
ticks = DateTime.UtcNow.Ticks;
num2 = ticks + num;
if (this.queue.IsTerminating)
{
path.Error();
}
}
num3 += DateTime.UtcNow.Ticks - ticks;
path.duration = (float)num3 * 0.0001f;
}
pathInternals.Cleanup();
if (path.immediateCallback != null)
{
path.immediateCallback(path);
}
if (this.OnPathPostSearch != null)
{
this.OnPathPostSearch(path);
}
this.returnQueue.Enqueue(path);
pathInternals.AdvanceState(PathState.ReturnQueue);
if (DateTime.UtcNow.Ticks > num2)
{
Thread.Sleep(1);
num2 = DateTime.UtcNow.Ticks + num;
}
}
}
catch (Exception ex)
{
if (ex is ThreadAbortException || ex is ThreadControlQueue.QueueTerminationException)
{
if (this.astar.logPathResults == PathLog.Heavy)
{
Debug.LogWarning("Shutting down pathfinding thread #" + threadInfo.threadIndex);
}
return;
}
Debug.LogException(ex);
Debug.LogError("Unhandled exception during pathfinding. Terminating.");
this.queue.TerminateReceivers();
}
Debug.LogError("Error : This part should never be reached.");
this.queue.ReceiverTerminated();
}
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);
}
}
}
/* 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;
}
/** Main pathfinding method (multithreaded).
* This method will calculate the paths in the pathfinding queue when multithreading is enabled.
*
* \see CalculatePaths
* \see StartPath
*
* \astarpro
*/
void CalculatePathsThreaded(PathThreadInfo threadInfo)
{
try {
//Initialize memory for this thread
PathHandler runData = threadInfo.runData;
if (runData.nodes == null)
{
throw new System.NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
}
//Max number of ticks before yielding/sleeping
long maxTicks = (long)(astar.maxFrameTime * 10000);
long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
while (true)
{
//The path we are currently calculating
Path p = queue.Pop();
//Max number of ticks we are allowed to continue working in one run
//One tick is 1/10000 of a millisecond
maxTicks = (long)(astar.maxFrameTime * 10000);
//Trying to prevent simple modding to allow more than one thread
if (threadInfo.threadIndex > 0)
{
throw new System.Exception("Thread Error");
}
AstarProfiler.StartFastProfile(0);
p.PrepareBase(runData);
//Now processing the path
//Will advance to Processing
p.AdvanceState(PathState.Processing);
//Call some callbacks
if (OnPathPreSearch != null)
{
OnPathPreSearch(p);
}
//Tick for when the path started, used for calculating how long time the calculation took
long startTicks = System.DateTime.UtcNow.Ticks;
long totalTicks = 0;
//Prepare the path
p.Prepare();
AstarProfiler.EndFastProfile(0);
if (!p.IsDone())
{
//For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
astar.debugPath = p;
AstarProfiler.StartFastProfile(1);
//Initialize the path, now ready to begin search
p.Initialize();
AstarProfiler.EndFastProfile(1);
//The error can turn up in the Init function
while (!p.IsDone())
{
//Do some work on the path calculation.
//The function will return when it has taken too much time
//or when it has finished calculation
AstarProfiler.StartFastProfile(2);
p.CalculateStep(targetTick);
p.searchIterations++;
AstarProfiler.EndFastProfile(2);
// If the path has finished calculation, we can break here directly instead of sleeping
if (p.IsDone())
{
break;
}
// Yield/sleep so other threads can work
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
Thread.Sleep(0);
startTicks = System.DateTime.UtcNow.Ticks;
targetTick = startTicks + maxTicks;
// Cancel function (and thus the thread) if no more paths should be accepted.
// This is done when the A* object is about to be destroyed
// The path is returned and then this function will be terminated
if (queue.IsTerminating)
{
p.Error();
}
}
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
p.duration = totalTicks * 0.0001F;
}
// Cleans up node tagging and other things
p.Cleanup();
AstarProfiler.StartFastProfile(9);
if (p.immediateCallback != null)
{
p.immediateCallback(p);
}
if (OnPathPostSearch != null)
{
OnPathPostSearch(p);
}
// Push the path onto the return stack
// It will be detected by the main Unity thread and returned as fast as possible (the next late update hopefully)
returnQueue.Enqueue(p);
// Will advance to ReturnQueue
p.AdvanceState(PathState.ReturnQueue);
AstarProfiler.EndFastProfile(9);
// Wait a bit if we have calculated a lot of paths
if (System.DateTime.UtcNow.Ticks > targetTick)
{
Thread.Sleep(1);
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
}
}
} catch (System.Exception e) {
#if !NETFX_CORE
if (e is ThreadAbortException || e is ThreadControlQueue.QueueTerminationException)
#else
if (e is ThreadControlQueue.QueueTerminationException)
#endif
{
if (astar.logPathResults == PathLog.Heavy)
{
Debug.LogWarning("Shutting down pathfinding thread #" + threadInfo.threadIndex);
}
return;
}
Debug.LogException(e);
Debug.LogError("Unhandled exception during pathfinding. Terminating.");
//Unhandled exception, kill pathfinding
queue.TerminateReceivers();
}
Debug.LogError("Error : This part should never be reached.");
queue.ReceiverTerminated();
}
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
}
/** Main pathfinding method.
* This method will calculate the paths in the pathfinding queue.
*
* \see CalculatePathsThreaded
* \see StartPath
*/
IEnumerator CalculatePaths(PathThreadInfo threadInfo)
{
int numPaths = 0;
// Initialize memory for this thread
PathHandler runData = threadInfo.runData;
if (runData.nodes == null)
{
throw new System.NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\n" +
"threadInfo is an argument to the thread functions");
}
// Max number of ticks before yielding/sleeping
long maxTicks = (long)(astar.maxFrameTime * 10000);
long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
while (true)
{
//The path we are currently calculating
Path p = null;
AstarProfiler.StartProfile("Path Queue");
//Try to get the next path to be calculated
bool blockedBefore = false;
while (p == null)
{
try {
p = queue.PopNoBlock(blockedBefore);
blockedBefore |= p == null;
} catch (ThreadControlQueue.QueueTerminationException) {
yield break;
}
if (p == null)
{
AstarProfiler.EndProfile();
yield return(null);
AstarProfiler.StartProfile("Path Queue");
}
}
AstarProfiler.EndProfile();
AstarProfiler.StartProfile("Path Calc");
//Max number of ticks we are allowed to continue working in one run
//One tick is 1/10000 of a millisecond
maxTicks = (long)(astar.maxFrameTime * 10000);
p.PrepareBase(runData);
//Now processing the path
//Will advance to Processing
p.AdvanceState(PathState.Processing);
// Call some callbacks
// It needs to be stored in a local variable to avoid race conditions
var tmpOnPathPreSearch = OnPathPreSearch;
if (tmpOnPathPreSearch != null)
{
tmpOnPathPreSearch(p);
}
numPaths++;
//Tick for when the path started, used for calculating how long time the calculation took
long startTicks = System.DateTime.UtcNow.Ticks;
long totalTicks = 0;
AstarProfiler.StartFastProfile(8);
AstarProfiler.StartFastProfile(0);
//Prepare the path
AstarProfiler.StartProfile("Path Prepare");
p.Prepare();
AstarProfiler.EndProfile("Path Prepare");
AstarProfiler.EndFastProfile(0);
// Check if the Prepare call caused the path to complete
// If this happens the path usually failed
if (!p.IsDone())
{
//For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
astar.debugPath = p;
//Initialize the path, now ready to begin search
AstarProfiler.StartProfile("Path Initialize");
p.Initialize();
AstarProfiler.EndProfile();
//The error can turn up in the Init function
while (!p.IsDone())
{
// Do some work on the path calculation.
// The function will return when it has taken too much time
// or when it has finished calculation
AstarProfiler.StartFastProfile(2);
AstarProfiler.StartProfile("Path Calc Step");
p.CalculateStep(targetTick);
AstarProfiler.EndFastProfile(2);
p.searchIterations++;
AstarProfiler.EndProfile();
// If the path has finished calculation, we can break here directly instead of sleeping
// Improves latency
if (p.IsDone())
{
break;
}
AstarProfiler.EndFastProfile(8);
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
// Yield/sleep so other threads can work
AstarProfiler.EndProfile();
yield return(null);
AstarProfiler.StartProfile("Path Calc");
startTicks = System.DateTime.UtcNow.Ticks;
AstarProfiler.StartFastProfile(8);
//Cancel function (and thus the thread) if no more paths should be accepted.
//This is done when the A* object is about to be destroyed
//The path is returned and then this function will be terminated (see similar IF statement higher up in the function)
if (queue.IsTerminating)
{
p.Error();
}
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
}
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
p.duration = totalTicks * 0.0001F;
}
// Cleans up node tagging and other things
p.Cleanup();
AstarProfiler.EndFastProfile(8);
// Call the immediate callback
// It needs to be stored in a local variable to avoid race conditions
var tmpImmediateCallback = p.immediateCallback;
if (tmpImmediateCallback != null)
{
tmpImmediateCallback(p);
}
AstarProfiler.StartFastProfile(13);
// It needs to be stored in a local variable to avoid race conditions
var tmpOnPathPostSearch = OnPathPostSearch;
if (tmpOnPathPostSearch != null)
{
tmpOnPathPostSearch(p);
}
AstarProfiler.EndFastProfile(13);
//Push the path onto the return stack
//It will be detected by the main Unity thread and returned as fast as possible (the next late update)
returnQueue.Enqueue(p);
p.AdvanceState(PathState.ReturnQueue);
AstarProfiler.EndProfile();
//Wait a bit if we have calculated a lot of paths
if (System.DateTime.UtcNow.Ticks > targetTick)
{
yield return(null);
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
numPaths = 0;
}
}
//Debug.LogError ("Error : This part should never be reached");
}
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 abstract void Open(Path path, PathNode pathNode, PathHandler 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 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 PathThreadInfo (int index, AstarPath astar, PathHandler runData) {
this.threadIndex = index;
this.astar = astar;
this.runData = runData;
}
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);
#if ASTAR_CONSTANT_PENALTY
uint tmpCost = neighbourCosts[i];
#else
// 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;
#endif
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_CONSTANT_PENALTY
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#else
if (pathNode.G + tmpCost < 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_CONSTANT_PENALTY
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
#else
else if (otherPN.G + tmpCost < 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_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);
#if ASTAR_CONSTANT_PENALTY
uint tmpCost = connectionCosts[i];
#else
uint tmpCost = (connectionCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other))) / 128;
#endif
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_CONSTANT_PENALTY
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#else
if (pathNode.G + tmpCost < 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_CONSTANT_PENALTY
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this))
#else
else if (otherPN.G + tmpCost < 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) {
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 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)
{
throw new System.NotImplementedException();
}
/** 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) {
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);
}
}
}
}
}
/** 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 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);
}
}
}
}
}
/** 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.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);
//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) {
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 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.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)
#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) {
throw new System.NotImplementedException();
}
// Token: 0x06002321 RID: 8993 RVA: 0x00193A00 File Offset: 0x00191C00
private void CalculatePathsThreaded(PathHandler pathHandler)
{
try
{
long num = 100000L;
long targetTick = DateTime.UtcNow.Ticks + num;
for (;;)
{
Path path = this.queue.Pop();
IPathInternals pathInternals = path;
pathInternals.PrepareBase(pathHandler);
pathInternals.AdvanceState(PathState.Processing);
if (this.OnPathPreSearch != null)
{
this.OnPathPreSearch(path);
}
long ticks = DateTime.UtcNow.Ticks;
pathInternals.Prepare();
if (!path.IsDone())
{
this.astar.debugPathData = pathInternals.PathHandler;
this.astar.debugPathID = path.pathID;
pathInternals.Initialize();
while (!path.IsDone())
{
pathInternals.CalculateStep(targetTick);
targetTick = DateTime.UtcNow.Ticks + num;
if (this.queue.IsTerminating)
{
path.FailWithError("AstarPath object destroyed");
}
}
path.duration = (float)(DateTime.UtcNow.Ticks - ticks) * 0.0001f;
}
pathInternals.Cleanup();
if (path.immediateCallback != null)
{
path.immediateCallback(path);
}
if (this.OnPathPostSearch != null)
{
this.OnPathPostSearch(path);
}
this.returnQueue.Enqueue(path);
pathInternals.AdvanceState(PathState.ReturnQueue);
}
}
catch (Exception ex)
{
if (ex is ThreadAbortException || ex is ThreadControlQueue.QueueTerminationException)
{
if (this.astar.logPathResults == PathLog.Heavy)
{
Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID);
}
return;
}
Debug.LogException(ex);
Debug.LogError("Unhandled exception during pathfinding. Terminating.");
this.queue.TerminateReceivers();
}
Debug.LogError("Error : This part should never be reached.");
this.queue.ReceiverTerminated();
}
/** 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;
}
/* 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)
{
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 / (float)AstarPath.active.debugRoof);
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 / (float)AstarPath.active.debugRoof);
break;
case GraphDebugMode.H:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.H / (float)AstarPath.active.debugRoof);
break;
case GraphDebugMode.F:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.F / (float)AstarPath.active.debugRoof);
break;
}
}
c.a *= 0.5F;
return(c);
}
/** 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
}
}
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);
}
}
}
}
/** Open the node */ public abstract void Open(Path path, PathNode pathNode, PathHandler 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) {
//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 PathThreadInfo(int index, AstarPath astar, PathHandler runData)
{
this.threadIndex = index;
this.astar = astar;
this.runData = runData;
}
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);
}
}
}
}
}
// Token: 0x06002322 RID: 8994 RVA: 0x00193BD0 File Offset: 0x00191DD0
private IEnumerator CalculatePaths(PathHandler pathHandler)
{
long maxTicks = (long)(this.astar.maxFrameTime * 10000f);
long targetTick = DateTime.UtcNow.Ticks + maxTicks;
for (;;)
{
Path p = null;
bool blockedBefore = false;
while (p == null)
{
try
{
p = this.queue.PopNoBlock(blockedBefore);
blockedBefore |= (p == null);
}
catch (ThreadControlQueue.QueueTerminationException)
{
yield break;
}
if (p == null)
{
yield return(null);
}
}
IPathInternals ip = p;
maxTicks = (long)(this.astar.maxFrameTime * 10000f);
ip.PrepareBase(pathHandler);
ip.AdvanceState(PathState.Processing);
Action <Path> onPathPreSearch = this.OnPathPreSearch;
if (onPathPreSearch != null)
{
onPathPreSearch(p);
}
long ticks = DateTime.UtcNow.Ticks;
long totalTicks = 0L;
ip.Prepare();
if (!p.IsDone())
{
this.astar.debugPathData = ip.PathHandler;
this.astar.debugPathID = p.pathID;
ip.Initialize();
while (!p.IsDone())
{
ip.CalculateStep(targetTick);
if (p.IsDone())
{
break;
}
totalTicks += DateTime.UtcNow.Ticks - ticks;
yield return(null);
ticks = DateTime.UtcNow.Ticks;
if (this.queue.IsTerminating)
{
p.FailWithError("AstarPath object destroyed");
}
targetTick = DateTime.UtcNow.Ticks + maxTicks;
}
totalTicks += DateTime.UtcNow.Ticks - ticks;
p.duration = (float)totalTicks * 0.0001f;
}
ip.Cleanup();
OnPathDelegate immediateCallback = p.immediateCallback;
if (immediateCallback != null)
{
immediateCallback(p);
}
Action <Path> onPathPostSearch = this.OnPathPostSearch;
if (onPathPostSearch != null)
{
onPathPostSearch(p);
}
this.returnQueue.Enqueue(p);
ip.AdvanceState(PathState.ReturnQueue);
if (DateTime.UtcNow.Ticks > targetTick)
{
yield return(null);
targetTick = DateTime.UtcNow.Ticks + maxTicks;
}
p = null;
ip = null;
}
yield break;
}
/** Prepares low level path variables for calculation.
* Called before a path search will take place.
* Always called before the Prepare, Initialize and CalculateStep functions
*/
public void PrepareBase (PathHandler pathHandler) {
//Path IDs have overflowed 65K, cleanup is needed
//Since pathIDs are handed out sequentially, we can do this
if (pathHandler.PathID > pathID) {
pathHandler.ClearPathIDs ();
}
//Make sure the path has a reference to the pathHandler
this.pathHandler = pathHandler;
//Assign relevant path data to the pathHandler
pathHandler.InitializeForPath (this);
// Make sure that internalTagPenalties is an array which has the length 32
if (internalTagPenalties == null || internalTagPenalties.Length != 32)
internalTagPenalties = ZeroTagPenalties;
try {
ErrorCheck ();
} catch (System.Exception e) {
ForceLogError ("Exception in path "+pathID+"\n"+e);
}
}
/** Reset all values to their default values.
*
* \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
* override this function, resetting ALL their variables to enable recycling of paths.
* If this is not done, trying to use that path type for pooling might result in weird behaviour.
* The best way is to reset to default values the variables declared in the extended path type and then
* call this base function in inheriting types with base.Reset ().
*
* \warning This function should not be called manually.
*/
public virtual void Reset () {
#if ASTAR_POOL_DEBUG
pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n";
pathTraceInfo += System.Environment.StackTrace;
#endif
if (AstarPath.active == null)
throw new System.NullReferenceException ("No AstarPath object found in the scene. " +
"Make sure there is one or do not create paths in Awake");
hasBeenReset = true;
state = (int)PathState.Created;
releasedNotSilent = false;
pathHandler = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = Pathfinding.Util.ListPool<GraphNode>.Claim();
vectorPath = Pathfinding.Util.ListPool<Vector3>.Claim();
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
//calltime
nnConstraint = PathNNConstraint.Default;
next = null;
radius = 0;
walkabilityMask = -1;
height = 0;
turnRadius = 0;
speed = 0;
//heuristic = (Heuristic)0;
//heuristicScale = 1F;
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
pathID = 0;
enabledTags = -1;
tagPenalties = null;
callTime = System.DateTime.UtcNow;
pathID = AstarPath.active.GetNextPathID ();
hTarget = Int3.zero;
}
/** Prepares low level path variables for calculation.
* Called before a path search will take place.
* Always called before the Prepare, Initialize and CalculateStep functions
*/
public void PrepareBase (PathHandler pathHandler) {
//Path IDs have overflowed 65K, cleanup is needed
//Since pathIDs are handed out sequentially, we can do this
if (pathHandler.PathID > pathID) {
pathHandler.ClearPathIDs ();
}
//Make sure the path has a reference to the pathHandler
this.pathHandler = pathHandler;
//Assign relevant path data to the pathHandler
pathHandler.InitializeForPath (this);
try {
ErrorCheck ();
} catch (System.Exception e) {
ForceLogError ("Exception in path "+pathID+"\n"+e.ToString());
}
}
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];
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);
}
}
}
}
}