/** Rebuilds the heap by trickeling down all items. Called after the hTarget on a path has been changed */
public void Rebuild()
{
for (int i = 2; i < numberOfItems; i++)
{
int bubbleIndex = i;
NodeRun node = binaryHeap[i];
uint nodeF = node.f;
while (bubbleIndex != 1)
{
int parentIndex = bubbleIndex / 2;
if (nodeF < binaryHeap[parentIndex].f)
{
//Node tmpValue = binaryHeap[parentIndex];
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
bubbleIndex = parentIndex;
}
else
{
break;
}
}
}
}
public override void Trace(NodeRun node)
{
base.Trace(node);
if (inverted)
{
//Invert the paths
int half = path.Length / 2;
for (int i = 0; i < half; i++)
{
Node tmp = path[i];
path[i] = path[path.Length - i - 1];
path[path.Length - i - 1] = tmp;
}
for (int i = 0; i < half; i++)
{
Vector3 tmp = vectorPath[i];
vectorPath[i] = vectorPath[vectorPath.Length - i - 1];
vectorPath[vectorPath.Length - i - 1] = tmp;
}
}
}
/** Traces the calculated path from the end node to the start.
* This will build an array (#path) of the nodes this path will pass through and also set the #vectorPath array to the #path arrays positions.
* This function will not revert the path as the original implementation does, so the path will go from \a from to the parent root node (usually the start node) */
public override void Trace(NodeRun from)
{
int count = 0;
NodeRun c = from;
while (c != null) {
c = c.parent;
count++;
if (count > 1024) {
Debug.LogWarning ("Inifinity loop? >1024 node path");
break;
}
}
path = new Node[count];
c = from;
for (int i = 0;i<count;i++) {
path[i] = c.node;
c = c.parent;
}
vectorPath = new Vector3[count];
for (int i=0;i<count;i++) {
vectorPath[i] = (Vector3)path[i].position;
}
}
/** Traces the calculated path from the end node to the start.
* This will build an array (#path) of the nodes this path will pass through and also set the #vectorPath array to the #path arrays positions */
public virtual void Trace(NodeRun from)
{
int count = 0;
NodeRun c = from;
while (c != null)
{
c = c.parent;
count++;
if (count > 1024)
{
Debug.LogWarning("Inifinity loop? >1024 node path");
break;
}
}
path = new Node[count];
c = from;
for (int i = 0; i < count; i++)
{
path[count - 1 - i] = c.node;
c = c.parent;
}
vectorPath = new Vector3[count];
for (int i = 0; i < count; i++)
{
vectorPath[i] = (Vector3)path[i].position;
}
}
public void UpdateG(NodeRun nodeR, NodeRunData nodeRunData)
{
nodeR.g = nodeR.parent.g + nodeR.cost + penalty
#if !ASTAR_NoTagPenalty
+ nodeRunData.path.GetTagPenalty(tags)
#endif
;
}
public void Reset()
{
g = 0;
h = 0;
pathID = 0;
cost = 0;
parent = null;
}
public void Reset()
{
g = 0;
h = 0;
pathID = 0;
cost = 0;
parent = null;
}
public override void Initialize()
{
if (hasEndPoint && startNode == endNode)
{
NodeRun endNodeR = endNode.GetNodeRun(runData);
endNodeR.parent = null;
endNodeR.h = 0;
endNodeR.g = 0;
Trace(endNodeR);
CompleteState = PathCompleteState.Complete;
return;
}
//Adjust the costs for the end node
/*if (hasEndPoint && recalcStartEndCosts) {
* endNodeCosts = endNode.InitialOpen (open,hTarget,(Int3)endPoint,this,false);
* callback += ResetCosts; /* \todo Might interfere with other paths since other paths might be calculated before #callback is called *
* }*/
NodeRun startRNode = startNode.GetNodeRun(runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH(hTarget, heuristic, heuristicScale, startRNode);
/*if (recalcStartEndCosts) {
* startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
* } else {*/
startNode.Open(runData, startRNode, hTarget, this);
//}
searchedNodes++;
partialBestTarget = startRNode;
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
if (calculatePartial)
{
CompleteState = PathCompleteState.Partial;
Trace(partialBestTarget);
}
else
{
Error();
LogError("No open points, the start node didn't open any nodes");
return;
}
}
currentR = runData.open.Remove();
}
public void RebuildOpenList()
{
for (int j = 1; j < runData.open.numberOfItems; j++)
{
NodeRun nodeR = runData.open.binaryHeap[j];
nodeR.node.UpdateH(hTarget, heuristic, heuristicScale, nodeR);
}
runData.open.Rebuild();
}
/** Returns if the node is in the search tree of the path.
* Only guaranteed to be correct if \a path is the latest path calculated.
* Use for gizmo drawing only.
*/
public bool InSearchTree(Node node, Path path)
{
if (path == null || path.runData == null)
{
return(true);
}
NodeRun nodeR = node.GetNodeRun(path.runData);
return(nodeR.pathID == path.pathID);
}
public override void Initialize()
{
runData.pathID = pathID;
//Resets the binary heap, don't clear everything because that takes an awful lot of time, instead we can just change the numberOfItems in it (which is just an int)
//Binary heaps are just like a standard array but are always sorted so the node with the lowest F value can be retrieved faster
runData.open.Clear();
for (int j = 0; j < targetNodes.Length; j++)
{
if (startNode == targetNodes[j])
{
FoundTarget(startNode.GetNodeRun(runData), j);
}
else
{
targetNodes[j].Bit8 = true;
}
}
//Reset Bit8 on all nodes after the pathfinding has completed (no matter if an error occurs or if the path is canceled)
AstarPath.OnPathPostSearch += ResetBit8;
//If all paths have either been invalidated or found already because they were at the same node as the start node
if (targetNodeCount <= 0)
{
foundEnd = true;
return;
}
NodeRun startRNode = startNode.GetNodeRun(runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH(hTarget, heuristic, heuristicScale, startRNode); //Will just set H to zero
//if (recalcStartEndCosts) {
// startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
//} else {
startNode.Open(runData, startRNode, hTarget, this);
//}
searchedNodes++;
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
LogError("No open points, the start node didn't open any nodes");
return;
}
currentR = runData.open.Remove();
}
public void Reset()
{
g = 0;
h = 0;
#if !ASTAR_SINGLE_THREAD_OPTIMIZE
node = null;
#endif
pathID = 0;
cost = 0;
parent = null;
}
/** 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;
runData = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = Pathfinding.Util.ListPool <Node> .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();
}
/** Traces the calculated path from the end node to the start.
* This will build an array (#path) of the nodes this path will pass through and also set the #vectorPath array to the #path arrays positions.
* Assumes the #vectorPath and #path are empty and not null (which will be the case for a correctly initialized path).
*/
protected virtual void Trace(NodeRun from)
{
int count = 0;
NodeRun c = from;
while (c != null)
{
c = c.parent;
count++;
if (count > 1024)
{
Debug.LogWarning("Inifinity loop? >1024 node path. Remove this message if you really have that long paths (Path.cs, Trace function)");
break;
}
}
//Ensure capacities for lists
AstarProfiler.StartProfile("Check List Capacities");
if (path.Capacity < count)
{
path.Capacity = count;
}
if (vectorPath.Capacity < count)
{
vectorPath.Capacity = count;
}
AstarProfiler.EndProfile();
c = from;
for (int i = 0; i < count; i++)
{
path.Add(c.node);
c = c.parent;
}
int half = count / 2;
for (int i = 0; i < half; i++)
{
Node tmp = path[i];
path[i] = path[count - i - 1];
path[count - i - 1] = tmp;
}
for (int i = 0; i < count; i++)
{
vectorPath.Add((Vector3)path[i].position);
}
}
/** Returns the node with the lowest F score from the heap */
public NodeRun Remove()
{
numberOfItems--;
NodeRun returnItem = binaryHeap[1];
//returnItem.heapIndex = 0;//Heap index
binaryHeap[1] = binaryHeap[numberOfItems];
//binaryHeap[1].heapIndex = 1;//Heap index
int swapItem = 1, parent = 1;
do
{
parent = swapItem;
int p2 = parent * 2;
if ((p2 + 1) <= numberOfItems)
{
// Both children exist
if (binaryHeap[parent].f >= binaryHeap[p2].f)
{
swapItem = p2; //2 * parent;
}
if (binaryHeap[swapItem].f >= binaryHeap[p2 + 1].f)
{
swapItem = p2 + 1;
}
}
else if ((p2) <= numberOfItems)
{
// Only one child exists
if (binaryHeap[parent].f >= binaryHeap[p2].f)
{
swapItem = p2;
}
}
// One if the parent's children are smaller or equal, swap them
if (parent != swapItem)
{
NodeRun tmpIndex = binaryHeap[parent];
//tmpIndex.heapIndex = swapItem;//Heap index
binaryHeap[parent] = binaryHeap[swapItem];
binaryHeap[swapItem] = tmpIndex;
//binaryHeap[parent].heapIndex = parent;//Heap index
}
} while (parent != swapItem);
return(returnItem);
}
/** Initializes the path. Sets up the open list and adds the first node to it */
public virtual void Initialize()
{
runData.pathID = pathID;
//Resets the binary heap, don't clear everything because that takes an awful lot of time, instead we can just change the numberOfItems in it (which is just an int)
//Binary heaps are just like a standard array but are always sorted so the node with the lowest F value can be retrieved faster
runData.open.Clear();
if (hasEndPoint && startNode == endNode)
{
NodeRun endNodeR = endNode.GetNodeRun(runData);
endNodeR.parent = null;
endNodeR.h = 0;
endNodeR.g = 0;
Trace(endNodeR);
foundEnd = true;
return;
}
//Adjust the costs for the end node
/*if (hasEndPoint && recalcStartEndCosts) {
* endNodeCosts = endNode.InitialOpen (open,hTarget,(Int3)endPoint,this,false);
* callback += ResetCosts; /* \todo Might interfere with other paths since other paths might be calculated before #callback is called *
* }*/
//Node.activePath = this;
NodeRun startRNode = startNode.GetNodeRun(runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH(hTarget, heuristic, heuristicScale, startRNode);
/*if (recalcStartEndCosts) {
* startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
* } else {*/
startNode.Open(runData, startRNode, hTarget, this);
//}
searchedNodes++;
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
LogError("No open points, the start node didn't open any nodes");
return;
}
currentR = runData.open.Remove();
}
/** Calculates the path until completed or until the time has passed \a targetTick.
* Usually a check is only done every 500 nodes if the time has passed \a targetTick.
* Time/Ticks are got from System.DateTime.Now.Ticks.
*
* Basic outline of what the function does for the standard path (Pathfinding.Path).
* \code
* while the end has not been found and no error has ocurred
* check if we have reached the end
* if so, exit and return the path
*
* open the current node, i.e loop through its neighbours, mark them as visited and put them on a heap
*
* check if there are still nodes left to process (or have we searched the whole graph)
* if there are none, flag error and exit
*
* pop the next node of the heap and set it as current
*
* check if the function has exceeded the time limit
* if so, return and wait for the function to get called again
* \endcode
*/
public virtual void CalculateStep(long targetTick)
{
int counter = 0;
//Continue to search while there hasn't ocurred an error and the end hasn't been found
while (!foundEnd && !error)
{
//@Performance Just for debug info
searchedNodes++;
//Close the current node, if the current node is the target node then the path is finnished
if (currentR.node == endNode)
{
foundEnd = true;
break;
}
//Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open(runData, currentR, hTarget, this);
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
LogError("No open points, whole area searched");
return;
}
//Select the node with the lowest F score and remove it from the open list
currentR = runData.open.Remove();
//Check for time every 500 nodes, roughly every 0.5 ms usually
if (counter > 500)
{
//Have we exceded the maxFrameTime, if so we should wait one frame before continuing the search since we don't want the game to lag
if (System.DateTime.Now.Ticks >= targetTick)
{
//Return instead of yield'ing, a separate function handles the yield (CalculatePaths)
return;
}
counter = 0;
}
counter++;
}
if (foundEnd && !error)
{
Trace(currentR);
}
}
public override void Initialize()
{
for (int j = 0; j < targetNodes.Length; j++)
{
if (startNode == targetNodes[j])
{
FoundTarget(startNode.GetNodeRun(runData), j);
}
else if (targetNodes[j] != null)
{
targetNodes[j].Bit8 = true;
}
}
//Reset Bit8 on all nodes after the pathfinding has completed (no matter if an error occurs or if the path is canceled)
AstarPath.OnPathPostSearch += ResetBit8;
//If all paths have either been invalidated or found already because they were at the same node as the start node
if (targetNodeCount <= 0)
{
CompleteState = PathCompleteState.Complete;
return;
}
NodeRun startRNode = startNode.GetNodeRun(runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH(hTarget, heuristic, heuristicScale, startRNode); //Will just set H to zero
//if (recalcStartEndCosts) {
// startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
//} else {
startNode.Open(runData, startRNode, hTarget, this);
//}
searchedNodes++;
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
LogError("No open points, the start node didn't open any nodes");
return;
}
currentR = runData.open.Remove();
}
public override void Reset()
{
base.Reset();
searchLength = 5000;
spread = 5000;
uniform = true;
replaceChance = 0.1F;
aimStrength = 0.0f;
chosenNodeR = null;
maxGScoreNodeR = null;
maxGScore = 0;
aim = Vector3.zero;
hasEndPoint = false;
}
/** Creates the structure for holding temporary path data.
* The data is for example the G, H and F scores and the search tree.
* The number of nodeRuns must be no less than the number of nodes contained in all graphs.
* So after adding nodes, this MUST be called.\n
* Ideally, I would code an update function which reuses most of the previous ones instead of recreating it every time.
* \param numParallel Number of parallel threads which will use the data.
* \see #nodeRuns
* \see AstarPath::UpdatePathThreadInfoNodes
*/
public void CreateNodeRuns(int numParallel)
{
if (graphs == null)
{
throw new System.Exception("Cannot create NodeRuns when no graphs exist. (Scan and or Load graphs first)");
}
int nodeCount = 0;
for (int i = 0; i < graphs.Length; i++)
{
if (graphs[i].nodes != null)
{
nodeCount += graphs[i].nodes.Length;
}
}
AssignNodeIndices();
#if !SingleCoreOptimize
nodeRuns = new NodeRun[numParallel][];
for (int i = 0; i < nodeRuns.Length; i++)
{
nodeRuns[i] = new NodeRun[nodeCount];
int counter = 0;
for (int j = 0; j < graphs.Length; j++)
{
Node[] nodes = graphs[j].nodes;
if (nodes == null)
{
continue;
}
for (int q = 0; q < nodes.Length; q++, counter++)
{
if (nodes[q] != null)
{
nodeRuns[i][counter] = new NodeRun();
nodeRuns[i][counter].Link(nodes[q], counter);
}
}
}
}
#endif
active.UpdatePathThreadInfoNodes();
}
/** Reset all values to their default values.
* All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths.
* Call this base function in inheriting types with base.Reset ();
*/
public override void Reset()
{
base.Reset();
startNode = null;
endNode = null;
startHint = null;
endHint = null;
originalStartPoint = Vector3.zero;
originalEndPoint = Vector3.zero;
startPoint = Vector3.zero;
endPoint = Vector3.zero;
calculatePartial = false;
partialBestTarget = null;
hasEndPoint = true;
startIntPoint = new Int3();
hTarget = new Int3();
endNodeCosts = null;
}
public virtual void Reset(Vector3 start, Vector3 end, OnPathDelegate callbackDelegate, bool reset)
{
if (reset)
{
processed = false;
vectorPath = null;
path = null;
next = null;
foundEnd = false;
error = false;
errorLog = "";
callback = null;
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
startHint = null;
endHint = null;
}
callTime = System.DateTime.Now;
callback = callbackDelegate;
/*if (AstarPath.active == null || AstarPath.active.graphs == null) {
* errorLog += "No NavGraphs have been calculated yet - Don't run any pathfinding calls in Awake";
* if (AstarPath.active.logPathResults != PathLog.None) {
* Debug.LogWarning (errorLog);
* }
* error = true;
* return;
* }*/
pathID = AstarPath.active.GetNextPathID();
UpdateStartEnd(start, end);
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
}
protected void BaseUpdateAllG(NodeRun nodeR, NodeRunData nodeRunData)
{
UpdateG(nodeR, nodeRunData);
nodeRunData.open.Add(nodeR);
if (connections == null)
{
return;
}
//Loop through the connections of this node and call UpdateALlG on nodes which have this node set as #parent and has been searched by the pathfinder for this path */
for (int i = 0; i < connections.Length; i++)
{
NodeRun otherR = connections[i].GetNodeRun(nodeRunData);
if (otherR.parent == nodeR && otherR.pathID == nodeRunData.pathID)
{
connections[i].UpdateAllG(otherR, nodeRunData);
}
}
}
/* F score. The F score is the #g score + #h score, that is the cost it taken to move to this node from the start + the estimated cost to move to the end node.\n
* Nodes are sorted by their F score, nodes with lower F scores are opened first */
/*public uint f {
* get {
* return g+h;
* }
* }*/
/** Calculates and updates the H score.
* Calculates the H score with respect to the target position and chosen heuristic.
* \param targetPosition The position to calculate the distance to.
* \param heuristic Heuristic to use. The heuristic can also be hard coded using pre processor directives (check sourcecode)
* \param scale Scale of the heuristic
* \param nodeR NodeRun object associated with this node.
*/
public void UpdateH(Int3 targetPosition, Heuristic heuristic, float scale, NodeRun nodeR)
{
//Choose the correct heuristic, compute it and store it in the \a h variable
if (heuristic == Heuristic.None)
{
nodeR.h = 0;
return;
}
if (heuristic == Heuristic.Euclidean)
{
nodeR.h = (uint)Mathfx.RoundToInt((position - targetPosition).magnitude * scale);
}
else if (heuristic == Heuristic.Manhattan)
{
nodeR.h = (uint)Mathfx.RoundToInt(
(Abs(position.x - targetPosition.x) +
Abs(position.y - targetPosition.y) +
Abs(position.z - targetPosition.z))
* scale
);
}
else //if (heuristic == Heuristic.DiagonalManhattan) {
{
int xDistance = Abs(position.x - targetPosition.x);
int zDistance = Abs(position.z - targetPosition.z);
if (xDistance > zDistance)
{
nodeR.h = (uint)(14 * zDistance + 10 * (xDistance - zDistance)) / 10;
}
else
{
nodeR.h = (uint)(14 * xDistance + 10 * (zDistance - xDistance)) / 10;
}
nodeR.h = (uint)Mathfx.RoundToInt(nodeR.h * scale);
}
}
/** Rebuilds the heap by trickeling down all items. Called after the hTarget on a path has been changed */
public void Rebuild()
{
#if DEBUG
int changes = 0;
#endif
for (int i = 2; i < numberOfItems; i++)
{
int bubbleIndex = i;
NodeRun node = binaryHeap[i];
uint nodeF = node.f;
while (bubbleIndex != 1)
{
int parentIndex = bubbleIndex / 2;
if (nodeF < binaryHeap[parentIndex].f)
{
//Node tmpValue = binaryHeap[parentIndex];
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;
bubbleIndex = parentIndex;
#if DEBUG
changes++;
#endif
}
else
{
break;
}
}
}
#if DEBUG
Debug.Log("+++ Rebuilt Heap - " + changes + " changes +++");
#endif
}
/** Initializes the path.
* Sets up the open list and adds the first node to it */
public override void Initialize () {
NodeRun startRNode = startNode.GetNodeRun (runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH (Int3.zero,heuristic,heuristicScale, startRNode);
startNode.Open (runData,startRNode,Int3.zero,this);
searchedNodes++;
allNodes.Add (startNode);
//any nodes left to search?
if (runData.open.numberOfItems <= 1) {
CompleteState = PathCompleteState.Complete;
return;
}
currentR = runData.open.Remove ();
}
/// <summary>
/// Returns a color to be used for the specified node with the current debug settings (editor only)
/// </summary>
/// <param name="node">
/// A <see cref="Node"/>
/// </param>
/// <returns>
/// A <see cref="Color"/>
/// </returns>
public virtual Color NodeColor(Node node, NodeRunData data)
{
#if !PhotonImplementation
Color c = AstarColor.NodeConnection;
bool colSet = false;
if (node == null)
{
return(AstarColor.NodeConnection);
}
switch (AstarPath.active.debugMode)
{
case GraphDebugMode.Areas:
c = AstarColor.GetAreaColor(node.area);
colSet = true;
break;
case GraphDebugMode.Penalty:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)node.penalty / (float)AstarPath.active.debugRoof);
colSet = true;
break;
case GraphDebugMode.Tags:
c = Mathfx.IntToColor(node.tags, 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);
}
NodeRun nodeR = node.GetNodeRun(data);
if (nodeR == null)
{
return(AstarColor.NodeConnection);
}
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);
#else
return(new Color(1, 1, 1));
#endif
}
public void UpdateG (NodeRun nodeR, NodeRunData nodeRunData) {
nodeR.g = nodeR.parent.g+nodeR.cost+penalty + nodeRunData.path.GetTagPenalty(tags);
}
/** Calculates the path until completed or until the time has passed \a targetTick.
* Usually a check is only done every 500 nodes if the time has passed \a targetTick.
* Time/Ticks are got from System.DateTime.UtcNow.Ticks.
*
* Basic outline of what the function does for the standard path (Pathfinding.ABPath).
* \code
* while the end has not been found and no error has ocurred
* check if we have reached the end
* if so, exit and return the path
*
* open the current node, i.e loop through its neighbours, mark them as visited and put them on a heap
*
* check if there are still nodes left to process (or have we searched the whole graph)
* if there are none, flag error and exit
*
* pop the next node of the heap and set it as current
*
* check if the function has exceeded the time limit
* if so, return and wait for the function to get called again
* \endcode
*/
public override void CalculateStep(long targetTick)
{
int counter = 0;
//Continue to search while there hasn't ocurred an error and the end hasn't been found
while (CompleteState == PathCompleteState.NotCalculated)
{
searchedNodes++;
//Close the current node, if the current node is the target node then the path is finished
if (currentR.node == endNode)
{
CompleteState = PathCompleteState.Complete;
break;
}
if (currentR.h < partialBestTarget.h)
{
partialBestTarget = currentR;
}
//Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open(runData, currentR, hTarget, this);
//any nodes left to search?
if (runData.open.numberOfItems <= 1)
{
Error();
LogError("No open points, whole area searched");
return;
}
//Select the node with the lowest F score and remove it from the open list
currentR = runData.open.Remove();
//Check for time every 500 nodes, roughly every 0.5 ms usually
if (counter > 500)
{
//Have we exceded the maxFrameTime, if so we should wait one frame before continuing the search since we don't want the game to lag
if (System.DateTime.UtcNow.Ticks >= targetTick)
{
//Return instead of yield'ing, a separate function handles the yield (CalculatePaths)
return;
}
counter = 0;
}
counter++;
}
AstarProfiler.StartProfile("Trace");
if (CompleteState == PathCompleteState.Complete)
{
Trace(currentR);
}
else if (calculatePartial && partialBestTarget != null)
{
CompleteState = PathCompleteState.Partial;
Trace(partialBestTarget);
}
AstarProfiler.EndProfile();
}
public void FoundTarget(NodeRun nodeR, int i)
{
Node node = nodeR.node;
//Debug.Log ("Found target "+i+" "+current.g+" "+current.f);
node.Bit8 = false;//Reset bit 8
Trace (nodeR);
vectorPaths[i] = vectorPath;
nodePaths[i] = path;
targetsFound[i] = true;
/*for (int j=i;j<targetNodeCount-1;j++) {
targetNodes[j] = targetNodes[j+1];
}*/
targetNodeCount--;
if (!pathsForAll) {
foundEnd = true;
chosenTarget = i; //Mark which path was found
targetNodeCount = 0;
return;
}
//If there are no more targets to find, return here and avoid calculating a new hTarget
if (targetNodeCount <= 0) {
return;
}
//No need to check for if pathsForAll is true since the function would have returned by now then
if (heuristicMode == HeuristicMode.MovingAverage) {
Vector3 avg = Vector3.zero;
int count = 0;
for (int j=0;j<targetPoints.Length;j++) {
if (!targetsFound[j]) {
avg += (Vector3)targetNodes[j].position;
count++;
}
}
if (count > 0) {
avg /= count;
}
hTarget = (Int3)avg;
RebuildOpenList ();
} else if (heuristicMode == HeuristicMode.MovingMidpoint) {
Vector3 min = Vector3.zero;
Vector3 max = Vector3.zero;
bool set = false;
for (int j=0;j<targetPoints.Length;j++) {
if (!targetsFound[j]) {
if (!set) {
min = (Vector3)targetNodes[j].position;
max = (Vector3)targetNodes[j].position;
set = true;
} else {
min = Vector3.Min ((Vector3)targetNodes[j].position,min);
max = Vector3.Max ((Vector3)targetNodes[j].position,max);
}
}
}
Int3 midpoint = (Int3)((min+max)*0.5F);
hTarget = (Int3)midpoint;
RebuildOpenList ();
} else if (heuristicMode == HeuristicMode.Sequential) {
//If this was the target hTarget was set to at the moment
if (sequentialTarget == i) {
//Pick a new hTarget
float dist = 0;
for (int j=0;j<targetPoints.Length;j++) {
if (!targetsFound[j]) {
float d = (targetNodes[j].position-startNode.position).sqrMagnitude;
if (d > dist) {
dist = d;
hTarget = (Int3)targetPoints[j];
sequentialTarget = j;
}
}
}
RebuildOpenList ();
}
}
}
/** Has the ending condition been fulfilled.
* \param node The current node.
* This is per default the same as asking if \a node == \a p.endNode */
public virtual bool TargetFound(NodeRun node)
{
return true;//node.node == p.endNode;
}
/** Opens the nodes connected to this node. This is a base call and can be called by node classes overriding the Open function to open all connections in the #connections array.
* \see #connections
* \see Open */
public void BaseOpen (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) {
if (connections == null) return;
for (int i=0;i<connections.Length;i++) {
Node conNode = connections[i];
if (!path.CanTraverse (conNode)) {
continue;
}
NodeRun nodeR2 = conNode.GetNodeRun (nodeRunData);
if (nodeR2.pathID != nodeRunData.pathID) {
nodeR2.parent = nodeR;
nodeR2.pathID = nodeRunData.pathID;
nodeR2.cost = (uint)connectionCosts[i];
conNode.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2);
conNode.UpdateG (nodeR2, nodeRunData);
nodeRunData.open.Add (nodeR2);
//Debug.DrawLine (position,node.position,Color.cyan);
//Debug.Log ("Opening Node "+node.position.ToString ()+" "+g+" "+node.cost+" "+node.g+" "+node.f);
} 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)connectionCosts[i];
if (nodeR.g+tmpCost+conNode.penalty
#if !ASTAR_NoTagPenalty
+ path.GetTagPenalty(conNode.tags)
#endif
< nodeR2.g) {
nodeR2.cost = tmpCost;
nodeR2.parent = nodeR;
conNode.UpdateAllG (nodeR2,nodeRunData);
nodeRunData.open.Add (nodeR2);
}
else if (nodeR2.g+tmpCost+penalty
#if !ASTAR_NoTagPenalty
+ path.GetTagPenalty(tags)
#endif
< nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better
bool contains = conNode.ContainsConnection (this);
//Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node.
/*if (node.connections != null) {
for (int y=0;y<node.connections.Length;y++) {
if (node.connections[y] == this) {
contains = true;
break;
}
}
}*/
if (!contains) {
continue;
}
nodeR.parent = nodeR2;
nodeR.cost = tmpCost;
UpdateAllG (nodeR,nodeRunData);
nodeRunData.open.Add (nodeR);
}
}
}
}
public override void Initialize()
{
if (hasEndPoint && startNode == endNode) {
NodeRun endNodeR = endNode.GetNodeRun(runData);
endNodeR.parent = null;
endNodeR.h = 0;
endNodeR.g = 0;
Trace (endNodeR);
CompleteState = PathCompleteState.Complete;
return;
}
//Adjust the costs for the end node
/*if (hasEndPoint && recalcStartEndCosts) {
endNodeCosts = endNode.InitialOpen (open,hTarget,(Int3)endPoint,this,false);
callback += ResetCosts; /* \todo Might interfere with other paths since other paths might be calculated before #callback is called *
}*/
NodeRun startRNode = startNode.GetNodeRun (runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH (hTarget,heuristic,heuristicScale, startRNode);
/*if (recalcStartEndCosts) {
startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
} else {*/
startNode.Open (runData,startRNode,hTarget,this);
//}
searchedNodes++;
partialBestTarget = startRNode;
//any nodes left to search?
if (runData.open.numberOfItems <= 1) {
if (calculatePartial) {
CompleteState = PathCompleteState.Partial;
Trace (partialBestTarget);
} else {
Error ();
LogError ("No open points, the start node didn't open any nodes");
return;
}
}
currentR = runData.open.Remove ();
}
/** Traces the calculated path from the end node to the start.
* This will build an array (#path) of the nodes this path will pass through and also set the #vectorPath array to the #path arrays positions.
* Assumes the #vectorPath and #path are empty and not null (which will be the case for a correctly initialized path).
*/
protected virtual void Trace(NodeRun from)
{
int count = 0;
NodeRun c = from;
while (c != null) {
c = c.parent;
count++;
if (count > 1024) {
Debug.LogWarning ("Inifinity loop? >1024 node path. Remove this message if you really have that long paths (Path.cs, Trace function)");
break;
}
}
//Ensure capacities for lists
AstarProfiler.StartProfile ("Check List Capacities");
if (path.Capacity < count) path.Capacity = count;
if (vectorPath.Capacity < count) vectorPath.Capacity = count;
AstarProfiler.EndProfile ();
c = from;
for (int i = 0;i<count;i++) {
path.Add (c.node);
c = c.parent;
}
int half = count/2;
for (int i=0;i<half;i++) {
Node tmp = path[i];
path[i] = path[count-i-1];
path[count - i - 1] = tmp;
}
for (int i=0;i<count;i++) {
vectorPath.Add ((Vector3)path[i].position);
}
}
/** Adds a node to the heap */
public void Add(NodeRun node) {
if (node == null) throw new System.ArgumentNullException ("Sending null node to BinaryHeap");
if (numberOfItems == binaryHeap.Length) {
int newSize = System.Math.Max(binaryHeap.Length+4,(int)System.Math.Round(binaryHeap.Length*growthFactor));
if (newSize > 1<<18) {
throw new System.Exception ("Binary Heap Size really large (2^18). A heap size this large is probably the cause of pathfinding running in an infinite loop. " +
"\nRemove this check (in BinaryHeap.cs) if you are sure that it is not caused by a bug");
}
NodeRun[] tmp = new NodeRun[newSize];
for (int i=0;i<binaryHeap.Length;i++) {
tmp[i] = binaryHeap[i];
}
binaryHeap = tmp;
//Debug.Log ("Forced to discard nodes because of binary heap size limit, please consider increasing the size ("+numberOfItems +" "+binaryHeap.Length+")");
//numberOfItems--;
}
binaryHeap[numberOfItems] = node;
//node.heapIndex = numberOfItems;//Heap index
int bubbleIndex = numberOfItems;
uint nodeF = node.f;
while (bubbleIndex != 1) {
int parentIndex = bubbleIndex / 2;
if (nodeF < binaryHeap[parentIndex].f) {
//binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /* \todo Wouldn't it be more efficient with '<' instead of '<=' ? * /
//Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;//binaryHeap[bubbleIndex];
//binaryHeap[bubbleIndex].heapIndex = bubbleIndex; //Heap index
//binaryHeap[parentIndex].heapIndex = parentIndex; //Heap index
bubbleIndex = parentIndex;
} else {
/*if (binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /* \todo Wouldn't it be more efficient with '<' instead of '<=' ? *
Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[parentIndex] = binaryHeap[bubbleIndex];
binaryHeap[bubbleIndex] = tmpValue;
bubbleIndex = parentIndex;
} else {*/
break;
}
}
numberOfItems++;
}
public override bool TargetFound (NodeRun node) {
return node.g >= maxGScore;
}
/** 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 (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;
runData = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = Pathfinding.Util.ListPool<Node>.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 ();
}
/** Creates the structure for holding temporary path data.
* The data is for example the G, H and F scores and the search tree.
* The number of nodeRuns must be no less than the number of nodes contained in all graphs.
* So after adding nodes, this MUST be called.\n
* Ideally, I would code an update function which reuses most of the previous ones instead of recreating it every time.
* \param numParallel Number of parallel threads which will use the data.
* \see #nodeRuns
* \see AstarPath.UpdatePathThreadInfoNodes
*/
public void CreateNodeRuns(int numParallel)
{
if (graphs == null) throw new System.Exception ("Cannot create NodeRuns when no graphs exist. (Scan and or Load graphs first)");
int nodeCount = 0;
for (int i=0;i<graphs.Length;i++) {
if (graphs[i] != null && graphs[i].nodes != null)
nodeCount += graphs[i].nodes.Length;
}
AssignNodeIndices ();
#if !ASTAR_SINGLE_THREAD_OPTIMIZE
if (nodeRuns == null || nodeRuns.Length != numParallel) nodeRuns = new NodeRun[numParallel][];
for (int i=0;i<nodeRuns.Length;i++) {
if (nodeRuns[i] == null || nodeRuns[i].Length != nodeCount) nodeRuns[i] = new NodeRun[nodeCount];
else Debug.Log ("Saved some allocations");
int counter = 0;
for (int j=0;j<graphs.Length;j++) {
if (graphs[j] == null) continue;
Node[] nodes = graphs[j].nodes;
if (nodes == null) continue;
for (int q=0;q<nodes.Length;q++, counter++) {
if (nodes[q] != null) {
if (nodeRuns[i][counter] == null) nodeRuns[i][counter] = new NodeRun ();
else nodeRuns[i][counter].Reset();
nodeRuns[i][counter].Link (nodes[q],counter);
}
}
}
}
#endif
active.UpdatePathThreadInfoNodes ();
}
public override void CalculateStep(long targetTick)
{
int counter = 0;
//Continue to search while there hasn't ocurred an error and the end hasn't been found
while (!IsDone()) {
//@Performance Just for debug info
searchedNodes++;
//Close the current node, if the current node is the target node then the path is finnished
if (currentR.g >= searchLength) {
if (chosenNodeR == null) {
chosenNodeR = currentR;
} else if (rnd.NextDouble () < replaceChance) {
chosenNodeR = currentR;
}
if (currentR.g >= searchLength+spread) {
CompleteState = PathCompleteState.Complete;
break;
}
} else if (currentR.g > maxGScore) {
maxGScore = (int)currentR.g;
maxGScoreNodeR = currentR;
}
//Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open (runData,currentR, hTarget,this);
//any nodes left to search?
if (runData.open.numberOfItems <= 1) {
if (chosenNodeR != null) {
CompleteState = PathCompleteState.Complete;
} else if (maxGScoreNodeR != null) {
chosenNodeR = maxGScoreNodeR;
CompleteState = PathCompleteState.Complete;
} else {
LogError ("Not a single node found to search");
Error ();
}
break;
}
//Select the node with the lowest F score and remove it from the open list
currentR = runData.open.Remove ();
//Check for time every 500 nodes, roughly every 0.5 ms usually
if (counter > 500) {
//Have we exceded the maxFrameTime, if so we should wait one frame before continuing the search since we don't want the game to lag
if (System.DateTime.UtcNow.Ticks >= targetTick) {
//Return instead of yield'ing, a separate function handles the yield (CalculatePaths)
return;
}
counter = 0;
}
counter++;
}
if (CompleteState == PathCompleteState.Complete) {
Trace (chosenNodeR);
}
//Return instead of yielding, a separate function handles the yield (CalculatePaths or CalculatePathsThreaded in AstarPath.cs)
return;
}
public override void Reset()
{
base.Reset ();
searchLength = 5000;
spread = 5000;
uniform = true;
replaceChance = 0.1F;
aimStrength = 0.0f;
chosenNodeR = null;
maxGScoreNodeR = null;
maxGScore = 0;
aim = Vector3.zero;
hasEndPoint = false;
}
public new override void Open(NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path)
{
BaseOpen (nodeRunData, nodeR, targetPosition, path);
LayerGridGraph graph = gridGraphs[indices >> 24];
int[] neighbourOffsets = graph.neighbourOffsets;
int[] neighbourCosts = graph.neighbourCosts;
Node[] nodes = graph.nodes;
int index = GetIndex();//indices & 0xFFFFFF;
for (int i=0;i<4;i++) {
int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF;
if (conn != LevelGridNode.NoConnection) {
Node node = nodes[index+neighbourOffsets[i] + graph.width*graph.depth*conn];
if (!path.CanTraverse (node)) {
continue;
}
NodeRun nodeR2 = node.GetNodeRun (nodeRunData);
if (nodeR2.pathID != nodeRunData.pathID) {
nodeR2.parent = nodeR;
nodeR2.pathID = nodeRunData.pathID;
nodeR2.cost = (uint)neighbourCosts[i];
node.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2);
node.UpdateG (nodeR2, nodeRunData);
nodeRunData.open.Add (nodeR2);
} 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)neighbourCosts[i];
if (nodeR.g+tmpCost+node.penalty
#if !NoTagPenalty
+ path.GetTagPenalty(node.tags)
#endif
< nodeR2.g) {
nodeR2.cost = tmpCost;
nodeR2.parent = nodeR;
//TODO!!!!! ??
node.UpdateAllG (nodeR2,nodeRunData);
nodeRunData.open.Add (nodeR2);
}
else if (nodeR2.g+tmpCost+penalty
#if !NoTagPenalty
+ path.GetTagPenalty(tags)
#endif
< nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better
bool contains = node.ContainsConnection (this);
//Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node.
/*if (node.connections != null) {
for (int y=0;y<node.connections.Length;y++) {
if (node.connections[y] == this) {
contains = true;
break;
}
}
}*/
if (!contains) {
continue;
}
nodeR.parent = nodeR2;
nodeR.cost = tmpCost;
//TODO!!!!!!! ??
UpdateAllG (nodeR,nodeRunData);
nodeRunData.open.Add (nodeR);
}
}
}
}
}
/** Has the ending condition been fulfilled.
* \param node The current node.
* This is per default the same as asking if \a node == \a p.endNode */
public virtual bool TargetFound(NodeRun node)
{
return(true); //node.node == p.endNode;
}
public override bool TargetFound(NodeRun node)
{
return node.g >= maxGScore;
}
/** Has the ending condition been fulfilled.
* \param node The current node.
* This is per default the same as asking if \a node == \a p.endNode */
public override bool TargetFound(NodeRun node)
{
return(node.node == abPath.endNode);
}
/** Calculates the path until completed or until the time has passed \a targetTick.
* Usually a check is only done every 500 nodes if the time has passed \a targetTick.
* Time/Ticks are got from System.DateTime.UtcNow.Ticks.
*
* Basic outline of what the function does for the standard path (Pathfinding.ABPath).
\code
while the end has not been found and no error has ocurred
check if we have reached the end
if so, exit and return the path
open the current node, i.e loop through its neighbours, mark them as visited and put them on a heap
check if there are still nodes left to process (or have we searched the whole graph)
if there are none, flag error and exit
pop the next node of the heap and set it as current
check if the function has exceeded the time limit
if so, return and wait for the function to get called again
\endcode
*/
public override void CalculateStep(long targetTick)
{
int counter = 0;
//Continue to search while there hasn't ocurred an error and the end hasn't been found
while (CompleteState == PathCompleteState.NotCalculated) {
searchedNodes++;
//Close the current node, if the current node is the target node then the path is finished
if (currentR.node == endNode) {
CompleteState = PathCompleteState.Complete;
break;
}
if (currentR.h < partialBestTarget.h) {
partialBestTarget = currentR;
}
//Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open (runData,currentR, hTarget,this);
//any nodes left to search?
if (runData.open.numberOfItems <= 1) {
Error ();
LogError ("No open points, whole area searched");
return;
}
//Select the node with the lowest F score and remove it from the open list
currentR = runData.open.Remove ();
//Check for time every 500 nodes, roughly every 0.5 ms usually
if (counter > 500) {
//Have we exceded the maxFrameTime, if so we should wait one frame before continuing the search since we don't want the game to lag
if (System.DateTime.UtcNow.Ticks >= targetTick) {
//Return instead of yield'ing, a separate function handles the yield (CalculatePaths)
return;
}
counter = 0;
}
counter++;
}
AstarProfiler.StartProfile ("Trace");
if (CompleteState == PathCompleteState.Complete) {
Trace (currentR);
} else if (calculatePartial && partialBestTarget != null) {
CompleteState = PathCompleteState.Partial;
Trace (partialBestTarget);
}
AstarProfiler.EndProfile ();
}
public override void UpdateAllG (NodeRun nodeR, NodeRunData nodeRunData) {
BaseUpdateAllG (nodeR, nodeRunData);
int index = GetIndex ();
int[] neighbourOffsets = gridGraphs[indices >> 24].neighbourOffsets;
Node[] nodes = gridGraphs[indices >> 24].nodes;
for (int i=0;i<8;i++) {
if (GetConnection (i)) {
//if (((flags >> i) & 1) == 1) {
Node node = nodes[index+neighbourOffsets[i]];
NodeRun nodeR2 = node.GetNodeRun (nodeRunData);
if (nodeR2.parent == nodeR && nodeR2.pathID == nodeRunData.pathID) {
node.UpdateAllG (nodeR2,nodeRunData);
}
}
}
}
/** Reset all values to their default values.
* All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths.
* Call this base function in inheriting types with base.Reset ();
*/
public override void Reset()
{
base.Reset ();
startNode = null;
endNode = null;
startHint = null;
endHint = null;
originalStartPoint = Vector3.zero;
originalEndPoint = Vector3.zero;
startPoint = Vector3.zero;
endPoint = Vector3.zero;
calculatePartial = false;
partialBestTarget = null;
hasEndPoint = true;
startIntPoint = new Int3();
hTarget = new Int3();
endNodeCosts = null;
}
public override void Open (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) {
BaseOpen (nodeRunData, nodeR, targetPosition, path);
GridGraph graph = gridGraphs[indices >> 24];
int[] neighbourOffsets = graph.neighbourOffsets;
int[] neighbourCosts = graph.neighbourCosts;
Node[] nodes = graph.nodes;
int index = GetIndex ();//indices & 0xFFFFFF;
for (int i=0;i<8;i++) {
if (GetConnection (i)) {
//if (((flags >> i) & 1) == 1) {
Node node = nodes[index+neighbourOffsets[i]];
if (!path.CanTraverse (node)) continue;
NodeRun nodeR2 = node.GetNodeRun (nodeRunData);
if (nodeR2.pathID != nodeRunData.pathID) {
nodeR2.parent = nodeR;
nodeR2.pathID = nodeRunData.pathID;
nodeR2.cost = (uint)neighbourCosts[i];
node.UpdateH (targetPosition,path.heuristic,path.heuristicScale, nodeR2);
node.UpdateG (nodeR2,nodeRunData);
nodeRunData.open.Add (nodeR2);
} 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)neighbourCosts[i];//(current.costs == null || current.costs.Length == 0 ? costs[current.neighboursKeys[i]] : current.costs[current.neighboursKeys[i]]);
if (nodeR.g+tmpCost+node.penalty
+ path.GetTagPenalty(node.tags)
< nodeR2.g) {
nodeR2.cost = tmpCost;
nodeR2.parent = nodeR;
node.UpdateAllG (nodeR2,nodeRunData);
}
else if (nodeR2.g+tmpCost+penalty
+ path.GetTagPenalty(tags)
< nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better
/*bool contains = false;
//[Edit, no one-way links between nodes in a single grid] Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be accesed from the Node.
/*for (int y=0;y<node.connections.Length;y++) {
if (node.connections[y].endNode == this) {
contains = true;
break;
}
}
if (!contains) {
continue;
}*/
nodeR.parent = nodeR2;
nodeR.cost = tmpCost;
UpdateAllG (nodeR,nodeRunData);
}
}
}
}
}
public override void Trace(NodeRun node)
{
base.Trace (node);
if (inverted) {
//Invert the paths
int half = path.Length/2;
for (int i=0;i<half;i++) {
Node tmp = path[i];
path[i] = path[path.Length-i-1];
path[path.Length-i-1] = tmp;
}
for (int i=0;i<half;i++) {
Vector3 tmp = vectorPath[i];
vectorPath[i] = vectorPath[vectorPath.Length-i-1];
vectorPath[vectorPath.Length-i-1] = tmp;
}
}
}
/* F score. The F score is the #g score + #h score, that is the cost it taken to move to this node from the start + the estimated cost to move to the end node.\n
* Nodes are sorted by their F score, nodes with lower F scores are opened first */
/*public uint f {
get {
return g+h;
}
}*/
/** Calculates and updates the H score.
* Calculates the H score with respect to the target position and chosen heuristic.
* \param targetPosition The position to calculate the distance to.
* \param heuristic Heuristic to use. The heuristic can also be hard coded using pre processor directives (check sourcecode)
* \param scale Scale of the heuristic
* \param nodeR NodeRun object associated with this node.
*/
public void UpdateH (Int3 targetPosition, Heuristic heuristic, float scale, NodeRun nodeR) {
//Choose the correct heuristic, compute it and store it in the \a h variable
if (heuristic == Heuristic.None) {
nodeR.h = 0;
return;
}
#if AstarFree || !ASTAR_DefinedHeuristic
if (heuristic == Heuristic.Euclidean) {
nodeR.h = (uint)Mathfx.RoundToInt ((position-targetPosition).magnitude*scale);
} else if (heuristic == Heuristic.Manhattan) {
nodeR.h = (uint)Mathfx.RoundToInt (
(Abs (position.x-targetPosition.x) +
Abs (position.y-targetPosition.y) +
Abs (position.z-targetPosition.z))
* scale
);
} else { //if (heuristic == Heuristic.DiagonalManhattan) {
int xDistance = Abs (position.x-targetPosition.x);
int zDistance = Abs (position.z-targetPosition.z);
if (xDistance > zDistance) {
nodeR.h = (uint)(14*zDistance + 10*(xDistance-zDistance))/10;
} else {
nodeR.h = (uint)(14*xDistance + 10*(zDistance-xDistance))/10;
}
nodeR.h = (uint)Mathfx.RoundToInt (nodeR.h * scale);
}
#elif ASTAR_NoHeuristic
nodeR.h = 0;
#elif ASTAR_ManhattanHeuristic
nodeR.h = (uint)Mathfx.RoundToInt (
(Abs (position.x-targetPosition.x) +
Abs (position.y-targetPosition.y) +
Abs (position.z-targetPosition.z))
* scale
);
#elif ASTAR_DiagonalManhattanHeuristic
int xDistance = Abs (position.x-targetPosition.x);
int zDistance = Abs (position.z-targetPosition.z);
if (xDistance > zDistance) {
nodeR.h = (uint)(14*zDistance + 10*(xDistance-zDistance))/10;
} else {
nodeR.h = (uint)(14*xDistance + 10*(zDistance-xDistance))/10;
}
nodeR.h = (uint)Mathfx.RoundToInt (nodeR.h * scale);
#elif ASTAR_EucledianHeuristic
nodeR.h = (uint)Mathfx.RoundToInt ((position-targetPosition).magnitude*scale);
#endif
}
/** Has the ending condition been fulfilled.
* \param node The current node.
* This is per default the same as asking if \a node == \a p.endNode */
public override bool TargetFound(NodeRun node)
{
return node.node == abPath.endNode;
}
public void UpdateG (NodeRun nodeR, NodeRunData nodeRunData) {
nodeR.g = nodeR.parent.g+nodeR.cost+penalty
#if !ASTAR_NoTagPenalty
+ nodeRunData.path.GetTagPenalty(tags)
#endif
;
}
public virtual void Reset(Vector3 start, Vector3 end, OnPathDelegate callbackDelegate, bool reset)
{
if (reset) {
processed = false;
vectorPath = null;
path = null;
next = null;
foundEnd = false;
error = false;
errorLog = "";
callback = null;
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
startHint = null;
endHint = null;
}
callTime = System.DateTime.Now;
callback = callbackDelegate;
/*if (AstarPath.active == null || AstarPath.active.graphs == null) {
errorLog += "No NavGraphs have been calculated yet - Don't run any pathfinding calls in Awake";
if (AstarPath.active.logPathResults != PathLog.None) {
Debug.LogWarning (errorLog);
}
error = true;
return;
}*/
pathID = AstarPath.active.GetNextPathID ();
UpdateStartEnd (start,end);
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
}
protected void BaseUpdateAllG (NodeRun nodeR, NodeRunData nodeRunData) {
UpdateG (nodeR, nodeRunData);
nodeRunData.open.Add (nodeR);
if (connections == null) {
return;
}
//Loop through the connections of this node and call UpdateALlG on nodes which have this node set as #parent and has been searched by the pathfinder for this path */
for (int i=0;i<connections.Length;i++) {
NodeRun otherR = connections[i].GetNodeRun (nodeRunData);
if (otherR.parent == nodeR && otherR.pathID == nodeRunData.pathID) {
connections[i].UpdateAllG (otherR, nodeRunData);
}
}
}
/* String builder used for all debug logging */
//public static System.Text.StringBuilder debugStringBuilder = new System.Text.StringBuilder ();
/** Returns a debug string for this path.
*/
public override string DebugString(PathLog logMode)
{
if (logMode == PathLog.None || (!error && logMode == PathLog.OnlyErrors))
{
return("");
}
//debugStringBuilder.Length = 0;
System.Text.StringBuilder text = new System.Text.StringBuilder();
text.Append(error ? "Path Failed : " : "Path Completed : ");
text.Append("Computation Time ");
text.Append((duration).ToString(logMode == PathLog.Heavy ? "0.000" : "0.00"));
text.Append(" ms Searched Nodes ");
text.Append(searchedNodes);
if (!error)
{
text.Append(" Path Length ");
text.Append(path == null ? "Null" : path.Count.ToString());
if (logMode == PathLog.Heavy)
{
text.Append("\nSearch Iterations " + searchIterations);
if (hasEndPoint && endNode != null)
{
NodeRun nodeR = endNode.GetNodeRun(runData);
text.Append("\nEnd Node\n G: ");
text.Append(nodeR.g);
text.Append("\n H: ");
text.Append(nodeR.h);
text.Append("\n F: ");
text.Append(nodeR.f);
text.Append("\n Point: ");
text.Append(((Vector3)endPoint).ToString());
text.Append("\n Graph: ");
text.Append(endNode.graphIndex);
}
text.Append("\nStart Node");
text.Append("\n Point: ");
text.Append(((Vector3)startPoint).ToString());
text.Append("\n Graph: ");
text.Append(startNode.graphIndex);
text.Append("\nBinary Heap size at completion: ");
text.Append(runData.open == null ? "Null" : (runData.open.numberOfItems - 2).ToString()); // -2 because numberOfItems includes the next item to be added and item zero is not used
}
/*"\nEnd node\n G = "+p.endNode.g+"\n H = "+p.endNode.h+"\n F = "+p.endNode.f+"\n Point "+p.endPoint
+"\nStart Point = "+p.startPoint+"\n"+"Start Node graph: "+p.startNode.graphIndex+" End Node graph: "+p.endNode.graphIndex+
* "\nBinary Heap size at completion: "+(p.open == null ? "Null" : p.open.numberOfItems.ToString ())*/
}
if (error)
{
text.Append("\nError: ");
text.Append(errorLog);
}
text.Append("\nPath Number ");
text.Append(pathID);
return(text.ToString());
}
public virtual void UpdateAllG (NodeRun nodeR, NodeRunData nodeRunData) {
BaseUpdateAllG (nodeR, nodeRunData);
}
/** Initializes the path. Sets up the open list and adds the first node to it */
public virtual void Initialize()
{
runData.pathID = pathID;
//Resets the binary heap, don't clear everything because that takes an awful lot of time, instead we can just change the numberOfItems in it (which is just an int)
//Binary heaps are just like a standard array but are always sorted so the node with the lowest F value can be retrieved faster
runData.open.Clear ();
if (hasEndPoint && startNode == endNode) {
NodeRun endNodeR = endNode.GetNodeRun(runData);
endNodeR.parent = null;
endNodeR.h = 0;
endNodeR.g = 0;
Trace (endNodeR);
foundEnd = true;
return;
}
//Adjust the costs for the end node
/*if (hasEndPoint && recalcStartEndCosts) {
endNodeCosts = endNode.InitialOpen (open,hTarget,(Int3)endPoint,this,false);
callback += ResetCosts; /* \todo Might interfere with other paths since other paths might be calculated before #callback is called *
}*/
//Node.activePath = this;
NodeRun startRNode = startNode.GetNodeRun (runData);
startRNode.pathID = pathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.g = startNode.penalty;
startNode.UpdateH (hTarget,heuristic,heuristicScale, startRNode);
/*if (recalcStartEndCosts) {
startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
} else {*/
startNode.Open (runData,startRNode,hTarget,this);
//}
searchedNodes++;
//any nodes left to search?
if (runData.open.numberOfItems <= 1) {
LogError ("No open points, the start node didn't open any nodes");
return;
}
currentR = runData.open.Remove ();
}
public virtual void Open (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) {
BaseOpen (nodeRunData,nodeR, targetPosition,path);
}
public new override void UpdateAllG(NodeRun nodeR, NodeRunData nodeRunData)
{
BaseUpdateAllG (nodeR, nodeRunData);
//Called in the base function
//open.Add (this);
int index = GetIndex ();//indices & 0xFFFFFF;
LayerGridGraph graph = gridGraphs[indices >> 24];
int[] neighbourOffsets = graph.neighbourOffsets;
Node[] nodes = graph.nodes;
//int[] nodeCellIndices = gridGraphs[indices >> 24].nodeCellIndices;
for (int i=0;i<4;i++) {
int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF;
if (conn != LevelGridNode.NoConnection) {
Node node = nodes[index+neighbourOffsets[i] + graph.width*graph.depth*conn];
//nodes[nodeCellIndices[index+neighbourOffsets[i]]+conn];
//nodes[index+neighbourOffsets[i]+conn];
NodeRun nodeR2 = node.GetNodeRun(nodeRunData);
if (nodeR2.parent == nodeR && nodeR2.pathID == nodeRunData.pathID) {
node.UpdateAllG (nodeR2,nodeRunData);
}
}
}
}
/** Creates the structure for holding temporary path data.
* The data is for example the G, H and F scores and the search tree.
* The number of nodeRuns must be no less than the number of nodes contained in all graphs.
* So after adding nodes, this MUST be called.\n
* Ideally, I would code an update function which reuses most of the previous ones instead of recreating it every time.
* \param numParallel Number of parallel threads which will use the data.
* \see #nodeRuns
* \see AstarPath::UpdatePathThreadInfoNodes
*/
public void CreateNodeRuns(int numParallel)
{
if (graphs == null) throw new System.Exception ("Cannot create NodeRuns when no graphs exist. (Scan and or Load graphs first)");
int nodeCount = 0;
for (int i=0;i<graphs.Length;i++) {
if (graphs[i].nodes != null)
nodeCount += graphs[i].nodes.Length;
}
AssignNodeIndices ();
#if !SingleCoreOptimize
nodeRuns = new NodeRun[numParallel][];
for (int i=0;i<nodeRuns.Length;i++) {
nodeRuns[i] = new NodeRun[nodeCount];
int counter = 0;
for (int j=0;j<graphs.Length;j++) {
Node[] nodes = graphs[j].nodes;
if (nodes == null) continue;
for (int q=0;q<nodes.Length;q++, counter++) {
if (nodes[q] != null) {
nodeRuns[i][counter] = new NodeRun ();
nodeRuns[i][counter].Link (nodes[q],counter);
}
}
}
}
#endif
active.UpdatePathThreadInfoNodes ();
}
