Пример #1
0
        protected override void Prepare()
        {
            nnConstraint.tags = enabledTags;
            var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);

            startPoint = startNNInfo.position;
            endPoint   = startPoint;

            startIntPoint = (Int3)startPoint;
            hTarget       = (Int3)aim;      //startIntPoint;

            startNode = startNNInfo.node;
            endNode   = startNode;

#if ASTARDEBUG
            Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
            Debug.DrawLine((Vector3)endNode.position, endPoint, Color.blue);
#endif

            if (startNode == null || endNode == null)
            {
                FailWithError("Couldn't find close nodes to the start point");
                return;
            }

            if (!CanTraverse(startNode))
            {
                FailWithError("The node closest to the start point could not be traversed");
                return;
            }

            heuristicScale = aimStrength;
        }
Пример #2
0
        protected override void Prepare()
        {
            if (startNode == null)
            {
                //Initialize the NNConstraint
                nnConstraint.tags = enabledTags;
                var startNNInfo = PathFindHelper.GetNearest(originalStartPoint, nnConstraint);

                startPoint = startNNInfo.position;
                startNode  = startNNInfo.node;
            }
            else
            {
                startPoint = (Vector3)startNode.position;
            }

#if ASTARDEBUG
            Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
#endif

            if (startNode == null)
            {
                FailWithError("Couldn't find a close node to the start point");
                return;
            }

            if (!CanTraverse(startNode))
            {
                FailWithError("The node closest to the start point could not be traversed");
                return;
            }
        }
Пример #3
0
        /** Pick N random walkable nodes from all nodes in all graphs and add them to the buffer.
         *
         * Here we select N random nodes from a stream of nodes.
         * Probability of choosing the first N nodes is 1
         * Probability of choosing node i is min(N/i,1)
         * A selected node will replace a random node of the previously
         * selected ones.
         *
         * \see https://en.wikipedia.org/wiki/Reservoir_sampling
         */
        void PickNRandomNodes(int count, List <GraphNode> buffer)
        {
            int n = 0;

            var graphs = PathFindHelper.GetConfig().graphs;

            // Loop through all graphs
            for (int j = 0; j < graphs.Length; j++)
            {
                // Loop through all nodes in the graph
                graphs[j].GetNodes(node => {
                    if (!node.Destroyed && node.Walkable)
                    {
                        n++;
                        if ((GetRandom() % n) < count)
                        {
                            if (buffer.Count < count)
                            {
                                buffer.Add(node);
                            }
                            else
                            {
                                buffer[(int)(GetRandom() % buffer.Count)] = node;
                            }
                        }
                    }
                });
            }
        }
Пример #4
0
        protected override void Prepare()
        {
            nnConstraint.tags = enabledTags;
            var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);

            startNode = startNNInfo.node;
            if (startNode == null)
            {
                FailWithError("Could not find close node to the start point");
                return;
            }
        }
Пример #5
0
        /** Returns the nearest node to a position using the specified NNConstraint.
         * \param position The position to try to find a close node to
         * \param hint Can be passed to enable some graph generators to find the nearest node faster.
         * \param constraint Can for example tell the function to try to return a walkable node. If you do not get a good node back, consider calling GetNearestForce.
         */
        public virtual NNInfoInternal GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
        {
            // This is a default implementation and it is pretty slow
            // Graphs usually override this to provide faster and more specialised implementations

            float maxDistSqr = constraint == null || constraint.constrainDistance ? PathFindHelper.GetConfig().maxNearestNodeDistanceSqr : float.PositiveInfinity;

            float     minDist = float.PositiveInfinity;
            GraphNode minNode = null;

            float     minConstDist = float.PositiveInfinity;
            GraphNode minConstNode = null;

            // Loop through all nodes and find the closest suitable node
            GetNodes(node => {
                float dist = (position - (Vector3)node.position).sqrMagnitude;

                if (dist < minDist)
                {
                    minDist = dist;
                    minNode = node;
                }

                if (dist < minConstDist && dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
                {
                    minConstDist = dist;
                    minConstNode = node;
                }
            });

            var nnInfo = new NNInfoInternal(minNode);

            nnInfo.constrainedNode = minConstNode;

            if (minConstNode != null)
            {
                nnInfo.constClampedPosition = (Vector3)minConstNode.position;
            }
            else if (minNode != null)
            {
                nnInfo.constrainedNode      = minNode;
                nnInfo.constClampedPosition = (Vector3)minNode.position;
            }

            return(nnInfo);
        }
Пример #6
0
        GraphNode PickAnyWalkableNode()
        {
            var       graphs = PathFindHelper.GetConfig().graphs;
            GraphNode first  = null;

            // Find any node in the graphs
            for (int j = 0; j < graphs.Length; j++)
            {
                graphs[j].GetNodes(node => {
                    if (node != null && node.Walkable && first == null)
                    {
                        first = node;
                    }
                });
            }

            return(first);
        }
Пример #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 pooling of paths.
         * If this is not done, trying to use that path type for pooling could result in weird behaviour.
         * The best way is to reset to default values the variables declared in the extended path type and then
         * call the base function in inheriting types with base.Reset().
         */
        protected 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

            hasBeenReset      = true;
            PipelineState     = (int)PathState.Created;
            releasedNotSilent = false;

            pathHandler       = null;
            callback          = null;
            immediateCallback = null;
            errorLog          = "";
            completeState     = PathCompleteState.NotCalculated;

            path = ListPool <GraphNode> .Claim();

            vectorPath = ListPool <Vector3> .Claim();

            currentR = null;

            duration      = 0;
            searchedNodes = 0;

            nnConstraint = PathNNConstraint.Default;
            next         = null;

            heuristic      = PathFindHelper.heuristic;
            heuristicScale = PathFindHelper.heuristicScale;

            enabledTags  = -1;
            tagPenalties = null;

            pathID = PathFindHelper.GetNextPathID();

            hTarget     = Int3.zero;
            hTargetNode = null;

            traversalProvider = null;
        }
