Example #1
0
 public PathThreadInfo(int index, AstarPath astar, PathHandler runData)
 {
     this.threadIndex = index;
     this.astar = astar;
     this.runData = runData;
     this.lockObject = new object();
 }
Example #2
0
		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);
				}
			}
		}
 static int PrepareBase(IntPtr L)
 {
     try
     {
         ToLua.CheckArgsCount(L, 2);
         Pathfinding.Path        obj  = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1);
         Pathfinding.PathHandler arg0 = (Pathfinding.PathHandler)ToLua.CheckObject <Pathfinding.PathHandler>(L, 2);
         obj.PrepareBase(arg0);
         return(0);
     }
     catch (Exception e)
     {
         return(LuaDLL.toluaL_exception(L, e));
     }
 }
    static int get_pathHandler(IntPtr L)
    {
        object o = null;

        try
        {
            o = ToLua.ToObject(L, 1);
            Pathfinding.Path        obj = (Pathfinding.Path)o;
            Pathfinding.PathHandler ret = obj.pathHandler;
            ToLua.PushObject(L, ret);
            return(1);
        }
        catch (Exception e)
        {
            return(LuaDLL.toluaL_exception(L, e, o, "attempt to index pathHandler on a nil value"));
        }
    }
Example #5
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);
                 }
             }
         }
     }
 }
Example #6
0
		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);
				}
			}
			
		}
