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);
}
}
}
public PathThreadInfo (int index, AstarPath astar, PathHandler runData) {
this.threadIndex = index;
this.astar = astar;
this.runData = runData;
lockObject = new object();
}
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 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) {
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 Match(
HttpRequestMessage request,
IHttpRoute route,
string parameterName,
IDictionary <string, object> values,
HttpRouteDirection routeDirection)
{
if (request == null)
{
throw new ArgumentNullException("request");
}
if (values == null)
{
throw new ArgumentNullException("values");
}
if (routeDirection != HttpRouteDirection.UriResolution)
{
return(true);
}
object oDataPathValue;
if (!values.TryGetValue(ODataRouteConstants.ODataPath, out oDataPathValue))
{
return(false);
}
string oDataPathString = oDataPathValue as string;
ODataPath path;
IEdmModel model;
try
{
request.Properties[Constants.CustomODataPath] = oDataPathString;
model = EdmModelProvider(request);
oDataPathString = (string)request.Properties[Constants.CustomODataPath];
string requestLeftPart = request.RequestUri.GetLeftPart(UriPartial.Path);
string serviceRoot = requestLeftPart;
if (!String.IsNullOrEmpty(oDataPathString))
{
serviceRoot = RemoveODataPath(serviceRoot, oDataPathString);
}
string oDataPathAndQuery = requestLeftPart.Substring(serviceRoot.Length);
if (!String.IsNullOrEmpty(request.RequestUri.Query))
{
oDataPathAndQuery += request.RequestUri.Query;
}
if (serviceRoot.EndsWith(_escapedSlash, StringComparison.OrdinalIgnoreCase))
{
serviceRoot = serviceRoot.Substring(0, serviceRoot.Length - 3);
}
path = PathHandler.Parse(model, serviceRoot, oDataPathAndQuery);
}
catch (ODataException)
{
throw new HttpResponseException(HttpStatusCode.NotFound);
}
if (path == null)
{
return(false);
}
HttpRequestMessageProperties odataProperties = request.ODataProperties();
odataProperties.Model = model;
odataProperties.PathHandler = PathHandler;
odataProperties.Path = path;
odataProperties.RouteName = RouteName;
odataProperties.RoutingConventions = RoutingConventions;
if (values.ContainsKey(ODataRouteConstants.Controller))
{
return(true);
}
string controllerName = SelectControllerName(path, request);
if (controllerName != null)
{
values[ODataRouteConstants.Controller] = controllerName;
}
return(true);
}
public static bool InSearchTree(GraphNode node, PathHandler handler, ushort pathID)
{
return(handler.GetPathNode(node).pathID == pathID);
}
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 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
}
/// <summary>
/// Internal method.
/// Performs pathfinding.
/// (asynchronous execution is assumed)
/// </summary>
/// <param name="inpData"></param>
void PathfindingThreadMethod(object inpData)
{
PathfindingParamContainer paramsData = (PathfindingParamContainer)inpData;
try
{
if (paramsData.generateEvents)
{
if (!IsAtConstraintsArea(paramsData.target))
{
lock (actionsQueue)
actionsQueue.Enqueue(new Action(() =>
{
curCond = WaitingForRequest;
ThrowUniversalEvent("EventPathfindingToOutwardPointRequested");
}));
return;
}
if (!IsAtConstraintsArea(paramsData.start))
{
actionsQueue.Enqueue(new Action(() =>
{
curCond = WaitingForRequest;
ThrowUniversalEvent("EventPathfindingFromOutwardPointRequested");
}));
return;
}
if (SpaceGraph.IsCellOccStaticOnLevel(paramsData.start, 0))
{
actionsQueue.Enqueue(new Action(() =>
{
curCond = WaitingForRequest;
ThrowUniversalEvent("EventPathfindingFromStaticOccupiedCellRequested");
}));
return;
}
if (SpaceGraph.IsCellOccStaticOnLevel(paramsData.target, 0))
{
actionsQueue.Enqueue(new Action(() =>
{
curCond = WaitingForRequest;
ThrowUniversalEvent("EventPathfindingToStaticOccupiedCellRequested");
}));
return;
}
}
List <Vector3> foundPath;
// поиск пути, результат кладется в переменную foundPath
if (paramsData.usingAlg == PathfindingAlgorithm.AStar)
{
Graph graph = new Graph(spaceManagerInstance, cTSInstance.Token, paramsData.start, paramsData.target,
paramsData.pathfindingLevel,
new SpaceConstraints {
xMin = xMin, xMax = xMax, yMin = yMin, yMax = yMax, zMin = zMin, zMax = zMax
},
heuristicFactor
);
foundPath = graph.GetWay();
}
else
{
WaveTrace waveTraceInst =
new WaveTrace(
new SpaceConstraints
{
xMin = xMin,
xMax = xMax,
yMin = yMin,
yMax = yMax,
zMin = zMin,
zMax = zMax
}, cTSInstance.Token, paramsData.start, paramsData.target);
waves = waveTraceInst.waves;
foundPath = waveTraceInst.GetWay();
}
if (foundPath == null)
{
lock (actionsQueue) actionsQueue.Enqueue(paramsData.failureActions);
return;
}
if (paramsData.inThreadOptimizePath)
{
foundPath = PathHandler.PathFancification(foundPath, paramsData.pathfindingLevel);
foundPath = PathHandler.PathOptimization(foundPath, paramsData.pathfindingLevel);
}
if (paramsData.inThreadSmoothPath)
{
foundPath = PathHandler.PathSmoothing(foundPath, paramsData.pathfindingLevel);
}
if (paramsData.inThreadOptimizePath || paramsData.inThreadSmoothPath)
{
totalLength = 0;
for (int i = 0; i < foundPath.Count - 1; i++)
{
totalLength += Vector3.Distance(foundPath[i], foundPath[i + 1]);
}
}
lock (paramsData.outPath)
{
paramsData.outPath.Clear();
paramsData.outPath.AddRange(foundPath);
}
lock (actionsQueue) actionsQueue.Enqueue(paramsData.succesActions);
}
catch (Exception ex)
{
UnityEngine.Debug.Log(ex.Message);
}
}
/** 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
}
}
/** 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;
}
/** Executes a straight jump search.
* \see http://en.wikipedia.org/wiki/Jump_point_search
*/
static GridNode JPSJumpStraight ( GridNode node, Path path, PathHandler handler, int parentDir, int depth=0) {
GridGraph gg = GetGridGraph (node.GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
GridNode[] nodes = gg.nodes;
GridNode origin = node;
// Indexing into the cache arrays from multiple threads like this should cause
// a lot of false sharing and cache trashing, but after profiling it seems
// that this is not a major concern
int threadID = handler.threadID;
int threadOffset = 8*handler.threadID;
int cyclicParentDir = JPSCyclic[parentDir];
GridNode result = null;
// Rotate 180 degrees
const int forwardDir = 4;
int forwardOffset = neighbourOffsets[JPSInverseCyclic[(forwardDir + cyclicParentDir) % 8]];
// Move forwards in the same direction
// until a node is encountered which we either
// * know the result for (memoization)
// * is a special node (flag2 set)
// * has custom connections
// * the node has a forced neighbour
// Then break out of the loop
// and start another loop which goes through the same nodes and sets the
// memoization caches to avoid expensive calls in the future
while(true) {
// This is needed to make sure different threads don't overwrite each others results
// It doesn't matter if we throw away some caching done by other threads as this will only
// happen during the first few path requests
if ( node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount ) {
lock (node) {
// Check again in case another thread has already created the array
if ( node.JPSLastCacheID == null || node.JPSLastCacheID.Length < handler.totalThreadCount ) {
node.JPSCache = new GridNode[8*handler.totalThreadCount];
node.JPSDead = new byte[handler.totalThreadCount];
node.JPSLastCacheID = new ushort[handler.totalThreadCount];
}
}
}
if ( node.JPSLastCacheID[threadID] != path.pathID ) {
for ( int i = 0; i < 8; i++ ) node.JPSCache[i + threadOffset] = null;
node.JPSLastCacheID[threadID] = path.pathID;
node.JPSDead[threadID] = 0;
}
// Cache earlier results, major optimization
// It is important to read from it once and then return the same result,
// if we read from it twice, we might get different results due to other threads clearing the array sometimes
GridNode cachedResult = node.JPSCache[parentDir + threadOffset];
if ( cachedResult != null ) {
result = cachedResult;
break;
}
if ( ((node.JPSDead[threadID] >> parentDir)&1) != 0 ) return null;
// Special node (e.g end node), take care of
if ( handler.GetPathNode(node).flag2 ) {
//Debug.Log ("Found end Node!");
//Debug.DrawRay ((Vector3)position, Vector3.up*2, Color.green);
result = node;
break;
}
#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
// Special node which has custom connections, take care of
if ( node.connections != null && node.connections.Length > 0 ) {
result = node;
break;
}
#endif
// These are the nodes this node is connected to, one bit for each of the 8 directions
int noncyclic = node.gridFlags;//We don't actually need to & with this because we don't use the other bits. & 0xFF;
int cyclic = 0;
for ( int i = 0; i < 8; i++ ) cyclic |= ((noncyclic >> i)&0x1) << JPSCyclic[i];
int forced = 0;
// Loop around to be able to assume -X is where we came from
cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF;
//for ( int i = 0; i < 8; i++ ) if ( ((cyclic >> i)&1) == 0 ) forced |= JPSForced[i];
if ( (cyclic & (1 << 2)) == 0 ) forced |= (1<<3);
if ( (cyclic & (1 << 6)) == 0 ) forced |= (1<<5);
int natural = JPSNaturalStraightNeighbours;
// Check if there are any forced neighbours which we can reach that are not natural neighbours
//if ( ((forced & cyclic) & (~(natural & cyclic))) != 0 ) {
if ( (forced & (~natural) & cyclic) != 0 ) {
// Some of the neighbour nodes are forced
result = node;
break;
}
// Make sure we can reach the next node
if ( (cyclic & (1 << forwardDir)) != 0 ) {
node = nodes[node.nodeInGridIndex + forwardOffset];
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.2f*(depth), (Vector3)other.position + Vector3.up*0.2f*(depth+1), Color.magenta);
} else {
result = null;
break;
}
}
if ( result == null ) {
while (origin != node) {
origin.JPSDead[threadID] |= (byte)(1 << parentDir);
origin = nodes[origin.nodeInGridIndex + forwardOffset];
}
} else {
while (origin != node) {
origin.JPSCache[parentDir + threadOffset] = result;
origin = nodes[origin.nodeInGridIndex + forwardOffset];
}
}
return result;
}
/** Prepares low level path variables for calculation.
* Called before a path search will take place.
* Always called before the Prepare, Initialize and CalculateStep functions
*/
public void PrepareBase (PathHandler pathHandler) {
//Path IDs have overflowed 65K, cleanup is needed
//Since pathIDs are handed out sequentially, we can do this
if (pathHandler.PathID > pathID) {
pathHandler.ClearPathIDs ();
}
//Make sure the path has a reference to the pathHandler
this.pathHandler = pathHandler;
//Assign relevant path data to the pathHandler
pathHandler.InitializeForPath (this);
// Make sure that internalTagPenalties is an array which has the length 32
if (internalTagPenalties == null || internalTagPenalties.Length != 32)
internalTagPenalties = ZeroTagPenalties;
try {
ErrorCheck ();
} catch (System.Exception e) {
ForceLogError ("Exception in path "+pathID+"\n"+e);
}
}
public void StartUpdateEnemiesPaths(PathHandler pathHandler)
{
StartCoroutine(SlowUpdateEnemiesPaths(pathHandler));
}
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 void InjectPathHandler(PathHandler pathHandler)
{
this.pathHandler = pathHandler;
}
/* Color to use for gizmos.
* Returns a color to be used for the specified node with the current debug settings (editor only).
*
* \version Since 3.6.1 this method will not handle null nodes
*/
public virtual Color NodeColor (GraphNode node, PathHandler data) {
#if !PhotonImplementation
Color c = AstarColor.NodeConnection;
switch (AstarPath.active.debugMode) {
case GraphDebugMode.Areas:
c = AstarColor.GetAreaColor (node.Area);
break;
case GraphDebugMode.Penalty:
c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)node.Penalty-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor));
break;
case GraphDebugMode.Tags:
c = AstarMath.IntToColor ((int)node.Tag,0.5F);
break;
default:
if (data == null) return AstarColor.NodeConnection;
PathNode nodeR = data.GetPathNode (node);
switch (AstarPath.active.debugMode) {
case GraphDebugMode.G:
c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.G-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor));
break;
case GraphDebugMode.H:
c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.H-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor));
break;
case GraphDebugMode.F:
c = Color.Lerp (AstarColor.ConnectionLowLerp,AstarColor.ConnectionHighLerp, ((float)nodeR.F-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor));
break;
}
break;
}
c.a *= 0.5F;
return c;
#else
return new Color (1,1,1);
#endif
}
public override void Open (Path path, PathNode pathNode, PathHandler handler)
{
throw new System.NotImplementedException ();
}