Пример #8
0
        /** Estimated cost from the specified node to the target.
         * \see https://en.wikipedia.org/wiki/A*_search_algorithm
         */
        internal uint CalculateHScore(GraphNode node)
        {
            uint h;

            switch (heuristic)
            {
            case Heuristic.Euclidean:
                h = (uint)(((GetHTarget() - node.position).costMagnitude) * heuristicScale);
                // Inlining this check and the return
                // for each case saves an extra jump.
                // This code is pretty hot
                if (hTargetNode != null)
                {
                    h = System.Math.Max(h, PathFindHelper.euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
                }
                return(h);

            case Heuristic.Manhattan:
                Int3 p2 = node.position;
                h = (uint)((System.Math.Abs(hTarget.x - p2.x) + System.Math.Abs(hTarget.y - p2.y) + System.Math.Abs(hTarget.z - p2.z)) * heuristicScale);
                if (hTargetNode != null)
                {
                    h = System.Math.Max(h, PathFindHelper.GetConfig().euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
                }
                return(h);

            case Heuristic.DiagonalManhattan:
                Int3 p = GetHTarget() - node.position;
                p.x = System.Math.Abs(p.x);
                p.y = System.Math.Abs(p.y);
                p.z = System.Math.Abs(p.z);
                int diag  = System.Math.Min(p.x, p.z);
                int diag2 = System.Math.Max(p.x, p.z);
                h = (uint)((((14 * diag) / 10) + (diag2 - diag) + p.y) * heuristicScale);
                if (hTargetNode != null)
                {
                    h = System.Math.Max(h, PathFindHelper.euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
                }
                return(h);
            }
            return(0U);
        }
Пример #9
0
        /** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
        protected override void Prepare()
        {
            //Initialize the NNConstraint
            nnConstraint.tags = enabledTags;
            var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);

            //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
            var pathNNConstraint = nnConstraint as PathNNConstraint;

            if (pathNNConstraint != null)
            {
                pathNNConstraint.SetStart(startNNInfo.node);
            }

            startPoint = startNNInfo.position;

            startIntPoint = (Int3)startPoint;
            startNode     = startNNInfo.node;

            if (startNode == null)
            {
                FailWithError("Couldn't find a node close to the start point");
                return;
            }

            if (!CanTraverse(startNode))
            {
                FailWithError("The node closest to the start point could not be traversed");
                return;
            }

            // If it is declared that this path type has an end point
            // Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage
            if (hasEndPoint)
            {
                var endNNInfo = PathFindHelper.GetNearest(endPoint, nnConstraint);
                endPoint = endNNInfo.position;
                endNode  = endNNInfo.node;

                if (endNode == null)
                {
                    FailWithError("Couldn't find a node close to the end point");
                    return;
                }

                // This should not trigger unless the user has modified the NNConstraint
                if (!CanTraverse(endNode))
                {
                    FailWithError("The node closest to the end point could not be traversed");
                    return;
                }

                // This should not trigger unless the user has modified the NNConstraint
                if (startNode.Area != endNode.Area)
                {
                    FailWithError("There is no valid path to the target");
                    return;
                }
                {
                    // Note, other methods assume hTarget is (Int3)endPoint
                    hTarget     = (Int3)endPoint;
                    hTargetNode = endNode;

                    // Mark end node with flag1 to mark it as a target point
                    pathHandler.GetPathNode(endNode).flag1 = true;
                }
            }
        }
Пример #10
0
        /** Main pathfinding method.
         * This method will calculate the paths in the pathfinding queue.
         *
         * \see CalculatePathsThreaded
         * \see StartPath
         */
        IEnumerator CalculatePaths(PathHandler pathHandler)
        {
            // Max number of ticks before yielding/sleeping
            long maxTicks   = (long)(PathFindHelper.GetConfig().maxFrameTime *10000);
            long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;

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

                while (p == null)
                {
                    try {
                        p = queue.PopNoBlock(false);
                    } catch (Exception e) {
                        yield break;
                    }

                    if (p == null)
                    {
                        yield return(null);
                    }
                }


                IPathInternals ip = (IPathInternals)p;

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

                ip.PrepareBase(pathHandler);

                // Now processing the path
                // Will advance to Processing
                ip.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);
                }

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

                ip.Prepare();

                // 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).
                    PathFindHelper.debugPathData = ip.PathHandler;
                    PathFindHelper.debugPathID   = p.pathID;

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

                    // 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
                        ip.CalculateStep(targetTick);

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

                        totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
                        // Yield/sleep so other threads can work

                        yield return(null);

                        startTicks = System.DateTime.UtcNow.Ticks;

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

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

                                        #if ProfileAstar
                    System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted);
                                        #endif
                }

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


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


                // It needs to be stored in a local variable to avoid race conditions
                var tmpOnPathPostSearch = OnPathPostSearch;
                if (tmpOnPathPostSearch != null)
                {
                    tmpOnPathPostSearch(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)
                returnQueue.Enqueue(p);

                ip.AdvanceState(PathState.ReturnQueue);

                // 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;
                }
            }
        }
