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;
int nodeInGridIndex = base.NodeInGridIndex;
for (int i = 0; i < 8; i++)
{
if (this.HasConnectionInDirection(i))
{
GridNode gridNode = nodes[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.heap.Add(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);
}
}
}
}
base.Open(path, pathNode, handler);
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gridGraph = 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 node = nodes[base.nodeInGridIndex + neighbourOffsets[i]];
if (path.CanTraverse(node))
{
PathNode node2 = handler.GetPathNode(node);
uint num3 = neighbourCosts[i];
if (node2.pathID != pathID)
{
node2.parent = pathNode;
node2.pathID = pathID;
node2.cost = num3;
node2.H = path.CalculateHScore(node);
node.UpdateG(path, node2);
handler.heap.Add(node2);
}
else if (((pathNode.G + num3) + path.GetTraversalCost(node)) < node2.G)
{
node2.cost = num3;
node2.parent = pathNode;
node.UpdateRecursiveG(path, node2, handler);
}
else if (((node2.G + num3) + path.GetTraversalCost(this)) < pathNode.G)
{
pathNode.parent = node2;
pathNode.cost = num3;
this.UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
ushort pid = handler.PathID;
{
int[] neighbourOffsets = gg.neighbourOffsets;
uint[] neighbourCosts = gg.neighbourCosts;
GridNode[] nodes = gg.nodes;
for (int i = 0; i < 8; i++)
{
if (GetConnectionInternal(i))
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
// Multiply the connection cost with 1 + the average of the traversal costs for the two nodes
uint tmpCost = (neighbourCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other))) / 128;
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = tmpCost;
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
// Sorry for the huge number of #ifs
//If not we can test if the path from the current node to this one is a better one then the one already used
if (pathNode.G + tmpCost < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost < pathNode.G)
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
if (connections != null)
{
for (int i = 0; i < connections.Length; i++)
{
GraphNode other = connections[i];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
uint tmpCost = (connectionCosts[i] * (256 + path.GetTraversalCost(this) + path.GetTraversalCost(other))) / 128;
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = tmpCost;
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
// Sorry for the huge number of #ifs
//If not we can test if the path from the current node to this one is a better one then the one already used
if (pathNode.G + tmpCost < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost < pathNode.G && other.ContainsConnection(this))
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
/** 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 ) {
* EB.Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir);
* EB.Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2));
* }*/
for (int i = 0; i < 8; i++)
{
if (((nb >> i) & 1) != 0)
{
int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8];
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
#if ASTARDEBUG
if (((forced >> i) & 1) != 0)
{
Debug.DrawLine((Vector3)position, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f), Color.red);
}
if (((natural >> i) & 1) != 0)
{
Debug.DrawLine((Vector3)position + Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) + Vector3.up * 0.2f, Color.green);
}
#endif
if (oi < 4)
{
other = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]);
}
else
{
other = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]);
}
if (other != null)
{
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.0f, (Vector3)other.position + Vector3.up*0.3f, Color.cyan);
//Debug.DrawRay ( (Vector3)other.position, Vector3.up, Color.cyan);
//GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
//if (!path.CanTraverse (other)) continue;
PathNode otherPN = handler.GetPathNode(other);
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = (uint)(other.position - position).costMagnitude; //neighbourCosts[i];
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
//If not we can test if the path from the current node to this one is a better one then the one already used
uint tmpCost = (uint)(other.position - position).costMagnitude; //neighbourCosts[i];
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
#if ASTARDEBUG
if (i == 0 && parentDir != -1 && this.nodeInGridIndex > 10)
{
int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8];
if (nodeInGridIndex + neighbourOffsets[oi] < 0 || nodeInGridIndex + neighbourOffsets[oi] >= nodes.Length)
{
//Debug.LogError ("ERR: " + (nodeInGridIndex + neighbourOffsets[oi]) + " " + cyclicParentDir + " " + parentDir + " Reverted " + oi);
//Debug.DrawRay ((Vector3)position, Vector3.up, Color.red);
}
else
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
Debug.DrawLine((Vector3)position - Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) - Vector3.up * 0.2f, Color.blue);
}
}
#endif
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
ushort pid = handler.PathID;
#if ASTAR_JPS
if (gg.useJumpPointSearch && !path.FloodingPath)
{
JPSOpen(path, pathNode, handler);
}
else
#endif
{
int[] neighbourOffsets = gg.neighbourOffsets;
uint[] neighbourCosts = gg.neighbourCosts;
GridNode[] nodes = gg.nodes;
for (int i = 0; i < 8; i++)
{
if (GetConnectionInternal(i))
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
uint tmpCost = neighbourCosts[i];
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = tmpCost;
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
// Sorry for the huge number of #ifs
//If not we can test if the path from the current node to this one is a better one then the one already used
#if ASTAR_NO_TRAVERSAL_COST
if (pathNode.G + tmpCost < otherPN.G)
#else
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#endif
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
#if ASTAR_NO_TRAVERSAL_COST
else if (otherPN.G + tmpCost < pathNode.G)
#else
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
#endif
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
if (connections != null)
{
for (int i = 0; i < connections.Length; i++)
{
GraphNode other = connections[i];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
uint tmpCost = connectionCosts[i];
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = tmpCost;
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
// Sorry for the huge number of #ifs
//If not we can test if the path from the current node to this one is a better one then the one already used
#if ASTAR_NO_TRAVERSAL_COST
if (pathNode.G + tmpCost < otherPN.G)
#else
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#endif
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
#if ASTAR_NO_TRAVERSAL_COST
else if (otherPN.G + tmpCost < pathNode.G && other.ContainsConnection(this))
#else
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this))
#endif
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
#endif
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph 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);
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);
}
}
}
}
}
