public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
UpdateG (path,pathNode);
handler.PushNode (pathNode);
for (var i=0;i<connections.Length;i++) {
var other = connections[i];
var otherPN = handler.GetPathNode (other);
if (otherPN.parent == pathNode && otherPN.pathID == handler.PathID) {
other.UpdateRecursiveG (path, otherPN,handler);
}
}
}
public override void Reset () {
base.Reset();
searchLength = 5000;
spread = 5000;
uniform = true;
aimStrength = 0.0f;
chosenNodeR = null;
maxGScoreNodeR = null;
maxGScore = 0;
aim = Vector3.zero;
nodesEvaluatedRep = 0;
}
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 Tuple(uint F, PathNode node)
{
this.F = F;
this.node = node;
}
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
var gg = GetGridGraph (GraphIndex);
var neighbourOffsets = gg.neighbourOffsets;
var nodes = gg.nodes;
UpdateG (path,pathNode);
handler.PushNode (pathNode);
var pid = handler.PathID;
for (var i=0;i<8;i++) {
if (GetConnectionInternal(i)) {
var other = nodes[nodeInGridIndex + neighbourOffsets[i]];
var otherPN = handler.GetPathNode (other);
if (otherPN.parent == pathNode && otherPN.pathID == pid) other.UpdateRecursiveG (path, otherPN,handler);
}
}
}
public override void Open (Path path, PathNode pathNode, PathHandler handler) {
throw new System.NotImplementedException();
}
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
//BaseUpdateAllG (nodeR, nodeRunData);
handler.PushNode (pathNode);
UpdateG (path, pathNode);
LayerGridGraph graph = GetGridGraph (GraphIndex);
int[] neighbourOffsets = graph.neighbourOffsets;
LevelGridNode[] nodes = graph.nodes;
int index = NodeInGridIndex;
for (int i=0;i<4;i++) {
int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF;
if (conn != LevelGridNode.NoConnection) {
LevelGridNode other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn];
PathNode otherPN = handler.GetPathNode (other);
if (otherPN != null && otherPN.parent == pathNode && otherPN.pathID == handler.PathID) {
other.UpdateRecursiveG (path, otherPN,handler);
}
}
}
}
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
UpdateG(path, pathNode);
handler.PushNode(pathNode);
if (connections == null) return;
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);
}
}
/** Open the node */ public abstract void Open (Path path, PathNode pathNode, PathHandler handler);
/// <summary>Has the ending condition been fulfilled.</summary> /// <param name="node">The current node.</param> public abstract bool TargetFound(PathNode node);
/** Reset all values to their default values.
* All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths.
* Call this base function in inheriting types with base.Reset ();
*/
public override void Reset () {
base.Reset ();
startNode = null;
endNode = null;
startHint = null;
endHint = null;
originalStartPoint = Vector3.zero;
originalEndPoint = Vector3.zero;
startPoint = Vector3.zero;
endPoint = Vector3.zero;
calculatePartial = false;
partialBestTarget = null;
startIntPoint = new Int3();
hTarget = new Int3();
endNodeCosts = null;
#if !ASTAR_NO_GRID_GRAPH
gridSpecialCaseNode = null;
#endif
}
/// <summary>
/// Calculates the path until completed or until the time has passed targetTick.
/// Usually a check is only done every 500 nodes if the time has passed 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 occurred
/// 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
/// </code>
/// </summary>
protected override void CalculateStep(long targetTick)
{
int counter = 0;
// Continue to search as long as we haven't encountered an error and we haven't found the target
while (CompleteState == PathCompleteState.NotCalculated)
{
searchedNodes++;
// Close the current node, if the current node is the target node then the path is finished
if (currentR.flag1)
{
// We found a target point
// Mark that node as the end point
CompleteWith(currentR.node);
break;
}
if (currentR.H < partialBestTarget.H)
{
partialBestTarget = currentR;
}
AstarProfiler.StartFastProfile(4);
// Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open(this, currentR, pathHandler);
AstarProfiler.EndFastProfile(4);
// Any nodes left to search?
if (pathHandler.heap.isEmpty)
{
if (calculatePartial && partialBestTarget != null)
{
CompleteState = PathCompleteState.Partial;
Trace(partialBestTarget);
}
else
{
FailWithError("Searched whole area but could not find target");
}
return;
}
// Select the node with the lowest F score and remove it from the open list
AstarProfiler.StartFastProfile(7);
currentR = pathHandler.heap.Remove();
AstarProfiler.EndFastProfile(7);
// 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;
// Mostly for development
if (searchedNodes > 1000000)
{
throw new System.Exception("Probable infinite loop. Over 1,000,000 nodes searched");
}
}
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 Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
ushort pid = handler.PathID;
#if ASTAR_JPS
if (gg.useJumpPointSearch && !path.FloodingPath)
{
JPSOpen(path, pathNode, handler);
}
else
#endif
{
int[] neighbourOffsets = gg.neighbourOffsets;
uint[] neighbourCosts = gg.neighbourCosts;
GridNode[] nodes = gg.nodes;
var index = NodeInGridIndex;
for (int i = 0; i < 8; i++)
{
if (HasConnectionInDirection(i))
{
GridNode other = nodes[index + neighbourOffsets[i]];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
uint tmpCost = neighbourCosts[i];
// Check if the other node has not yet been visited by this path
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = tmpCost;
otherPN.H = path.CalculateHScore(other);
otherPN.UpdateG(path);
handler.heap.Add(otherPN);
}
else
{
// Sorry for the huge number of #ifs
//If not we can test if the path from the current node to this one is a better one then the one already used
#if ASTAR_NO_TRAVERSAL_COST
if (pathNode.G + tmpCost < otherPN.G)
#else
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#endif
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
}
}
}
}
}
#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
base.Open(path, pathNode, handler);
#endif
}
public override void CalculateStep(long targetTick)
{
for (int i = 0; base.CompleteState == PathCompleteState.NotCalculated; i++)
{
base.searchedNodes++;
if (base.currentR.G >= this.searchLength)
{
if (base.currentR.G <= (this.searchLength + this.spread))
{
this.nodesEvaluatedRep++;
if (this.rnd.NextDouble() <= (1f / ((float)this.nodesEvaluatedRep)))
{
this.chosenNodeR = base.currentR;
}
goto Label_00E6;
}
if (this.chosenNodeR == null)
{
this.chosenNodeR = base.currentR;
}
base.CompleteState = PathCompleteState.Complete;
break;
}
if (base.currentR.G > this.maxGScore)
{
this.maxGScore = (int)base.currentR.G;
this.maxGScoreNodeR = base.currentR;
}
Label_00E6:
base.currentR.node.Open(this, base.currentR, base.pathHandler);
if (base.pathHandler.heap.isEmpty)
{
if (this.chosenNodeR != null)
{
base.CompleteState = PathCompleteState.Complete;
}
else if (this.maxGScoreNodeR != null)
{
this.chosenNodeR = this.maxGScoreNodeR;
base.CompleteState = PathCompleteState.Complete;
}
else
{
base.Error();
}
break;
}
base.currentR = base.pathHandler.heap.Remove();
if (i > 500)
{
if (DateTime.UtcNow.Ticks >= targetTick)
{
return;
}
i = 0;
if (base.searchedNodes > 0xf4240)
{
throw new Exception("Probable infinite loop. Over 1,000,000 nodes searched");
}
}
}
if (base.CompleteState == PathCompleteState.Complete)
{
this.Trace(this.chosenNodeR);
}
}
internal override string DebugString (PathLog logMode) {
if (logMode == PathLog.None || (!error && logMode == PathLog.OnlyErrors)) {
return "";
}
System.Text.StringBuilder text = pathHandler.DebugStringBuilder;
text.Length = 0;
DebugStringPrefix(logMode, text);
if (!error) {
text.Append("\nShortest path was ");
text.Append(chosenTarget == -1 ? "undefined" : nodePaths[chosenTarget].Count.ToString());
text.Append(" nodes long");
if (logMode == PathLog.Heavy) {
text.Append("\nPaths (").Append(targetsFound.Length).Append("):");
for (int i = 0; i < targetsFound.Length; i++) {
text.Append("\n\n Path ").Append(i).Append(" Found: ").Append(targetsFound[i]);
if (nodePaths[i] != null) {
text.Append("\n Length: ");
text.Append(nodePaths[i].Count);
GraphNode node = nodePaths[i][nodePaths[i].Count-1];
if (node != null) {
PathNode nodeR = pathHandler.GetPathNode(endNode);
if (nodeR != null) {
text.Append("\n End Node");
text.Append("\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);
} else {
text.Append("\n End Node: Null");
}
}
}
}
text.Append("\nStart Node");
text.Append("\n Point: ");
text.Append(((Vector3)endPoint).ToString());
text.Append("\n Graph: ");
text.Append(startNode.GraphIndex);
text.Append("\nBinary Heap size at completion: ");
text.AppendLine(pathHandler.heap == null ? "Null" : (pathHandler.heap.numberOfItems-2).ToString()); // -2 because numberOfItems includes the next item to be added and item zero is not used
}
}
DebugStringSuffix(logMode, text);
return text.ToString();
}
/** Internal method to initialize node data */
public void InitializeNode(GraphNode node)
{
//Get the index of the node
int ind = node.NodeIndex;
int bucketNumber = ind >> BucketSizeLog2;
int bucketIndex = ind & BucketIndexMask;
if (bucketNumber >= nodes.Length)
{
// A resize is required
// At least increase the size to:
// Current size * 1.5
// Current size + 2 or
// bucketNumber+1
var newNodes = new PathNode[System.Math.Max(System.Math.Max(nodes.Length * 3 / 2, bucketNumber + 1), nodes.Length + 2)][];
for (int i = 0; i < nodes.Length; i++)
{
newNodes[i] = nodes[i];
}
var newBucketNew = new bool[newNodes.Length];
for (int i = 0; i < nodes.Length; i++)
{
newBucketNew[i] = bucketNew[i];
}
var newBucketCreated = new bool[newNodes.Length];
for (int i = 0; i < nodes.Length; i++)
{
newBucketCreated[i] = bucketCreated[i];
}
nodes = newNodes;
bucketNew = newBucketNew;
bucketCreated = newBucketCreated;
}
if (nodes[bucketNumber] == null)
{
PathNode[] ns;
if (bucketCache.Count > 0)
{
ns = bucketCache.Pop();
}
else
{
ns = new PathNode[BucketSize];
for (int i = 0; i < BucketSize; i++)
{
ns[i] = new PathNode();
}
}
nodes[bucketNumber] = ns;
if (!bucketCreated[bucketNumber])
{
bucketNew[bucketNumber] = true;
bucketCreated[bucketNumber] = true;
}
filledBuckets++;
}
PathNode pn = nodes[bucketNumber][bucketIndex];
pn.node = node;
}
/** 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;
}
/// <summary>Has the ending condition been fulfilled.</summary>
/// <param name="node">The current node.
/// This is per default the same as asking if node == p.endNode</param>
public override bool TargetFound(PathNode node)
{
return(node.node == abPath.endNode);
}
/** 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 (PathNode node) {
return true;//node.node == p.endNode;
}
public override bool TargetFound(PathNode node)
{
return(((Vector3)node.node.position - abPath.originalEndPoint).sqrMagnitude <= maxDistance * maxDistance);
}
/** 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.flag1) {
// We found a target point
// Mark that node as the end point
CompleteWith(currentR.node);
break;
}
if (currentR.H < partialBestTarget.H) {
partialBestTarget = currentR;
}
AstarProfiler.StartFastProfile (4);
// Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open (this, currentR, pathHandler);
AstarProfiler.EndFastProfile (4);
// Any nodes left to search?
if (pathHandler.HeapEmpty()) {
Error ();
LogError ("Searched whole area but could not find target");
return;
}
// Select the node with the lowest F score and remove it from the open list
AstarProfiler.StartFastProfile (7);
currentR = pathHandler.PopNode ();
AstarProfiler.EndFastProfile (7);
// 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;
// Mostly for development
if (searchedNodes > 1000000) {
throw new System.Exception ("Probable infinite loop. Over 1,000,000 nodes searched");
}
}
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 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 void UpdateG (Path path, PathNode pathNode) {
#if ASTAR_NO_TRAVERSAL_COST
pathNode.G = pathNode.parent.G + pathNode.cost;
#else
pathNode.G = pathNode.parent.G + pathNode.cost + path.GetTraversalCost(this);
#endif
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
if (connections == null)
{
return;
}
// Flag2 indicates if this node needs special treatment
// with regard to connection costs
bool flag2 = pathNode.flag2;
// Loop through all connections
for (int i = connections.Length - 1; i >= 0; i--)
{
GraphNode other = connections[i];
// Make sure we can traverse the neighbour
if (path.CanTraverse(other))
{
PathNode pathOther = handler.GetPathNode(other);
// Fast path out, worth it for triangle mesh nodes since they usually have degree 2 or 3
if (pathOther == pathNode.parent)
{
continue;
}
uint cost = connectionCosts[i];
if (flag2 || pathOther.flag2)
{
// Get special connection cost from the path
// This is used by the start and end nodes
cost = path.GetConnectionSpecialCost(this, other, cost);
}
// Test if we have seen the other node before
if (pathOther.pathID != handler.PathID)
{
// We have not seen the other node before
// So the path from the start through this node to the other node
// must be the shortest one so far
// Might not be assigned
pathOther.node = other;
pathOther.parent = pathNode;
pathOther.pathID = handler.PathID;
pathOther.cost = cost;
pathOther.H = path.CalculateHScore(other);
other.UpdateG(path, pathOther);
handler.PushNode(pathOther);
}
else
{
// If not we can test if the path from this node to the other one is a better one than the one already used
if (pathNode.G + cost + path.GetTraversalCost(other) < pathOther.G)
{
pathOther.cost = cost;
pathOther.parent = pathNode;
other.UpdateRecursiveG(path, pathOther, handler);
}
else if (pathOther.G + cost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this))
{
// Or if the path from the other node to this one is better
pathNode.parent = pathOther;
pathNode.cost = cost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
uint[] neighbourCosts = gg.neighbourCosts;
GridNode[] nodes = gg.nodes;
ushort pid = handler.PathID;
for (int i = 0; i < 8; i++)
{
if (GetConnectionInternal(i))
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = neighbourCosts[i];
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
//If not we can test if the path from the current node to this one is a better one then the one already used
uint tmpCost = neighbourCosts[i];
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
throw new System.NotImplementedException();
}
/** 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
}
}
/** 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
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gridGraph = GridNode.GetGridGraph(base.GraphIndex);
ushort pathID = handler.PathID;
int[] neighbourOffsets = gridGraph.neighbourOffsets;
uint[] neighbourCosts = gridGraph.neighbourCosts;
GridNode[] nodes = gridGraph.nodes;
for (int i = 0; i < 8; i++)
{
if (this.GetConnectionInternal(i))
{
GridNode gridNode = nodes[this.nodeInGridIndex + neighbourOffsets[i]];
if (path.CanTraverse(gridNode))
{
PathNode pathNode2 = handler.GetPathNode(gridNode);
uint num = neighbourCosts[i];
if (pathNode2.pathID != pathID)
{
pathNode2.parent = pathNode;
pathNode2.pathID = pathID;
pathNode2.cost = num;
pathNode2.H = path.CalculateHScore(gridNode);
gridNode.UpdateG(path, pathNode2);
handler.PushNode(pathNode2);
}
else if (pathNode.G + num + path.GetTraversalCost(gridNode) < pathNode2.G)
{
pathNode2.cost = num;
pathNode2.parent = pathNode;
gridNode.UpdateRecursiveG(path, pathNode2, handler);
}
else if (pathNode2.G + num + path.GetTraversalCost(this) < pathNode.G)
{
pathNode.parent = pathNode2;
pathNode.cost = num;
this.UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
if (this.connections != null)
{
for (int j = 0; j < this.connections.Length; j++)
{
GraphNode graphNode = this.connections[j];
if (path.CanTraverse(graphNode))
{
PathNode pathNode3 = handler.GetPathNode(graphNode);
uint num2 = this.connectionCosts[j];
if (pathNode3.pathID != pathID)
{
pathNode3.parent = pathNode;
pathNode3.pathID = pathID;
pathNode3.cost = num2;
pathNode3.H = path.CalculateHScore(graphNode);
graphNode.UpdateG(path, pathNode3);
handler.PushNode(pathNode3);
}
else if (pathNode.G + num2 + path.GetTraversalCost(graphNode) < pathNode3.G)
{
pathNode3.cost = num2;
pathNode3.parent = pathNode;
graphNode.UpdateRecursiveG(path, pathNode3, handler);
}
else if (pathNode3.G + num2 + path.GetTraversalCost(this) < pathNode.G && graphNode.ContainsConnection(this))
{
pathNode.parent = pathNode3;
pathNode.cost = num2;
this.UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
public override void 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
}
// Token: 0x06000756 RID: 1878 RVA: 0x00047AF6 File Offset: 0x00045EF6
public override bool TargetFound(PathNode node)
{
return((ulong)node.G >= (ulong)((long)this.maxGScore));
}
public void UpdateG(Path path, PathNode pathNode)
{
pathNode.G = pathNode.parent.G + pathNode.cost + path.GetTraversalCost(this);
}
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);
}
}
}
}
}
}
/** Open the node */ public abstract void Open(Path path, PathNode pathNode, PathHandler handler);
/** 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 (PathNode from) {
var count = 0;
var 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 (var i = 0;i<count;i++) {
path.Add (c.node);
c = c.parent;
}
var half = count/2;
for (var i=0;i<half;i++) {
var tmp = path[i];
path[i] = path[count-i-1];
path[count - i - 1] = tmp;
}
for (var i=0;i<count;i++) {
vectorPath.Add ((Vector3)path[i].position);
}
}
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);
}
}
}
}
}
/** 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 (PathNode node) {
return node.node == abPath.endNode;
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
if (connections == null)
{
return;
}
var flag2 = pathNode.flag2;
for (var i = connections.Length - 1; i >= 0; i--)
{
var other = connections[i];
if (path.CanTraverse(other))
{
var 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;
}
var 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 Initialize () {
// Mark nodes to enable special connection costs for start and end nodes
// See GetConnectionSpecialCost
if (startNode != null) pathHandler.GetPathNode (startNode).flag2 = true;
if (endNode != null) pathHandler.GetPathNode (endNode).flag2 = true;
// Zero out the properties on the start node
PathNode startRNode = pathHandler.GetPathNode (startNode);
startRNode.node = startNode;
startRNode.pathID = pathHandler.PathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.G = GetTraversalCost (startNode);
startRNode.H = CalculateHScore (startNode);
// Check if the start node is the target and complete the path if that is the case
CompletePathIfStartIsValidTarget ();
if (CompleteState == PathCompleteState.Complete) return;
// Open the start node and puts its neighbours in the open list
startNode.Open (this,startRNode,pathHandler);
searchedNodes++;
partialBestTarget = startRNode;
// Any nodes left to search?
if (pathHandler.HeapEmpty ()) {
if (calculatePartial) {
CompleteState = PathCompleteState.Partial;
Trace (partialBestTarget);
} else {
Error ();
LogError ("No open points, the start node didn't open any nodes");
return;
}
}
// Pop the first node off the open list
currentR = pathHandler.PopNode ();
}
public void RecalculateCosts()
{
if (pivots == null)
{
RecalculatePivots();
}
if (mode == HeuristicOptimizationMode.None)
{
return;
}
pivotCount = 0;
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] != null && (pivots[i].Destroyed || !pivots[i].Walkable))
{
throw new System.Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] == null)
{
throw new System.Exception("Invalid pivot nodes (null)");
}
}
}
Debug.Log("Recalculating costs...");
pivotCount = pivots.Length;
System.Action <int> startCostCalculation = null;
int numComplete = 0;
OnPathDelegate onComplete = (Path path) => {
numComplete++;
if (numComplete == pivotCount)
{
// Last completed path
ApplyGridGraphEndpointSpecialCase();
}
};
startCostCalculation = (int pivotIndex) => {
GraphNode pivot = pivots[pivotIndex];
FloodPath floodPath = null;
floodPath = FloodPath.Construct(pivot, onComplete);
floodPath.immediateCallback = (Path _p) => {
// Handle path pooling
_p.Claim(this);
// When paths are calculated on navmesh based graphs
// the costs are slightly modified to match the actual target and start points
// instead of the node centers
// so we have to remove the cost for the first and last connection
// in each path
var meshNode = pivot as MeshNode;
uint costOffset = 0;
if (meshNode != null && meshNode.connections != null)
{
for (int i = 0; i < meshNode.connections.Length; i++)
{
costOffset = System.Math.Max(costOffset, meshNode.connections[i].cost);
}
}
var graphs = AstarPath.active.graphs;
// Process graphs in reverse order to raise probability that we encounter large NodeIndex values quicker
// to avoid resizing the internal array too often
for (int j = graphs.Length - 1; j >= 0; j--)
{
graphs[j].GetNodes(node => {
int idx = node.NodeIndex * pivotCount + pivotIndex;
EnsureCapacity(idx);
PathNode pn = ((IPathInternals)floodPath).PathHandler.GetPathNode(node);
if (costOffset > 0)
{
costs[idx] = pn.pathID == floodPath.pathID && pn.parent != null ? System.Math.Max(pn.parent.G - costOffset, 0) : 0;
}
else
{
costs[idx] = pn.pathID == floodPath.pathID ? pn.G : 0;
}
});
}
if (mode == HeuristicOptimizationMode.RandomSpreadOut && pivotIndex < pivots.Length - 1)
{
// If the next pivot is null
// then find the node which is furthest away from the earlier
// pivot points
if (pivots[pivotIndex + 1] == null)
{
int best = -1;
uint bestScore = 0;
// Actual number of nodes
int totCount = maxNodeIndex / pivotCount;
// Loop through all nodes
for (int j = 1; j < totCount; j++)
{
// Find the minimum distance from the node to all existing pivot points
uint mx = 1 << 30;
for (int p = 0; p <= pivotIndex; p++)
{
mx = System.Math.Min(mx, costs[j * pivotCount + p]);
}
// Pick the node which has the largest minimum distance to the existing pivot points
// (i.e pick the one furthest away from the existing ones)
GraphNode node = ((IPathInternals)floodPath).PathHandler.GetPathNode(j).node;
if ((mx > bestScore || best == -1) && node != null && !node.Destroyed && node.Walkable)
{
best = j;
bestScore = mx;
}
}
if (best == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
pivots[pivotIndex + 1] = ((IPathInternals)floodPath).PathHandler.GetPathNode(best).node;
}
// Start next path
startCostCalculation(pivotIndex + 1);
}
// Handle path pooling
_p.Release(this);
};
AstarPath.StartPath(floodPath, true);
};
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
// All calculated in parallel
for (int i = 0; i < pivots.Length; i++)
{
startCostCalculation(i);
}
}
else
{
// Recursive and serial
startCostCalculation(0);
}
dirty = false;
}
public override void Open (Path path, PathNode pathNode, PathHandler handler) {
if (connections == null) return;
for (var i=0;i<connections.Length;i++) {
var other = connections[i];
if (path.CanTraverse (other)) {
var 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
var 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);
}
}
}
}
}
// Token: 0x06002C91 RID: 11409 RVA: 0x001CA04B File Offset: 0x001C824B
public Tuple(uint f, PathNode node)
{
this.F = f;
this.node = node;
}
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 UpdateRecursiveG(Path path, PathNode pathNode, PathHandler handler)
{
base.UpdateG(path, pathNode);
handler.PushNode(pathNode);
for (int i = 0; i < this.connections.Length; i++)
{
GraphNode graphNode = this.connections[i];
PathNode pathNode2 = handler.GetPathNode(graphNode);
if (pathNode2.parent == pathNode && pathNode2.pathID == handler.PathID)
{
graphNode.UpdateRecursiveG(path, pathNode2, handler);
}
}
}
public 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);
});
}
protected virtual void Trace(PathNode from)
{
int num = 0;
PathNode pathNode = from;
while (pathNode != null)
{
pathNode = pathNode.parent;
num++;
if (num > 2048)
{
UnityEngine.Debug.LogWarning("Infinite loop? >2048 node path. Remove this message if you really have that long paths (Path.cs, Trace method)");
break;
}
}
if (this.path.Capacity < num)
{
this.path.Capacity = num;
}
if (this.vectorPath.Capacity < num)
{
this.vectorPath.Capacity = num;
}
pathNode = from;
for (int i = 0; i < num; i++)
{
this.path.Add(pathNode.node);
pathNode = pathNode.parent;
}
int num2 = num / 2;
for (int j = 0; j < num2; j++)
{
GraphNode value = this.path[j];
this.path[j] = this.path[num - j - 1];
this.path[num - j - 1] = value;
}
for (int k = 0; k < num; k++)
{
this.vectorPath.Add((Vector3)this.path[k].position);
}
}
public override void Initialize()
{
// Mark nodes to enable special connection costs for start and end nodes
// See GetConnectionSpecialCost
if (startNode != null)
{
pathHandler.GetPathNode(startNode).flag2 = true;
}
if (endNode != null)
{
pathHandler.GetPathNode(endNode).flag2 = true;
}
if (hasEndPoint && startNode == endNode)
{
PathNode endNodeR = pathHandler.GetPathNode(endNode);
endNodeR.node = endNode;
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 *
* }*/
PathNode startRNode = pathHandler.GetPathNode(startNode);
startRNode.node = startNode;
startRNode.pathID = pathHandler.PathID;
startRNode.parent = null;
startRNode.cost = 0;
startRNode.G = GetTraversalCost(startNode);
startRNode.H = CalculateHScore(startNode);
/*if (recalcStartEndCosts) {
* startNode.InitialOpen (open,hTarget,startIntPoint,this,true);
* } else {*/
startNode.Open(this, startRNode, pathHandler);
//}
searchedNodes++;
partialBestTarget = startRNode;
//any nodes left to search?
if (pathHandler.HeapEmpty())
{
if (calculatePartial)
{
CompleteState = PathCompleteState.Partial;
Trace(partialBestTarget);
}
else
{
Error();
LogError("No open points, the start node didn't open any nodes");
return;
}
}
currentR = pathHandler.PopNode();
}
public void RecalculateCosts()
{
if (pivots == null)
{
RecalculatePivots();
}
if (mode == HeuristicOptimizationMode.None)
{
return;
}
pivotCount = 0;
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] != null && (pivots[i].Destroyed || !pivots[i].Walkable))
{
throw new System.Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] == null)
{
throw new System.Exception("Invalid pivot nodes (null)");
}
}
}
UnityEngine.Debug.Log("Recalculating costs...");
pivotCount = pivots.Length;
System.Action <int> startCostCalculation = null;
startCostCalculation = delegate(int k) {
GraphNode pivot = pivots[k];
FloodPath fp = null;
fp = FloodPath.Construct(pivot);
fp.immediateCallback = delegate(Path _p) {
// Handle path pooling
_p.Claim(this);
// When paths are calculated on navmesh based graphs
// the costs are slightly modified to match the actual target and start points
// instead of the node centers
// so we have to remove the cost for the first and last connection
// in each path
MeshNode mn = pivot as MeshNode;
uint costOffset = 0;
if (mn != null && mn.connectionCosts != null)
{
for (int i = 0; i < mn.connectionCosts.Length; i++)
{
costOffset = System.Math.Max(costOffset, mn.connectionCosts[i]);
}
}
var graphs = AstarPath.active.graphs;
// Process graphs in reverse order to raise probability that we encounter large NodeIndex values quicker
// to avoid resizing the internal array too often
for (int j = graphs.Length - 1; j >= 0; j--)
{
graphs[j].GetNodes(delegate(GraphNode node) {
int idx = node.NodeIndex * pivotCount + k;
EnsureCapacity(idx);
PathNode pn = fp.pathHandler.GetPathNode(node);
if (costOffset > 0)
{
costs[idx] = pn.pathID == fp.pathID && pn.parent != null ? System.Math.Max(pn.parent.G - costOffset, 0) : 0;
}
else
{
costs[idx] = pn.pathID == fp.pathID ? pn.G : 0;
}
return(true);
});
}
if (mode == HeuristicOptimizationMode.RandomSpreadOut && k < pivots.Length - 1)
{
int best = -1;
uint bestScore = 0;
// Actual number of nodes
int totCount = maxNodeIndex / pivotCount;
// Loop through all nodes
for (int j = 1; j < totCount; j++)
{
// Find the minimum distance from the node to all existing pivot points
uint mx = 1 << 30;
for (int p = 0; p <= k; p++)
{
mx = System.Math.Min(mx, costs[j * pivotCount + p]);
}
// Pick the node which has the largest minimum distance to the existing pivot points
// (i.e pick the one furthest away from the existing ones)
GraphNode node = fp.pathHandler.GetPathNode(j).node;
if ((mx > bestScore || best == -1) && node != null && !node.Destroyed && node.Walkable)
{
best = j;
bestScore = mx;
}
}
if (best == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
pivots[k + 1] = fp.pathHandler.GetPathNode(best).node;
Debug.Log("Found node at " + pivots[k + 1].position + " with score " + bestScore);
startCostCalculation(k + 1);
}
// Handle path pooling
_p.Release(this);
};
AstarPath.StartPath(fp, true);
};
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
// All calculated in paralell
for (int i = 0; i < pivots.Length; i++)
{
startCostCalculation(i);
}
}
else
{
// Recursive and serial
startCostCalculation(0);
}
dirty = false;
}
public virtual void Reset()
{
if (object.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");
}
this.hasBeenReset = true;
this.state = PathState.Created;
this.releasedNotSilent = false;
this.pathHandler = null;
this.callback = null;
this._errorLog = string.Empty;
this.pathCompleteState = PathCompleteState.NotCalculated;
this.path = ListPool<GraphNode>.Claim();
this.vectorPath = ListPool<Vector3>.Claim();
this.currentR = null;
this.duration = 0f;
this.searchIterations = 0;
this.searchedNodes = 0;
this.nnConstraint = PathNNConstraint.Default;
this.next = null;
this.heuristic = AstarPath.active.heuristic;
this.heuristicScale = AstarPath.active.heuristicScale;
this.enabledTags = -1;
this.tagPenalties = null;
this.callTime = DateTime.UtcNow;
this.pathID = AstarPath.active.GetNextPathID();
this.hTarget = Int3.zero;
this.hTargetNode = null;
}
/** 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;
}
AstarProfiler.StartFastProfile(4);
//Debug.DrawRay ((Vector3)currentR.node.Position, Vector3.up*2,Color.red);
//Loop through all walkable neighbours of the node and add them to the open list.
currentR.node.Open(this, currentR, pathHandler);
AstarProfiler.EndFastProfile(4);
//any nodes left to search?
if (pathHandler.HeapEmpty())
{
Error();
LogError("No open points, whole area searched");
return;
}
//Select the node with the lowest F score and remove it from the open list
AstarProfiler.StartFastProfile(7);
currentR = pathHandler.PopNode();
AstarProfiler.EndFastProfile(7);
//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;
if (searchedNodes > 1000000)
{
throw new System.Exception("Probable infinite loop. Over 1,000,000 nodes searched");
}
}
counter++;
}
AstarProfiler.StartProfile("Trace");
if (CompleteState == PathCompleteState.Complete)
{
Trace(currentR);
}
else if (calculatePartial && partialBestTarget != null)
{
CompleteState = PathCompleteState.Partial;
Trace(partialBestTarget);
}
AstarProfiler.EndProfile();
}
public override bool TargetFound (PathNode node) {
return node.G >= maxGScore;
}
/* 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)
{
PathNode nodeR = pathHandler.GetPathNode(endNode);
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: ");
if (startNode != null)
{
text.Append(startNode.GraphIndex);
}
else
{
text.Append("< null startNode >");
}
//text.Append ("\nBinary Heap size at completion: ");
//text.Append (pathHandler.open == null ? "Null" : (pathHandler.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);
}
if (logMode == PathLog.Heavy && !AstarPath.IsUsingMultithreading)
{
text.Append("\nCallback references ");
if (callback != null)
{
text.Append(callback.Target.GetType().FullName).AppendLine();
}
else
{
text.AppendLine("NULL");
}
}
text.Append("\nPath Number ");
text.Append(pathID);
return(text.ToString());
}
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
}
public override bool TargetFound(PathNode node)
{
return(node.G >= this.maxGScore);
}
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 void PushNode(PathNode node)
{
this.heap.Add(node);
}
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);
}
}
}
}
}
/* 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);
}
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);
}
}
}
}
/** Returns the node with the lowest F score from the heap */
public PathNode Remove()
{
numberOfItems--;
PathNode returnItem = binaryHeap[0].node;
binaryHeap[0] = binaryHeap[numberOfItems];
int swapItem = 0, parent;
do
{
if (D == 0)
{
parent = swapItem;
int p2 = parent * D;
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;
}
}
}
else
{
parent = swapItem;
uint swapF = binaryHeap[swapItem].F;
int pd = parent * D + 1;
if (D >= 1 && pd + 0 <= numberOfItems && (binaryHeap[pd + 0].F < swapF || (SortGScores && binaryHeap[pd + 0].F == swapF && binaryHeap[pd + 0].node.G < binaryHeap[swapItem].node.G)))
{
swapF = binaryHeap[pd + 0].F;
swapItem = pd + 0;
}
if (D >= 2 && pd + 1 <= numberOfItems && (binaryHeap[pd + 1].F < swapF || (SortGScores && binaryHeap[pd + 1].F == swapF && binaryHeap[pd + 1].node.G < binaryHeap[swapItem].node.G)))
{
swapF = binaryHeap[pd + 1].F;
swapItem = pd + 1;
}
if (D >= 3 && pd + 2 <= numberOfItems && (binaryHeap[pd + 2].F < swapF || (SortGScores && binaryHeap[pd + 2].F == swapF && binaryHeap[pd + 2].node.G < binaryHeap[swapItem].node.G)))
{
swapF = binaryHeap[pd + 2].F;
swapItem = pd + 2;
}
if (D >= 4 && pd + 3 <= numberOfItems && (binaryHeap[pd + 3].F < swapF || (SortGScores && binaryHeap[pd + 3].F == swapF && binaryHeap[pd + 3].node.G < binaryHeap[swapItem].node.G)))
{
swapF = binaryHeap[pd + 3].F;
swapItem = pd + 3;
}
if (D >= 5 && pd + 4 <= numberOfItems && binaryHeap[pd + 4].F < swapF)
{
swapF = binaryHeap[pd + 4].F;
swapItem = pd + 4;
}
if (D >= 6 && pd + 5 <= numberOfItems && binaryHeap[pd + 5].F < swapF)
{
swapF = binaryHeap[pd + 5].F;
swapItem = pd + 5;
}
if (D >= 7 && pd + 6 <= numberOfItems && binaryHeap[pd + 6].F < swapF)
{
swapF = binaryHeap[pd + 6].F;
swapItem = pd + 6;
}
if (D >= 8 && pd + 7 <= numberOfItems && binaryHeap[pd + 7].F < swapF)
{
swapF = binaryHeap[pd + 7].F;
swapItem = pd + 7;
}
if (D >= 9 && pd + 8 <= numberOfItems && binaryHeap[pd + 8].F < swapF)
{
swapF = binaryHeap[pd + 8].F;
swapItem = pd + 8;
}
}
// One if the parent's children are smaller or equal, swap them
if (parent != swapItem)
{
var tmpIndex = binaryHeap[parent];
binaryHeap[parent] = binaryHeap[swapItem];
binaryHeap[swapItem] = tmpIndex;
}
else
{
break;
}
} while (true);
//Validate ();
return(returnItem);
}