Пример #11
0
        protected override void Prepare()
        {
            nnConstraint.tags = enabledTags;
            var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);

            startNode = startNNInfo.node;

            if (startNode == null)
            {
                FailWithError("Could not find start node for multi target path");
                return;
            }

            if (!CanTraverse(startNode))
            {
                FailWithError("The node closest to the start point could not be traversed");
                return;
            }

            // Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
            var pathNNConstraint = nnConstraint as PathNNConstraint;

            if (pathNNConstraint != null)
            {
                pathNNConstraint.SetStart(startNNInfo.node);
            }

            vectorPaths     = new List <Vector3> [targetPoints.Length];
            nodePaths       = new List <GraphNode> [targetPoints.Length];
            targetNodes     = new GraphNode[targetPoints.Length];
            targetsFound    = new bool[targetPoints.Length];
            targetNodeCount = targetPoints.Length;

            bool anyWalkable = false;
            bool anySameArea = false;
            bool anyNotNull  = false;

            for (int i = 0; i < targetPoints.Length; i++)
            {
                var endNNInfo = PathFindHelper.GetNearest(targetPoints[i], nnConstraint);

                targetNodes[i] = endNNInfo.node;

                targetPoints[i] = endNNInfo.position;
                if (targetNodes[i] != null)
                {
                    anyNotNull = true;
                    endNode    = targetNodes[i];
                }

                bool notReachable = false;

                if (endNNInfo.node != null && CanTraverse(endNNInfo.node))
                {
                    anyWalkable = true;
                }
                else
                {
                    notReachable = true;
                }

                if (endNNInfo.node != null && endNNInfo.node.Area == startNode.Area)
                {
                    anySameArea = true;
                }
                else
                {
                    notReachable = true;
                }

                if (notReachable)
                {
                    // Signal that the pathfinder should not look for this node because we have already found it
                    targetsFound[i] = true;
                    targetNodeCount--;
                }
            }

            startPoint = startNNInfo.position;

            startIntPoint = (Int3)startPoint;

            if (!anyNotNull)
            {
                FailWithError("Couldn't find nodes close to the all of the end points");
                return;
            }

            if (!anyWalkable)
            {
                FailWithError("No target nodes could be traversed");
                return;
            }

            if (!anySameArea)
            {
                FailWithError("There are no valid paths to the targets");
                return;
            }

            RecalculateHTarget(true);
        }
Пример #12
0
 /** Constructor for a graph node. */
 protected GraphNode()
 {
     this.nodeIndex = PathFindHelper.GetNewNodeIndex();
     PathFindHelper.InitializeNode(this);
 }