Ejemplo n.º 1
0
        /** Queries the tree for the best node, searching within a circle around \a p with the specified radius.
         * Will fill in both the constrained node and the not constrained node in the NNInfo.
         *
         * \see QueryClosest
         */
        public NNInfo QueryCircle(Int3 p, int radius, NNConstraint constraint)
        {
            if (count == 0)
            {
                return(new NNInfo(null));
            }

            var nnInfo = new NNInfo(null);

            SearchBoxCircle(0, p, radius, constraint, ref nnInfo);

            nnInfo.UpdateInfo();

            return(nnInfo);
        }
Ejemplo n.º 2
0
        public NNInfo Query(Int3 p, NNConstraint constraint)
        {
            if (count == 0)
            {
                return(new NNInfo(null));
            }

            var nnInfo = new NNInfo();

            SearchBox(0, p, constraint, ref nnInfo);

            nnInfo.UpdateInfo();

            return(nnInfo);
        }
Ejemplo n.º 3
0
        void SearchBox(int boxi, Int3 p, NNConstraint constraint, ref NNInfo nnInfo)
        {        //, int intendentLevel = 0) {
            BBTreeBox box = arr[boxi];

            if (box.node != null)
            {
                //Leaf node
                if (box.node.ContainsPoint(p))
                {
                    //Update the NNInfo

                    if (nnInfo.node == null)
                    {
                        nnInfo.node = box.node;
                    }
                    else if (Mathf.Abs((box.node.position).y - p.y) < Mathf.Abs((nnInfo.node.position).y - p.y))
                    {
                        nnInfo.node = box.node;
                    }
                    if (constraint.Suitable(box.node))
                    {
                        if (nnInfo.constrainedNode == null)
                        {
                            nnInfo.constrainedNode = box.node;
                        }
                        else if (Mathf.Abs(box.node.position.y - p.y) < Mathf.Abs(nnInfo.constrainedNode.position.y - p.y))
                        {
                            nnInfo.constrainedNode = box.node;
                        }
                    }
                }
                return;
            }

            //Search children
            if (arr[box.left].Contains(p))
            {
                SearchBox(box.left, p, constraint, ref nnInfo);
            }

            if (arr[box.right].Contains(p))
            {
                SearchBox(box.right, p, constraint, ref nnInfo);
            }
        }
Ejemplo n.º 4
0
        public NNInfo GetNearest(Int3 position, NNConstraint constraint, bool accurateNearestNode = false)
        {
            if (constraint == null)
            {
                constraint = NNConstraint.None;
            }

            //Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
            int w = (bbTree.Size.Width + bbTree.Size.Height) / 100;

            NNInfo query = bbTree.QueryCircle(position, w, constraint);

            if (query.node == null)
            {
                for (int i = 1; i <= 8; i++)
                {
                    query = bbTree.QueryCircle(position, i * i * w, constraint);
                    if (query.node != null || (i - 1) * (i - 1) * w > maxNearestNodeDistance * 2)
                    {                     // *2 for a margin
                        break;
                    }
                }
            }

            if (query.node != null)
            {
                query.clampedPosition = ClosestPointOnNode(query.node, position);
            }

            if (query.constrainedNode != null)
            {
                if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position.vec3).sqrMagnitude > maxNearestNodeDistanceSqr)
                {
                    query.constrainedNode = null;
                }
                else
                {
                    query.constClampedPosition = ClosestPointOnNode(query.constrainedNode, position);
                }
            }

            return(query);
        }
Ejemplo n.º 5
0
        public override Int3 GetNearestPosition(Int3 position)
        {
            if (IsWalkable(position))
            {
                return(position);
            }
            NNInfo info  = GetNearest(position, NNConstraint.None);
            Int3   clamp = info.clampedPosition;
            Int3   dir   = info.node.position - clamp;
            Int3   mod   = clamp;
            int    d     = 10;

            while (mod == clamp && d > 0)
            {
                mod = clamp + dir / d;
                d--;
            }
            return(mod);
        }
Ejemplo n.º 6
0
        void SearchBoxCircle(int boxi, Int3 p, int radius, NNConstraint constraint, ref NNInfo nnInfo)
        {        //, int intendentLevel = 0) {
            BBTreeBox box = arr[boxi];

            if (box.node != null)
            {
                //Leaf node
                if (NodeIntersectsCircle(box.node, p, radius))
                {
                    //Update the NNInfo

#if ASTARDEBUG
                    Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.red);
                    Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.red);
                    Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.red);