Example #7
0
		/** 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;
		}
Example #8
0
		public override void Open (Path path, PathNode pathNode, PathHandler handler) {
			if (connections == null) return;
			
			bool flag2 = pathNode.flag2;
			
			for (int i=connections.Length-1;i >= 0;i--) {
				GraphNode other = connections[i];
				
				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) {
						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 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);
                        }
                    }
                }
            }
        }
Example #10
0
        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);
            });
        }
Example #11
0
		/** 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;
		}
Example #12
0
 public void PrepareBase(PathHandler pathHandler)
 {
     if (pathHandler.PathID > this.pathID)
     {
         pathHandler.ClearPathIDs();
     }
     this.pathHandler = pathHandler;
     pathHandler.InitializeForPath(this);
     if (this.internalTagPenalties == null || this.internalTagPenalties.Length != 32)
     {
         this.internalTagPenalties = Path.ZeroTagPenalties;
     }
     try
     {
         this.ErrorCheck();
     }
     catch (Exception ex)
     {
         this.ForceLogError(string.Concat(new object[]
         {
             "Exception in path ",
             this.pathID,
             "\n",
             ex
         }));
     }
 }
Example #13
0
        public override void OnDrawGizmos(bool drawNodes)
        {
            if (!drawNodes)
            {
                return;
            }

            Matrix4x4 preMatrix = matrix;

            GenerateMatrix();

            if (nodes == null)
            {
                //Scan ();
            }

            if (nodes == null)
            {
                return;
            }

            if (bbTree != null)
            {
                bbTree.OnDrawGizmos();
            }

            if (preMatrix != matrix)
            {
                //Debug.Log ("Relocating Nodes");
                RelocateNodes(preMatrix, matrix);
            }

            PathHandler debugData = AstarPath.active.debugPathData;

            for (int i = 0; i < nodes.Length; i++)
            {
                var node = nodes[i];

                Gizmos.color = NodeColor(node, AstarPath.active.debugPathData);

                if (node.Walkable)
                {
                    if (AstarPath.active.showSearchTree && debugData != null && debugData.GetPathNode(node).parent != null)
                    {
                        Gizmos.DrawLine((Vector3)node.position, (Vector3)debugData.GetPathNode(node).parent.node.position);
                    }
                    else
                    {
                        for (int q = 0; q < node.connections.Length; q++)
                        {
                            Gizmos.DrawLine((Vector3)node.position, Vector3.Lerp((Vector3)node.position, (Vector3)node.connections[q].position, 0.45f));
                        }
                    }

                    Gizmos.color = AstarColor.MeshEdgeColor;
                }
                else
                {
                    Gizmos.color = AstarColor.UnwalkableNode;
                }
                Gizmos.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1]);
                Gizmos.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2]);
                Gizmos.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0]);
            }
        }
Example #14
0
        /// <summary>
        /// Main pathfinding method (multithreaded).
        /// This method will calculate the paths in the pathfinding queue when multithreading is enabled.
        ///
        /// See: CalculatePaths
        /// See: StartPath
        /// </summary>
        void CalculatePathsThreaded(PathHandler pathHandler)
        {
#if UNITY_2017_3_OR_NEWER
            UnityEngine.Profiling.Profiler.BeginThreadProfiling("Pathfinding", "Pathfinding thread #" + (pathHandler.threadID + 1));
#endif

#if !ASTAR_FAST_BUT_NO_EXCEPTIONS
            try {
#endif

            // Max number of ticks we are allowed to continue working in one run.
            // One tick is 1/10000 of a millisecond.
            // We need to check once in a while if the thread should be stopped.
            long maxTicks   = (long)(10 * 10000);
            long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
            while (true)
            {
                // The path we are currently calculating
                Path path = queue.Pop();
#if UNITY_2017_3_OR_NEWER
                profilingSampler.Begin();
#endif
                // Access the internal implementation methods
                IPathInternals ipath = (IPathInternals)path;


                AstarProfiler.StartFastProfile(0);
                ipath.PrepareBase(pathHandler);

                // Now processing the path
                // Will advance to Processing
                ipath.AdvanceState(PathState.Processing);

                // Call some callbacks
                if (OnPathPreSearch != null)
                {
                    OnPathPreSearch(path);
                }

                // Tick for when the path started, used for calculating how long time the calculation took
                long startTicks = System.DateTime.UtcNow.Ticks;

                // Prepare the path
                ipath.Prepare();

                AstarProfiler.EndFastProfile(0);

                if (path.CompleteState == PathCompleteState.NotCalculated)
                {
                    // For visualization purposes, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
                    astar.debugPathData = ipath.PathHandler;
                    astar.debugPathID   = path.pathID;

                    AstarProfiler.StartFastProfile(1);

                    // Initialize the path, now ready to begin search
                    ipath.Initialize();

                    AstarProfiler.EndFastProfile(1);

                    // Loop while the path has not been fully calculated
                    while (path.CompleteState == PathCompleteState.NotCalculated)
                    {
                        // Do some work on the path calculation.
                        // The function will return when it has taken too much time
                        // or when it has finished calculation
                        AstarProfiler.StartFastProfile(2);
                        ipath.CalculateStep(targetTick);
                        AstarProfiler.EndFastProfile(2);

                        targetTick = System.DateTime.UtcNow.Ticks + maxTicks;

                        // Cancel function (and thus the thread) if no more paths should be accepted.
                        // This is done when the A* object is about to be destroyed
                        // The path is returned and then this function will be terminated
                        if (queue.IsTerminating)
                        {
                            path.FailWithError("AstarPath object destroyed");
                        }
                    }

                    path.duration = (System.DateTime.UtcNow.Ticks - startTicks) * 0.0001F;

#if ProfileAstar
                    System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted);
                    System.Threading.Interlocked.Add(ref AstarPath.TotalSearchTime, System.DateTime.UtcNow.Ticks - startTicks);
#endif
                }

                // Cleans up node tagging and other things
                ipath.Cleanup();

                AstarProfiler.StartFastProfile(9);

                if (path.immediateCallback != null)
                {
                    path.immediateCallback(path);
                }

                if (OnPathPostSearch != null)
                {
                    OnPathPostSearch(path);
                }

                // Push the path onto the return stack
                // It will be detected by the main Unity thread and returned as fast as possible (the next late update hopefully)
                returnQueue.Enqueue(path);

                // Will advance to ReturnQueue
                ipath.AdvanceState(PathState.ReturnQueue);

                AstarProfiler.EndFastProfile(9);
#if UNITY_2017_3_OR_NEWER
                profilingSampler.End();
#endif
            }
#if !ASTAR_FAST_BUT_NO_EXCEPTIONS
        }

        catch (System.Exception e) {
#if !NETFX_CORE
            if (e is ThreadAbortException || e is ThreadControlQueue.QueueTerminationException)
#else
            if (e is ThreadControlQueue.QueueTerminationException)
#endif
            {
                if (astar.logPathResults == PathLog.Heavy)
                {
                    Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID);
                }
                return;
            }
            Debug.LogException(e);
            Debug.LogError("Unhandled exception during pathfinding. Terminating.");
            // Unhandled exception, kill pathfinding
            queue.TerminateReceivers();
        } finally {
#if UNITY_2017_3_OR_NEWER
            UnityEngine.Profiling.Profiler.EndThreadProfiling();
#endif
        }
#endif

            Debug.LogError("Error : This part should never be reached.");
            queue.ReceiverTerminated();
        }
Example #15
0
 void IPathInternals.PrepareBase(PathHandler handler)
 {
     PrepareBase(handler);
 }
Example #16
0
        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
        }
Example #17
0
        private IEnumerator CalculatePaths(PathThreadInfo threadInfo)
        {
            int         numPaths = 0;
            PathHandler runData  = threadInfo.runData;

            if (runData.nodes == null)
            {
                throw new NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
            }
            long maxTicks   = (long)(this.astar.maxFrameTime * 10000f);
            long targetTick = DateTime.UtcNow.Ticks + maxTicks;

            for (;;)
            {
                Path p             = null;
                bool blockedBefore = false;
                while (p == null)
                {
                    try
                    {
                        p              = this.queue.PopNoBlock(blockedBefore);
                        blockedBefore |= (p == null);
                    }
                    catch (ThreadControlQueue.QueueTerminationException)
                    {
                        yield break;
                    }
                    if (p == null)
                    {
                        yield return(null);
                    }
                }
                IPathInternals ip = p;
                maxTicks = (long)(this.astar.maxFrameTime * 10000f);
                ip.PrepareBase(runData);
                ip.AdvanceState(PathState.Processing);
                Action <Path> tmpOnPathPreSearch = this.OnPathPreSearch;
                if (tmpOnPathPreSearch != null)
                {
                    tmpOnPathPreSearch(p);
                }
                numPaths++;
                long startTicks = DateTime.UtcNow.Ticks;
                long totalTicks = 0L;
                ip.Prepare();
                if (!p.IsDone())
                {
                    this.astar.debugPathData = ip.PathHandler;
                    this.astar.debugPathID   = p.pathID;
                    ip.Initialize();
                    while (!p.IsDone())
                    {
                        ip.CalculateStep(targetTick);
                        if (p.IsDone())
                        {
                            break;
                        }
                        totalTicks += DateTime.UtcNow.Ticks - startTicks;
                        yield return(null);

                        startTicks = DateTime.UtcNow.Ticks;
                        if (this.queue.IsTerminating)
                        {
                            p.Error();
                        }
                        targetTick = DateTime.UtcNow.Ticks + maxTicks;
                    }
                    totalTicks += DateTime.UtcNow.Ticks - startTicks;
                    p.duration  = (float)totalTicks * 0.0001f;
                }
                ip.Cleanup();
                OnPathDelegate tmpImmediateCallback = p.immediateCallback;
                if (tmpImmediateCallback != null)
                {
                    tmpImmediateCallback(p);
                }
                Action <Path> tmpOnPathPostSearch = this.OnPathPostSearch;
                if (tmpOnPathPostSearch != null)
                {
                    tmpOnPathPostSearch(p);
                }
                this.returnQueue.Enqueue(p);
                ip.AdvanceState(PathState.ReturnQueue);
                if (DateTime.UtcNow.Ticks > targetTick)
                {
                    yield return(null);

                    targetTick = DateTime.UtcNow.Ticks + maxTicks;
                    numPaths   = 0;
                }
            }
            yield break;
        }
Example #18
0
 private void CalculatePathsThreaded(PathThreadInfo threadInfo)
 {
     try
     {
         PathHandler runData = threadInfo.runData;
         if (runData.nodes == null)
         {
             throw new NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
         }
         long num  = (long)(this.astar.maxFrameTime * 10000f);
         long num2 = DateTime.UtcNow.Ticks + num;
         for (;;)
         {
             Path           path          = this.queue.Pop();
             IPathInternals pathInternals = path;
             num = (long)(this.astar.maxFrameTime * 10000f);
             pathInternals.PrepareBase(runData);
             pathInternals.AdvanceState(PathState.Processing);
             if (this.OnPathPreSearch != null)
             {
                 this.OnPathPreSearch(path);
             }
             long ticks = DateTime.UtcNow.Ticks;
             long num3  = 0L;
             pathInternals.Prepare();
             if (!path.IsDone())
             {
                 this.astar.debugPathData = pathInternals.PathHandler;
                 this.astar.debugPathID   = path.pathID;
                 pathInternals.Initialize();
                 while (!path.IsDone())
                 {
                     pathInternals.CalculateStep(num2);
                     if (path.IsDone())
                     {
                         break;
                     }
                     num3 += DateTime.UtcNow.Ticks - ticks;
                     Thread.Sleep(0);
                     ticks = DateTime.UtcNow.Ticks;
                     num2  = ticks + num;
                     if (this.queue.IsTerminating)
                     {
                         path.Error();
                     }
                 }
                 num3         += DateTime.UtcNow.Ticks - ticks;
                 path.duration = (float)num3 * 0.0001f;
             }
             pathInternals.Cleanup();
             if (path.immediateCallback != null)
             {
                 path.immediateCallback(path);
             }
             if (this.OnPathPostSearch != null)
             {
                 this.OnPathPostSearch(path);
             }
             this.returnQueue.Enqueue(path);
             pathInternals.AdvanceState(PathState.ReturnQueue);
             if (DateTime.UtcNow.Ticks > num2)
             {
                 Thread.Sleep(1);
                 num2 = DateTime.UtcNow.Ticks + num;
             }
         }
     }
     catch (Exception ex)
     {
         if (ex is ThreadAbortException || ex is ThreadControlQueue.QueueTerminationException)
         {
             if (this.astar.logPathResults == PathLog.Heavy)
             {
                 Debug.LogWarning("Shutting down pathfinding thread #" + threadInfo.threadIndex);
             }
             return;
         }
         Debug.LogException(ex);
         Debug.LogError("Unhandled exception during pathfinding. Terminating.");
         this.queue.TerminateReceivers();
     }
     Debug.LogError("Error : This part should never be reached.");
     this.queue.ReceiverTerminated();
 }
Example #19
0
 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);
         }
     }
 }
Example #20
0
		/* 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) {
			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;

		}
Example #21
0
        /** Main pathfinding method (multithreaded).
         * This method will calculate the paths in the pathfinding queue when multithreading is enabled.
         *
         * \see CalculatePaths
         * \see StartPath
         *
         * \astarpro
         */
        void CalculatePathsThreaded(PathThreadInfo threadInfo)
        {
            try {
                //Initialize memory for this thread
                PathHandler runData = threadInfo.runData;

                if (runData.nodes == null)
                {
                    throw new System.NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\nthreadInfo is an argument to the thread functions");
                }

                //Max number of ticks before yielding/sleeping
                long maxTicks   = (long)(astar.maxFrameTime * 10000);
                long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;

                while (true)
                {
                    //The path we are currently calculating
                    Path p = queue.Pop();

                    //Max number of ticks we are allowed to continue working in one run
                    //One tick is 1/10000 of a millisecond
                    maxTicks = (long)(astar.maxFrameTime * 10000);

                    //Trying to prevent simple modding to allow more than one thread
                    if (threadInfo.threadIndex > 0)
                    {
                        throw new System.Exception("Thread Error");
                    }

                    AstarProfiler.StartFastProfile(0);
                    p.PrepareBase(runData);

                    //Now processing the path
                    //Will advance to Processing
                    p.AdvanceState(PathState.Processing);

                    //Call some callbacks
                    if (OnPathPreSearch != null)
                    {
                        OnPathPreSearch(p);
                    }

                    //Tick for when the path started, used for calculating how long time the calculation took
                    long startTicks = System.DateTime.UtcNow.Ticks;
                    long totalTicks = 0;

                    //Prepare the path
                    p.Prepare();

                    AstarProfiler.EndFastProfile(0);

                    if (!p.IsDone())
                    {
                        //For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
                        astar.debugPath = p;

                        AstarProfiler.StartFastProfile(1);

                        //Initialize the path, now ready to begin search
                        p.Initialize();

                        AstarProfiler.EndFastProfile(1);

                        //The error can turn up in the Init function
                        while (!p.IsDone())
                        {
                            //Do some work on the path calculation.
                            //The function will return when it has taken too much time
                            //or when it has finished calculation
                            AstarProfiler.StartFastProfile(2);
                            p.CalculateStep(targetTick);
                            p.searchIterations++;

                            AstarProfiler.EndFastProfile(2);

                            // If the path has finished calculation, we can break here directly instead of sleeping
                            if (p.IsDone())
                            {
                                break;
                            }

                            // Yield/sleep so other threads can work
                            totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
                            Thread.Sleep(0);
                            startTicks = System.DateTime.UtcNow.Ticks;

                            targetTick = startTicks + maxTicks;

                            // Cancel function (and thus the thread) if no more paths should be accepted.
                            // This is done when the A* object is about to be destroyed
                            // The path is returned and then this function will be terminated
                            if (queue.IsTerminating)
                            {
                                p.Error();
                            }
                        }

                        totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
                        p.duration  = totalTicks * 0.0001F;
                    }

                    // Cleans up node tagging and other things
                    p.Cleanup();

                    AstarProfiler.StartFastProfile(9);

                    if (p.immediateCallback != null)
                    {
                        p.immediateCallback(p);
                    }

                    if (OnPathPostSearch != null)
                    {
                        OnPathPostSearch(p);
                    }

                    // Push the path onto the return stack
                    // It will be detected by the main Unity thread and returned as fast as possible (the next late update hopefully)
                    returnQueue.Enqueue(p);

                    // Will advance to ReturnQueue
                    p.AdvanceState(PathState.ReturnQueue);

                    AstarProfiler.EndFastProfile(9);

                    // Wait a bit if we have calculated a lot of paths
                    if (System.DateTime.UtcNow.Ticks > targetTick)
                    {
                        Thread.Sleep(1);
                        targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
                    }
                }
            } catch (System.Exception e) {
                                #if !NETFX_CORE
                if (e is ThreadAbortException || e is ThreadControlQueue.QueueTerminationException)
                                        #else
                if (e is ThreadControlQueue.QueueTerminationException)
                                                #endif
                {
                    if (astar.logPathResults == PathLog.Heavy)
                    {
                        Debug.LogWarning("Shutting down pathfinding thread #" + threadInfo.threadIndex);
                    }
                    return;
                }
                Debug.LogException(e);
                Debug.LogError("Unhandled exception during pathfinding. Terminating.");
                //Unhandled exception, kill pathfinding
                queue.TerminateReceivers();
            }

            Debug.LogError("Error : This part should never be reached.");
            queue.ReceiverTerminated();
        }
