/** Called when a path has completed it's calculation */
public void OnPathComplete (Path p) {
/*if (Time.time-lastPathSearch >= repathRate) {
Repath ();
} else {*/
StartCoroutine (WaitToRepath ());
//}
//If the path didn't succeed, don't proceed
if (p.error) {
return;
}
//Get the calculated path as a Vector3 array
path = p.vectorPath.ToArray();
//Find the segment in the path which is closest to the AI
//If a closer segment hasn't been found in '6' iterations, break because it is unlikely to find any closer ones then
float minDist = Mathf.Infinity;
int notCloserHits = 0;
for (int i=0;i<path.Length-1;i++) {
float dist = AstarMath.DistancePointSegmentStrict (path[i],path[i+1],tr.position);
if (dist < minDist) {
notCloserHits = 0;
minDist = dist;
pathIndex = i+1;
} else if (notCloserHits > 6) {
break;
}
}
}
/** Pops all items from the stack and returns the head.
* To loop through all popped items, simple traverse the linked list starting with the head and continuing with item.next until item equals null
* \code
* Path p = stack.PopAll ();
* while (p != null) {
* //Do something
* p = p.next;
* }
* \endcode
*/
public Path PopAll () {
#if UNITY_IPHONE || UNITY_PSP2 || UNITY_XBOXONE || UNITY_PS3 || UNITY_PS4 || UNITY_WIIU
lock (lockObj) {
Path h = head;
head = null;
return h;
}
#else
return Interlocked.Exchange<Path> (ref head, null);
#endif
}
public override void Apply (Path p, ModifierData source) {
List<GraphNode> path = p.path;
List<Vector3> vectorPath = p.vectorPath;
if (path == null || path.Count == 0 || vectorPath == null || vectorPath.Count != path.Count) {
return;
}
List<Vector3> funnelPath = ListPool<Vector3>.Claim ();
// Claim temporary lists and try to find lists with a high capacity
List<Vector3> left = ListPool<Vector3>.Claim (path.Count+1);
List<Vector3> right = ListPool<Vector3>.Claim (path.Count+1);
AstarProfiler.StartProfile ("Construct Funnel");
// Enqueue start point
left.Add (vectorPath[0]);
right.Add (vectorPath[0]);
// Loop through all nodes in the path (except the last one)
for (int i=0;i<path.Count-1;i++) {
// Get the portal between path[i] and path[i+1] and add it to the left and right lists
bool portalWasAdded = path[i].GetPortal (path[i+1], left, right, false);
if (!portalWasAdded) {
// Fallback, just use the positions of the nodes
left.Add ((Vector3)path[i].position);
right.Add ((Vector3)path[i].position);
left.Add ((Vector3)path[i+1].position);
right.Add ((Vector3)path[i+1].position);
}
}
// Enqueue end point
left.Add (vectorPath[vectorPath.Count-1]);
right.Add (vectorPath[vectorPath.Count-1]);
if (!RunFunnel (left,right,funnelPath)) {
// If funnel algorithm failed, degrade to simple line
funnelPath.Add (vectorPath[0]);
funnelPath.Add (vectorPath[vectorPath.Count-1]);
}
// Release lists back to the pool
ListPool<Vector3>.Release (p.vectorPath);
p.vectorPath = funnelPath;
ListPool<Vector3>.Release (left);
ListPool<Vector3>.Release (right);
}
public override void Apply (Path p, ModifierData source) {
if (this == null) return;
lock (lockObject) {
toBeApplied = p.path.ToArray();
if (!waitingForApply) {
waitingForApply = true;
AstarPath.OnPathPreSearch += ApplyNow;
}
}
}
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
UpdateG (path,pathNode);
handler.PushNode (pathNode);
for (int i=0;i<connections.Length;i++) {
GraphNode other = connections[i];
PathNode otherPN = handler.GetPathNode (other);
if (otherPN.parent == pathNode && otherPN.pathID == handler.PathID) {
other.UpdateRecursiveG (path, otherPN,handler);
}
}
}
/** Pushes a path onto the stack.
* Will loop while trying to set the head of the stack to \a p. */
public void Push (Path p) {
#if UNITY_IPHONE || UNITY_PSP2 || UNITY_XBOXONE || UNITY_PS3 || UNITY_PS4 || UNITY_WIIU
lock (lockObj) {
p.next = head;
head = p;
}
#else
while (true) {
p.next = head;
//Compare head and p.next, if they are equal, set head to p
Path old = Interlocked.CompareExchange<Path>(ref head, p, p.next);
//If the exchange suceeded, break. Otherwise, try again
if (old == p.next) break;
}
#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.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
}
}
/** Starts a path specified by PathTypesDemo.activeDemo */
void DemoPath () {
Path p = null;
if (activeDemo == DemoMode.ABPath) {
p = ABPath.Construct (start.position,end.position, OnPathComplete);
if (agents != null && agents.Length > 0) {
List<Vector3> pts = ListPool<Vector3>.Claim(agents.Length);
Vector3 avg = Vector3.zero;
for (int i=0;i<agents.Length;i++) {
pts.Add (agents[i].transform.position);
avg += pts[i];
}
avg /= pts.Count;
for (int i=0;i<agents.Length;i++) pts[i] -= avg;
PathUtilities.GetPointsAroundPoint (end.position, AstarPath.active.graphs[0] as IRaycastableGraph, pts, 0, 0.2f);
for (int i=0;i<agents.Length;i++) {
if (agents[i] == null) continue;
agents[i].target.position = pts[i];
agents[i].UpdatePath();
}
}
} else if (activeDemo == DemoMode.MultiTargetPath) {
MultiTargetPath mp = MultiTargetPath.Construct (multipoints.ToArray (), end.position, null, OnPathComplete);
p = mp;
} else if (activeDemo == DemoMode.RandomPath) {
RandomPath rp = RandomPath.Construct (start.position,searchLength, OnPathComplete);
rp.spread = spread;
rp.aimStrength = aimStrength;
rp.aim = end.position;
p = rp;
} else if (activeDemo == DemoMode.FleePath) {
FleePath fp = FleePath.Construct (start.position, end.position, searchLength, OnPathComplete);
fp.aimStrength = aimStrength;
fp.spread = spread;
p = fp;
} else if (activeDemo == DemoMode.ConstantPath) {
StartCoroutine(CalculateConstantPath());
p = null;
} else if (activeDemo == DemoMode.FloodPath) {
FloodPath fp = FloodPath.Construct (end.position, null);
lastFlood = fp;
p = fp;
} else if (activeDemo == DemoMode.FloodPathTracer && lastFlood != null) {
FloodPathTracer fp = FloodPathTracer.Construct (end.position, lastFlood, OnPathComplete);
p = fp;
}
if (p != null) {
AstarPath.StartPath (p);
lastPath = p;
}
}
public override void Apply (Path p, ModifierData source) {
List<Vector3> vs = p.vectorPath;
List<Vector3> res = Apply (vs);
if (res != vs) {
ListPool<Vector3>.Release(p.vectorPath);
p.vectorPath = res;
}
}
public void PreProcess (Path p) {
// Required by IPathModifier
}
public override void Open (Path path, PathNode pathNode, PathHandler handler) {
if (connections == null) return;
for (int i=0;i<connections.Length;i++) {
GraphNode other = connections[i];
if (path.CanTraverse (other)) {
PathNode pathOther = handler.GetPathNode (other);
if (pathOther.pathID != handler.PathID) {
pathOther.parent = pathNode;
pathOther.pathID = handler.PathID;
pathOther.cost = connectionCosts[i];
pathOther.H = path.CalculateHScore (other);
other.UpdateG (path, pathOther);
handler.PushNode (pathOther);
} else {
//If not we can test if the path from this node to the other one is a better one then the one already used
uint tmpCost = connectionCosts[i];
#if ASTAR_NO_TRAVERSAL_COST
if (pathNode.G + tmpCost < pathOther.G)
#else
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < pathOther.G)
#endif
{
pathOther.cost = tmpCost;
pathOther.parent = pathNode;
other.UpdateRecursiveG (path, pathOther,handler);
}
#if ASTAR_NO_TRAVERSAL_COST
else if (pathOther.G+tmpCost < pathNode.G && other.ContainsConnection (this))
#else
else if (pathOther.G+tmpCost+path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection (this))
#endif
{
//Or if the path from the other node to this one is better
pathNode.parent = pathOther;
pathNode.cost = tmpCost;
UpdateRecursiveG (path, pathNode,handler);
}
}
}
}
}
public override void Apply (Path p, ModifierData source) {
//System.DateTime startTime = System.DateTime.UtcNow;
if (iterations <= 0) {
return;
}
if (nodes == null) {
nodes = new List<Vector3> (p.vectorPath.Count);
} else {
nodes.Clear ();
}
nodes.AddRange (p.vectorPath);
// = new List<Vector3> (p.vectorPath);
for (int it=0;it<iterations;it++) {
if (subdivideEveryIter && it != 0) {
if (nodes.Capacity < nodes.Count*3) {
nodes.Capacity = nodes.Count*3;
}
int preLength = nodes.Count;
for (int j=0;j<preLength-1;j++) {
nodes.Add (Vector3.zero);
nodes.Add (Vector3.zero);
}
for (int j=preLength-1;j > 0;j--) {
Vector3 p1 = nodes[j];
Vector3 p2 = nodes[j+1];
nodes[j*3] = nodes[j];
if (j != preLength-1) {
nodes[j*3+1] = Vector3.Lerp (p1,p2,0.33F);
nodes[j*3+2] = Vector3.Lerp (p1,p2,0.66F);
}
}
}
int i = 0;
while (i < nodes.Count-2) {
Vector3 start = nodes[i];
Vector3 end = nodes[i+2];
var watch = System.Diagnostics.Stopwatch.StartNew();
if (ValidateLine (null,null,start,end)) {
nodes.RemoveAt (i+1);
} else {
i++;
}
watch.Stop ();
}
}
p.vectorPath.Clear ();
p.vectorPath.AddRange (nodes);
}
public override void Open (Path path, PathNode pathNode, PathHandler handler) {
//BaseOpen (nodeRunData, nodeR, targetPosition, path);
LayerGridGraph graph = GetGridGraph(GraphIndex);
int[] neighbourOffsets = graph.neighbourOffsets;
uint[] neighbourCosts = graph.neighbourCosts;
LevelGridNode[] nodes = graph.nodes;
int index = NodeInGridIndex;//indices & 0xFFFFFF;
for (int i=0;i<4;i++) {
int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF;
if (conn != LevelGridNode.NoConnection) {
GraphNode other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn];
if (!path.CanTraverse (other)) {
continue;
}
PathNode otherPN = handler.GetPathNode (other);
if (otherPN.pathID != handler.PathID) {
otherPN.parent = pathNode;
otherPN.pathID = handler.PathID;
otherPN.cost = neighbourCosts[i];
otherPN.H = path.CalculateHScore (other);
other.UpdateG (path, otherPN);
handler.PushNode (otherPN);
} else {
//If not we can test if the path from the current node to this one is a better one then the one already used
uint tmpCost = neighbourCosts[i];
#if ASTAR_NO_TRAVERSAL_COST
if (pathNode.G + tmpCost < otherPN.G)
#else
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
#endif
{
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG (path,otherPN, handler);
}
//Or if the path from this node ("other") to the current ("current") is better
#if ASTAR_NO_TRAVERSAL_COST
else if (otherPN.G+tmpCost < pathNode.G)
#else
else if (otherPN.G+tmpCost+path.GetTraversalCost(this) < pathNode.G)
#endif
{
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
}
public override void UpdateRecursiveG (Path path, PathNode pathNode, PathHandler handler) {
//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 void OnPathComplete (Path _p) {
ABPath p = _p as ABPath;
canSearchAgain = true;
if (path != null) path.Release (this);
path = p;
p.Claim (this);
if (p.error) {
wp = 0;
vectorPath = null;
return;
}
Vector3 p1 = p.originalStartPoint;
Vector3 p2 = transform.position;
p1.y = p2.y;
float d = (p2-p1).magnitude;
wp = 0;
vectorPath = p.vectorPath;
Vector3 waypoint;
for (float t=0;t<=d;t+=moveNextDist*0.6f) {
wp--;
Vector3 pos = p1 + (p2-p1)*t;
do {
wp++;
waypoint = vectorPath[wp];
waypoint.y = pos.y;
} while ((pos - waypoint).sqrMagnitude < moveNextDist*moveNextDist && wp != vectorPath.Count-1);
}
}
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
}
void ClearOnDestroy (Path p) {
lock (lockObject) {
AstarPath.OnPathPreSearch -= ClearOnDestroy;
waitingForApply = false;
InversePrevious ();
}
}
//public EndingConditionDistance () {}
public EndingConditionDistance (Path p, int maxGScore) : base (p) {
this.maxGScore = maxGScore;
}
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 Apply (Path _p, ModifierData source) {
var p = _p as ABPath;
//Only for ABPaths
if (p == null) return;
if (p.vectorPath.Count == 0) {
return;
}
if (p.vectorPath.Count == 1 && !addPoints) {
// Duplicate first point
p.vectorPath.Add (p.vectorPath[0]);
}
Vector3 pStart = Vector3.zero;
Vector3 pEnd = Vector3.zero;
switch(exactStartPoint) {
case Exactness.Original:
pStart = GetClampedPoint ((Vector3)p.path[0].position, p.originalStartPoint, p.path[0]);
break;
case Exactness.ClosestOnNode:
pStart = GetClampedPoint ((Vector3)p.path[0].position, p.startPoint, p.path[0]);
break;
case Exactness.SnapToNode:
pStart = (Vector3)p.path[0].position;
break;
case Exactness.Interpolate:
pStart = GetClampedPoint ((Vector3)p.path[0].position, p.originalStartPoint, p.path[0]);
pStart = AstarMath.NearestPointStrict ((Vector3)p.path[0].position,(Vector3)p.path[1>=p.path.Count?0:1].position,pStart);
break;
}
switch(exactEndPoint) {
case Exactness.Original:
pEnd = GetClampedPoint ((Vector3)p.path[p.path.Count-1].position, p.originalEndPoint, p.path[p.path.Count-1]);
break;
case Exactness.ClosestOnNode:
pEnd = GetClampedPoint ((Vector3)p.path[p.path.Count-1].position, p.endPoint, p.path[p.path.Count-1]);
break;
case Exactness.SnapToNode:
pEnd = (Vector3)p.path[p.path.Count-1].position;
break;
case Exactness.Interpolate:
pEnd = GetClampedPoint ((Vector3)p.path[p.path.Count-1].position, p.originalEndPoint, p.path[p.path.Count-1]);
pEnd = AstarMath.NearestPointStrict ((Vector3)p.path[p.path.Count-1].position,(Vector3)p.path[p.path.Count-2<0?0:p.path.Count-2].position,pEnd);
break;
}
if (!addPoints) {
p.vectorPath[0] = pStart;
p.vectorPath[p.vectorPath.Count-1] = pEnd;
} else {
if (exactStartPoint != Exactness.SnapToNode) {
p.vectorPath.Insert (0,pStart);
}
if (exactEndPoint != Exactness.SnapToNode) {
p.vectorPath.Add (pEnd);
}
}
}
/** Converts a path from \a input to \a output */
public static ModifierData Convert (Path p, ModifierData input, ModifierData output) {
//"Input" can not be converted to "output", log error
if (!CanConvert (input,output)) {
Debug.LogError ("Can't convert "+input+" to "+output);
return ModifierData.None;
}
//"Output" can take "input" with no change, return
if (AnyBits (input,output)) {
return input;
}
//If input is a node path, and output wants a vector array, convert the node array to a vector array
if (AnyBits (input,ModifierData.Nodes) && AnyBits (output, ModifierData.Vector)) {
p.vectorPath.Clear();
for (int i=0;i<p.vectorPath.Count;i++) {
p.vectorPath.Add ((Vector3)p.path[i].position);
}
//Return VectorPath and also StrictVectorPath if input has StrictNodePath set
return ModifierData.VectorPath | (AnyBits (input, ModifierData.StrictNodePath) ? ModifierData.StrictVectorPath : ModifierData.None);
}
Debug.LogError ("This part should not be reached - Error in ModifierConverted\nInput: "+input+" ("+(int)input+")\nOutput: "+output+" ("+(int)output+")");
return ModifierData.None;
}
/** Resets End Node Costs. Costs are updated on the end node at the start of the search to better reflect the end point passed to the path, the previous ones are saved in #endNodeCosts and are reset in this function which is called after the path search is complete */
public void ResetCosts (Path p) {
#if FALSE
if (!hasEndPoint) return;
endNode.ResetCosts (endNodeCosts);
#endif
}
/** Main Post-Processing function */ public abstract void Apply (Path p, ModifierData source);
/** Reset all values to their default values.
*
* \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
* override this function, resetting ALL their variables to enable recycling of paths.
* If this is not done, trying to use that path type for pooling might result in weird behaviour.
* The best way is to reset to default values the variables declared in the extended path type and then
* call this base function in inheriting types with base.Reset ().
*
* \warning This function should not be called manually.
*/
public virtual void Reset () {
#if ASTAR_POOL_DEBUG
pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n";
pathTraceInfo += System.Environment.StackTrace;
#endif
if (System.Object.ReferenceEquals (AstarPath.active, null))
throw new System.NullReferenceException ("No AstarPath object found in the scene. " +
"Make sure there is one or do not create paths in Awake");
hasBeenReset = true;
state = (int)PathState.Created;
releasedNotSilent = false;
pathHandler = null;
callback = null;
_errorLog = "";
pathCompleteState = PathCompleteState.NotCalculated;
path = ListPool<GraphNode>.Claim();
vectorPath = ListPool<Vector3>.Claim();
currentR = null;
duration = 0;
searchIterations = 0;
searchedNodes = 0;
//calltime
nnConstraint = PathNNConstraint.Default;
next = null;
heuristic = AstarPath.active.heuristic;
heuristicScale = AstarPath.active.heuristicScale;
enabledTags = -1;
tagPenalties = null;
callTime = System.DateTime.UtcNow;
pathID = AstarPath.active.GetNextPathID ();
hTarget = Int3.zero;
hTargetNode = null;
}
/** Returns if the node is in the search tree of the path.
* Only guaranteed to be correct if \a path is the latest path calculated.
* Use for gizmo drawing only.
*/
public static bool InSearchTree (GraphNode node, Path path) {
if (path == null || path.pathHandler == null) return true;
PathNode nodeR = path.pathHandler.GetPathNode (node);
return nodeR.pathID == path.pathID;
}
public PathEndingCondition (Path p) {
if (p == null) throw new System.ArgumentNullException ("p");
this.path = p;
}
/** Get the path back */
public void OnPathComplete (Path p) {
//To prevent it from creating new GameObjects when the application is quitting when using multithreading.
if(lastRender == null) return;
if (p.error) {
ClearPrevious ();
return;
}
if (p.GetType () == typeof (MultiTargetPath)) {
List<GameObject> unused = new List<GameObject> (lastRender);
lastRender.Clear ();
MultiTargetPath mp = p as MultiTargetPath;
for (int i=0;i<mp.vectorPaths.Length;i++) {
if (mp.vectorPaths[i] == null) continue;
List<Vector3> vpath = mp.vectorPaths[i];
GameObject ob = null;
if (unused.Count > i && unused[i].GetComponent<LineRenderer>() != null) {
ob = unused[i];
unused.RemoveAt (i);
} else {
ob = new GameObject ("LineRenderer_"+i,typeof(LineRenderer));
}
LineRenderer lr = ob.GetComponent<LineRenderer>();
lr.sharedMaterial = lineMat;
lr.SetWidth (lineWidth,lineWidth);
lr.SetVertexCount (vpath.Count);
for (int j=0;j<vpath.Count;j++) {
lr.SetPosition (j,vpath[j] + pathOffset);
}
lastRender.Add (ob);
}
for (int i=0;i<unused.Count;i++) {
Destroy (unused[i]);
}
} else if (p.GetType () == typeof (ConstantPath)) {
ClearPrevious ();
//The following code will build a mesh with a square for each node visited
ConstantPath constPath = p as ConstantPath;
List<GraphNode> nodes = constPath.allNodes;
Mesh mesh = new Mesh ();
List<Vector3> verts = new List<Vector3>();
bool drawRaysInstead = false;
// Just some debugging code which selects random points on the nodes
/*List<Vector3> pts = Pathfinding.PathUtilities.GetPointsOnNodes (nodes, 20, 0);
Vector3 avg = Vector3.zero;
for (int i=0;i<pts.Count;i++) {
Debug.DrawRay (pts[i], Vector3.up*5, Color.red, 3);
avg += pts[i];
}
if (pts.Count > 0) avg /= pts.Count;
for (int i=0;i<pts.Count;i++) {
pts[i] -= avg;
}
Pathfinding.PathUtilities.GetPointsAroundPoint (start.position, AstarPath.active.astarData.graphs[0] as IRaycastableGraph, pts, 0, 1);
for (int i=0;i<pts.Count;i++) {
Debug.DrawRay (pts[i], Vector3.up*5, Color.blue, 3);
}*/
//This will loop through the nodes from furthest away to nearest, not really necessary... but why not :D
//Note that the reverse does not, as common sense would suggest, loop through from the closest to the furthest away
//since is might contain duplicates and only the node duplicate placed at the highest index is guarenteed to be ordered correctly.
for (int i=nodes.Count-1;i>=0;i--) {
Vector3 pos = (Vector3)nodes[i].position+pathOffset;
if (verts.Count == 65000 && !drawRaysInstead) {
Debug.LogError ("Too many nodes, rendering a mesh would throw 65K vertex error. Using Debug.DrawRay instead for the rest of the nodes");
drawRaysInstead = true;
}
if (drawRaysInstead) {
Debug.DrawRay (pos,Vector3.up,Color.blue);
continue;
}
//Enqueue vertices in a square
GridGraph gg = AstarData.GetGraph (nodes[i]) as GridGraph;
float scale = 1F;
if (gg != null) scale = gg.nodeSize;
verts.Add (pos+new Vector3 (-0.5F,0,-0.5F)*scale);
verts.Add (pos+new Vector3 (0.5F,0,-0.5F)*scale);
verts.Add (pos+new Vector3 (-0.5F,0,0.5F)*scale);
verts.Add (pos+new Vector3 (0.5F,0,0.5F)*scale);
}
//Build triangles for the squares
Vector3[] vs = verts.ToArray ();
int[] tris = new int[(3*vs.Length)/2];
for (int i=0, j=0;i<vs.Length;j+=6, i+=4) {
tris[j+0] = i;
tris[j+1] = i+1;
tris[j+2] = i+2;
tris[j+3] = i+1;
tris[j+4] = i+3;
tris[j+5] = i+2;
}
Vector2[] uv = new Vector2[vs.Length];
//Set up some basic UV
for (int i=0;i<uv.Length;i+=4) {
uv[i] = new Vector2(0,0);
uv[i+1] = new Vector2(1,0);
uv[i+2] = new Vector2(0,1);
uv[i+3] = new Vector2(1,1);
}
mesh.vertices = vs;
mesh.triangles = tris;
mesh.uv = uv;
mesh.RecalculateNormals ();
GameObject go = new GameObject("Mesh",typeof(MeshRenderer),typeof(MeshFilter));
MeshFilter fi = go.GetComponent<MeshFilter>();
fi.mesh = mesh;
MeshRenderer re = go.GetComponent<MeshRenderer>();
re.material = squareMat;
lastRender.Add (go);
} else {
ClearPrevious ();
GameObject ob = new GameObject ("LineRenderer",typeof(LineRenderer));
LineRenderer lr = ob.GetComponent<LineRenderer>();
lr.sharedMaterial = lineMat;
lr.SetWidth (lineWidth,lineWidth);
lr.SetVertexCount (p.vectorPath.Count);
for (int i=0;i<p.vectorPath.Count;i++) {
lr.SetPosition (i,p.vectorPath[i] + pathOffset);
}
lastRender.Add (ob);
}
}
void ApplyNow (Path somePath) {
lock (lockObject) {
waitingForApply = false;
AstarPath.OnPathPreSearch -= ApplyNow;
InversePrevious ();
if (destroyed) return;
//Calculate a new seed
int seed = seedGenerator.Next ();
rnd = new System.Random (seed);
if (toBeApplied != null) {
int rndStart = rnd.Next (randomStep);
for (int i=rndStart;i<toBeApplied.Length;i+= rnd.Next (1,randomStep)) {
toBeApplied[i].Penalty = (uint)(toBeApplied[i].Penalty+penalty);
}
}
prevPenalty = penalty;
prevSeed = seed;
prevNodes = toBeApplied;
}
}
public IEnumerator CalculateConstantPath () {
ConstantPath constPath = ConstantPath.Construct (end.position, searchLength, OnPathComplete);
AstarPath.StartPath (constPath);
lastPath = constPath;
yield return constPath.WaitForPath();
}
/** Executes a diagonal jump search.
* \see http://en.wikipedia.org/wiki/Jump_point_search
*/
GridNode JPSJumpDiagonal ( Path path, PathHandler handler, int parentDir, int depth=0) {
// Indexing into the cache arrays from multiple threads like this should cause
// a lot of false sharing and cache trashing, but after profiling it seems
// that this is not a major concern
int threadID = handler.threadID;
int threadOffset = 8*handler.threadID;
// This is needed to make sure different threads don't overwrite each others results
// It doesn't matter if we throw away some caching done by other threads as this will only
// happen during the first few path requests
if ( JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount ) {
lock (this) {
// Check again in case another thread has already created the array
if ( JPSLastCacheID == null || JPSLastCacheID.Length < handler.totalThreadCount ) {
JPSCache = new GridNode[8*handler.totalThreadCount];
JPSDead = new byte[handler.totalThreadCount];
JPSLastCacheID = new ushort[handler.totalThreadCount];
}
}
}
if ( JPSLastCacheID[threadID] != path.pathID ) {
for ( int i = 0; i < 8; i++ ) JPSCache[i + threadOffset] = null;
JPSLastCacheID[threadID] = path.pathID;
JPSDead[threadID] = 0;
}
// Cache earlier results, major optimization
// It is important to read from it once and then return the same result,
// if we read from it twice, we might get different results due to other threads clearing the array sometimes
GridNode cachedResult = JPSCache[parentDir + threadOffset];
if ( cachedResult != null ) {
//return cachedResult;
}
//if ( ((JPSDead[threadID] >> parentDir)&1) != 0 ) return null;
// Special node (e.g end node), take care of
if ( handler.GetPathNode(this).flag2 ) {
//Debug.Log ("Found end Node!");
//Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green);
JPSCache[parentDir + threadOffset] = this;
return this;
}
#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
// Special node which has custom connections, take care of
if ( connections != null && connections.Length > 0 ) {
JPSCache[parentDir] = this;
return this;
}
#endif
int noncyclic = gridFlags;//We don't actually need to & with this because we don't use the other bits. & 0xFF;
int cyclic = 0;
for ( int i = 0; i < 8; i++ ) cyclic |= ((noncyclic >> i)&0x1) << JPSCyclic[i];
int forced = 0;
int cyclicParentDir = JPSCyclic[parentDir];
// Loop around to be able to assume -X is where we came from
cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF;
int natural;
for ( int i = 0; i < 8; i++ ) if ( ((cyclic >> i)&1) == 0 ) forced |= JPSForcedDiagonal[i];
natural = JPSNaturalDiagonalNeighbours;
/*
if ( ((Vector3)position - new Vector3(1.5f,0,-1.5f)).magnitude < 0.5f ) {
Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir);
Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2));
}*/
// Don't force nodes we cannot reach anyway
forced &= cyclic;
natural &= cyclic;
if ( (forced & (~natural)) != 0 ) {
// Some of the neighbour nodes are forced
JPSCache[parentDir+threadOffset] = this;
return this;
}
int forwardDir;
GridGraph gg = GetGridGraph (GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
GridNode[] nodes = gg.nodes;
{
// Rotate 180 degrees - 1 node
forwardDir = 3;
if ( ((cyclic >> forwardDir)&1) != 0 ) {
int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8];
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.black);
GridNode v;
if ( oi < 4) {
v = JPSJumpStraight ( other, path, handler, JPSInverseCyclic[(cyclicParentDir-1+8)%8], depth+1 );
} else {
v = other.JPSJumpDiagonal ( path, handler, JPSInverseCyclic[(cyclicParentDir-1+8)%8], depth+1 );
}
if ( v != null ) {
JPSCache[parentDir+threadOffset] = this;
return this;
}
}
// Rotate 180 degrees + 1 node
forwardDir = 5;
if ( ((cyclic >> forwardDir)&1) != 0 ) {
int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8];
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.grey);
GridNode v;
if ( oi < 4) {
v = JPSJumpStraight ( other, path, handler, JPSInverseCyclic[(cyclicParentDir+1+8)%8], depth+1 );
} else {
v = other.JPSJumpDiagonal ( path, handler, JPSInverseCyclic[(cyclicParentDir+1+8)%8], depth+1 );
}
if ( v != null ) {
JPSCache[parentDir+threadOffset] = this;
return this;
}
}
}
// Rotate 180 degrees
forwardDir = 4;
if ( ((cyclic >> forwardDir)&1) != 0 ) {
int oi = JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8];
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta);
var v = other.JPSJumpDiagonal ( path, handler, parentDir, depth+1 );
if (v != null) {
JPSCache[parentDir+threadOffset] = v;
return v;
}
}
JPSDead[threadID] |= (byte)(1 << parentDir);
return null;
}