#endif

                    Int3   closest = box.node.ClosestPointOnNode(p);                   //NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
                    double dist    = (closest - p).sqrMagnitude;

                    if (nnInfo.node == null)
                    {
                        nnInfo.node            = box.node;
                        nnInfo.clampedPosition = closest;
                    }
                    else if (dist < (nnInfo.clampedPosition - p).sqrMagnitude)
                    {
                        nnInfo.node            = box.node;
                        nnInfo.clampedPosition = closest;
                    }
                    if (constraint == null || constraint.Suitable(box.node))
                    {
                        if (nnInfo.constrainedNode == null)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                        }
                        else if (dist < (nnInfo.constClampedPosition - p).sqrMagnitude)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                        }
                    }
                }
                else
                {
#if ASTARDEBUG
                    Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
                    Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
                    Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
                }
                return;
            }

#if ASTARDEBUG
            Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymin), Color.white);
            Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymax), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
            Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmin, 0, box.rect.ymax), Color.white);
            Debug.DrawLine(new Vector3(box.rect.xmax, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
#endif

            //Search children
            if (RectIntersectsCircle(arr[box.left].rect, p, radius))
            {
                SearchBoxCircle(box.left, p, radius, constraint, ref nnInfo);
            }

            if (RectIntersectsCircle(arr[box.right].rect, p, radius))
            {
                SearchBoxCircle(box.right, p, radius, constraint, ref nnInfo);
            }
        }
Ejemplo n.º 7
0
        void SearchBoxClosest(int boxi, Int3 p, ref int closestDist, NNConstraint constraint, ref NNInfo nnInfo)
        {
            BBTreeBox box = arr[boxi];

            if (box.node != null)
            {
                //Leaf node
                if (NodeIntersectsCircle(box.node, p, closestDist))
                {
                    //Update the NNInfo
#if ASTARDEBUG
                    Debug.DrawLine((Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, Color.red);
                    Debug.DrawLine((Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, Color.red);
                    Debug.DrawLine((Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, Color.red);
#endif

                    Int3 closest = box.node.ClosestPointOnNode(p);

                    if (constraint == null || constraint.Suitable(box.node))
                    {
                        double dist = (closest - p).sqrMagnitude;

                        if (nnInfo.constrainedNode == null)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                            closestDist = (int)Math.Round(Math.Sqrt(dist));
                        }
                        else if (dist < closestDist * closestDist)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                            closestDist = (int)Math.Round(Math.Sqrt(dist));
                        }
                    }
                }
                else
                {
#if ASTARDEBUG
                    Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
                    Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
                    Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
                }
            }
            else
            {
#if ASTARDEBUG
                Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymin), Color.white);
                Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymax), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
                Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmin, 0, box.rect.ymax), Color.white);
                Debug.DrawLine(new Vector3(box.rect.xmax, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
#endif

                //Search children
                if (RectIntersectsCircle(arr[box.left].rect, p, closestDist))
                {
                    SearchBoxClosest(box.left, p, ref closestDist, constraint, ref nnInfo);
                }

                if (RectIntersectsCircle(arr[box.right].rect, p, closestDist))
                {
                    SearchBoxClosest(box.right, p, ref closestDist, constraint, ref nnInfo);
                }
            }
        }
Ejemplo n.º 8
0
        /** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution.
         * Note that this function will, unlike QueryCircle, only fill in the constrained node.
         * If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
         *
         * \param p Point to search around
         * \param constraint Optionally set to constrain which nodes to return
         * \param distance The best distance for the \a previous solution. Will be updated with the best distance
         * after this search. Will be positive infinity if no node could be found.
         * Set to positive infinity if there was no previous solution.
         * \param previous This search will start from the \a previous NNInfo and improve it if possible.
         * Even if the search fails on this call, the solution will never be worse than \a previous.
         *
         * \see QueryCircle
         */
        public NNInfo QueryClosest(Int3 p, NNConstraint constraint, ref int distance, NNInfo previous)
        {
            if (count == 0)
            {
                return(previous);
            }

            SearchBoxClosest(0, p, ref distance, constraint, ref previous);

            return(previous);
        }