Example #22
0
		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
		}
Example #23
0
        /** Main pathfinding method.
         * This method will calculate the paths in the pathfinding queue.
         *
         * \see CalculatePathsThreaded
         * \see StartPath
         */
        IEnumerator CalculatePaths(PathThreadInfo threadInfo)
        {
            int numPaths = 0;

            // Initialize memory for this thread
            PathHandler runData = threadInfo.runData;

            if (runData.nodes == null)
            {
                throw new System.NullReferenceException("NodeRuns must be assigned to the threadInfo.runData.nodes field before threads are started\n" +
                                                        "threadInfo is an argument to the thread functions");
            }

            // Max number of ticks before yielding/sleeping
            long maxTicks   = (long)(astar.maxFrameTime * 10000);
            long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;

            while (true)
            {
                //The path we are currently calculating
                Path p = null;

                AstarProfiler.StartProfile("Path Queue");

                //Try to get the next path to be calculated
                bool blockedBefore = false;
                while (p == null)
                {
                    try {
                        p              = queue.PopNoBlock(blockedBefore);
                        blockedBefore |= p == null;
                    } catch (ThreadControlQueue.QueueTerminationException) {
                        yield break;
                    }

                    if (p == null)
                    {
                        AstarProfiler.EndProfile();
                        yield return(null);

                        AstarProfiler.StartProfile("Path Queue");
                    }
                }

                AstarProfiler.EndProfile();

                AstarProfiler.StartProfile("Path Calc");

                //Max number of ticks we are allowed to continue working in one run
                //One tick is 1/10000 of a millisecond
                maxTicks = (long)(astar.maxFrameTime * 10000);

                p.PrepareBase(runData);

                //Now processing the path
                //Will advance to Processing
                p.AdvanceState(PathState.Processing);

                // Call some callbacks
                // It needs to be stored in a local variable to avoid race conditions
                var tmpOnPathPreSearch = OnPathPreSearch;
                if (tmpOnPathPreSearch != null)
                {
                    tmpOnPathPreSearch(p);
                }

                numPaths++;

                //Tick for when the path started, used for calculating how long time the calculation took
                long startTicks = System.DateTime.UtcNow.Ticks;
                long totalTicks = 0;

                AstarProfiler.StartFastProfile(8);

                AstarProfiler.StartFastProfile(0);
                //Prepare the path
                AstarProfiler.StartProfile("Path Prepare");
                p.Prepare();
                AstarProfiler.EndProfile("Path Prepare");
                AstarProfiler.EndFastProfile(0);

                // Check if the Prepare call caused the path to complete
                // If this happens the path usually failed
                if (!p.IsDone())
                {
                    //For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
                    astar.debugPath = p;

                    //Initialize the path, now ready to begin search
                    AstarProfiler.StartProfile("Path Initialize");
                    p.Initialize();
                    AstarProfiler.EndProfile();

                    //The error can turn up in the Init function
                    while (!p.IsDone())
                    {
                        // Do some work on the path calculation.
                        // The function will return when it has taken too much time
                        // or when it has finished calculation
                        AstarProfiler.StartFastProfile(2);

                        AstarProfiler.StartProfile("Path Calc Step");
                        p.CalculateStep(targetTick);
                        AstarProfiler.EndFastProfile(2);
                        p.searchIterations++;

                        AstarProfiler.EndProfile();

                        // If the path has finished calculation, we can break here directly instead of sleeping
                        // Improves latency
                        if (p.IsDone())
                        {
                            break;
                        }

                        AstarProfiler.EndFastProfile(8);
                        totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
                        // Yield/sleep so other threads can work

                        AstarProfiler.EndProfile();
                        yield return(null);

                        AstarProfiler.StartProfile("Path Calc");

                        startTicks = System.DateTime.UtcNow.Ticks;
                        AstarProfiler.StartFastProfile(8);

                        //Cancel function (and thus the thread) if no more paths should be accepted.
                        //This is done when the A* object is about to be destroyed
                        //The path is returned and then this function will be terminated (see similar IF statement higher up in the function)
                        if (queue.IsTerminating)
                        {
                            p.Error();
                        }

                        targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
                    }

                    totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
                    p.duration  = totalTicks * 0.0001F;
                }

                // Cleans up node tagging and other things
                p.Cleanup();

                AstarProfiler.EndFastProfile(8);

                // Call the immediate callback
                // It needs to be stored in a local variable to avoid race conditions
                var tmpImmediateCallback = p.immediateCallback;
                if (tmpImmediateCallback != null)
                {
                    tmpImmediateCallback(p);
                }

                AstarProfiler.StartFastProfile(13);

                // It needs to be stored in a local variable to avoid race conditions
                var tmpOnPathPostSearch = OnPathPostSearch;
                if (tmpOnPathPostSearch != null)
                {
                    tmpOnPathPostSearch(p);
                }

                AstarProfiler.EndFastProfile(13);

                //Push the path onto the return stack
                //It will be detected by the main Unity thread and returned as fast as possible (the next late update)
                returnQueue.Enqueue(p);

                p.AdvanceState(PathState.ReturnQueue);

                AstarProfiler.EndProfile();

                //Wait a bit if we have calculated a lot of paths
                if (System.DateTime.UtcNow.Ticks > targetTick)
                {
                    yield return(null);

                    targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
                    numPaths   = 0;
                }
            }

            //Debug.LogError ("Error : This part should never be reached");
        }
Example #24
0
		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
		}
Example #25
0
 public abstract void Open(Path path, PathNode pathNode, PathHandler 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);
						}
					}
				}
			}
		}
Example #27
0
        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);
                        }
                    }
                }
            }
        }
Example #28
0
		public PathThreadInfo (int index, AstarPath astar, PathHandler runData) {
			this.threadIndex = index;
			this.astar = astar;
			this.runData = runData;
		}
Example #29
0
        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
        }
Example #30
0
		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);
							}
						}
					}
				}
			}

		}
Example #31
0
        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);
                        }
                    }
                }
            }
        }
Example #32
0
 public override void Open(Path path, PathNode pathNode, PathHandler handler)
 {
     throw new System.NotImplementedException();
 }
Example #33
0
        /** 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);
        }
Example #34
0
		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);
						}
					}
				}
			}
			
		}
Example #35
0
        /** 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);
        }
Example #36
0
		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);
						}
					}
				}
			}
		}
Example #37
0
        /** 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
            }
        }
Example #38
0
		/* 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) {
			
			var c = AstarColor.NodeConnection;
			var 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-AstarPath.active.debugFloor) / (AstarPath.active.debugRoof-AstarPath.active.debugFloor));
					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;
				
				var 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-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;
				}
			}
			c.a *= 0.5F;
			return c;
			
		}
Example #39
0
        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
        }
Example #40
0
		public override void Open (Path path, PathNode pathNode, PathHandler handler) {
			throw new System.NotImplementedException();
		}
 // Token: 0x06002321 RID: 8993 RVA: 0x00193A00 File Offset: 0x00191C00
 private void CalculatePathsThreaded(PathHandler pathHandler)
 {
     try
     {
         long num        = 100000L;
         long targetTick = DateTime.UtcNow.Ticks + num;
         for (;;)
         {
             Path           path          = this.queue.Pop();
             IPathInternals pathInternals = path;
             pathInternals.PrepareBase(pathHandler);
             pathInternals.AdvanceState(PathState.Processing);
             if (this.OnPathPreSearch != null)
             {
                 this.OnPathPreSearch(path);
             }
             long ticks = DateTime.UtcNow.Ticks;
             pathInternals.Prepare();
             if (!path.IsDone())
             {
                 this.astar.debugPathData = pathInternals.PathHandler;
                 this.astar.debugPathID   = path.pathID;
                 pathInternals.Initialize();
                 while (!path.IsDone())
                 {
                     pathInternals.CalculateStep(targetTick);
                     targetTick = DateTime.UtcNow.Ticks + num;
                     if (this.queue.IsTerminating)
                     {
                         path.FailWithError("AstarPath object destroyed");
                     }
                 }
                 path.duration = (float)(DateTime.UtcNow.Ticks - ticks) * 0.0001f;
             }
             pathInternals.Cleanup();
             if (path.immediateCallback != null)
             {
                 path.immediateCallback(path);
             }
             if (this.OnPathPostSearch != null)
             {
                 this.OnPathPostSearch(path);
             }
             this.returnQueue.Enqueue(path);
             pathInternals.AdvanceState(PathState.ReturnQueue);
         }
     }
     catch (Exception ex)
     {
         if (ex is ThreadAbortException || ex is ThreadControlQueue.QueueTerminationException)
         {
             if (this.astar.logPathResults == PathLog.Heavy)
             {
                 Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID);
             }
             return;
         }
         Debug.LogException(ex);
         Debug.LogError("Unhandled exception during pathfinding. Terminating.");
         this.queue.TerminateReceivers();
     }
     Debug.LogError("Error : This part should never be reached.");
     this.queue.ReceiverTerminated();
 }
Example #42
0
		/** 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;
		}
Example #43
0
        /* 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);
        }
Example #44
0
		/** 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
			}
		}
Example #45
0
 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);
             }
         }
     }
 }
Example #46
0
 /** Open the node */
 public abstract void Open(Path path, PathNode pathNode, PathHandler handler);
Example #47
0
		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 (pathNode.G + tmpCost + path.GetTraversalCost(other) < pathOther.G) {
							
							pathOther.cost = tmpCost;
							pathOther.parent = pathNode;
							
							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;
							
							UpdateRecursiveG (path, pathNode,handler);
							
							//handler.PushNode (pathNode);
						}
					}
				}
			}
		}
		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);
					}
				}
			}
		}
Example #49
0
 public PathThreadInfo(int index, AstarPath astar, PathHandler runData)
 {
     this.threadIndex = index;
     this.astar       = astar;
     this.runData     = runData;
 }
Example #50
0
 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);
                 }
             }
         }
     }
 }
        // Token: 0x06002322 RID: 8994 RVA: 0x00193BD0 File Offset: 0x00191DD0
        private IEnumerator CalculatePaths(PathHandler pathHandler)
        {
            long maxTicks   = (long)(this.astar.maxFrameTime * 10000f);
            long targetTick = DateTime.UtcNow.Ticks + maxTicks;

            for (;;)
            {
                Path p             = null;
                bool blockedBefore = false;
                while (p == null)
                {
                    try
                    {
                        p              = this.queue.PopNoBlock(blockedBefore);
                        blockedBefore |= (p == null);
                    }
                    catch (ThreadControlQueue.QueueTerminationException)
                    {
                        yield break;
                    }
                    if (p == null)
                    {
                        yield return(null);
                    }
                }
                IPathInternals ip = p;
                maxTicks = (long)(this.astar.maxFrameTime * 10000f);
                ip.PrepareBase(pathHandler);
                ip.AdvanceState(PathState.Processing);
                Action <Path> onPathPreSearch = this.OnPathPreSearch;
                if (onPathPreSearch != null)
                {
                    onPathPreSearch(p);
                }
                long ticks      = DateTime.UtcNow.Ticks;
                long totalTicks = 0L;
                ip.Prepare();
                if (!p.IsDone())
                {
                    this.astar.debugPathData = ip.PathHandler;
                    this.astar.debugPathID   = p.pathID;
                    ip.Initialize();
                    while (!p.IsDone())
                    {
                        ip.CalculateStep(targetTick);
                        if (p.IsDone())
                        {
                            break;
                        }
                        totalTicks += DateTime.UtcNow.Ticks - ticks;
                        yield return(null);

                        ticks = DateTime.UtcNow.Ticks;
                        if (this.queue.IsTerminating)
                        {
                            p.FailWithError("AstarPath object destroyed");
                        }
                        targetTick = DateTime.UtcNow.Ticks + maxTicks;
                    }
                    totalTicks += DateTime.UtcNow.Ticks - ticks;
                    p.duration  = (float)totalTicks * 0.0001f;
                }
                ip.Cleanup();
                OnPathDelegate immediateCallback = p.immediateCallback;
                if (immediateCallback != null)
                {
                    immediateCallback(p);
                }
                Action <Path> onPathPostSearch = this.OnPathPostSearch;
                if (onPathPostSearch != null)
                {
                    onPathPostSearch(p);
                }
                this.returnQueue.Enqueue(p);
                ip.AdvanceState(PathState.ReturnQueue);
                if (DateTime.UtcNow.Ticks > targetTick)
                {
                    yield return(null);

                    targetTick = DateTime.UtcNow.Ticks + maxTicks;
                }
                p  = null;
                ip = null;
            }
            yield break;
        }
Example #52
0
		/** 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);
			}
		}
Example #53
0
		/** Reset all values to their default values.
		 * 
		 * \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
		 * override this function, resetting ALL their variables to enable recycling of paths.
		 * If this is not done, trying to use that path type for pooling might result in weird behaviour.
		 * The best way is to reset to default values the variables declared in the extended path type and then
		 * call this base function in inheriting types with base.Reset ().
		 * 
		 * \warning This function should not be called manually.
		  */
		public virtual void Reset () {
#if ASTAR_POOL_DEBUG
			pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n";
			pathTraceInfo += System.Environment.StackTrace;
#endif
				
			if (AstarPath.active == null)
				throw new System.NullReferenceException ("No AstarPath object found in the scene. " +
					"Make sure there is one or do not create paths in Awake");
			
			hasBeenReset = true;
			state = (int)PathState.Created;
			releasedNotSilent = false;
			
			pathHandler = null;
			callback = null;
			_errorLog = "";
			pathCompleteState = PathCompleteState.NotCalculated;
			
			path = Pathfinding.Util.ListPool<GraphNode>.Claim();
			vectorPath = Pathfinding.Util.ListPool<Vector3>.Claim();
			
			currentR = null;
			
			duration = 0;
			searchIterations = 0;
			searchedNodes = 0;
			//calltime
			
			nnConstraint = PathNNConstraint.Default;
			next = null;
			
			radius = 0;
			walkabilityMask = -1;
			height = 0;
			turnRadius = 0;
			speed = 0;
			
			//heuristic = (Heuristic)0;
			//heuristicScale = 1F;
			heuristic = AstarPath.active.heuristic;
			heuristicScale = AstarPath.active.heuristicScale;
			
			pathID = 0;
			enabledTags = -1;
			tagPenalties = null;
			
			callTime = System.DateTime.UtcNow;
			pathID = AstarPath.active.GetNextPathID ();
			
			hTarget = Int3.zero;
		}
Example #54
0
		/** 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);

			try {
				ErrorCheck ();
			} catch (System.Exception e) {
				ForceLogError ("Exception in path "+pathID+"\n"+e.ToString());
			}
		}
Example #55
0
        public override void Open(Path path, PathNode pathNode, PathHandler handler)
        {
            ushort pid = handler.PathID;

            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);
                        }
                    }
                }
            }
        }