Ejemplo n.º 1
0
        /// <summary>
        /// Will calculate a number of points around center which are on the graph and are separated by clearance from each other.
        /// The maximum distance from center to any point will be radius.
        /// Points will first be tried to be laid out as previousPoints and if that fails, random points will be selected.
        /// This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
        /// this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
        /// some kind of local avoidance.
        ///
        /// TODO: Write unit tests
        /// </summary>
        /// <param name="center">The point to generate points around</param>
        /// <param name="g">The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
        /// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.</param>
        /// <param name="previousPoints">The points to use for reference. Note that these should not be in world space. They are treated as relative to center.
        ///      The new points will overwrite the existing points in the list. The result will be in world space, not relative to center.</param>
        /// <param name="radius">The final points will be at most this distance from center.</param>
        /// <param name="clearanceRadius">The points will if possible be at least this distance from each other.</param>
        public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new System.ArgumentNullException("g");
            }

            var graph = g as NavGraph;

            if (graph == null)
            {
                throw new System.ArgumentException("g is not a NavGraph");
            }

            NNInfoInternal nn = graph.GetNearestForce(center, NNConstraint.Default);

            center = nn.clampedPosition;

            if (nn.node == null)
            {
                // No valid point to start from
                return;
            }


            // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
            clearanceRadius *= clearanceRadius;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                Vector3 dir  = previousPoints[i];
                float   magn = dir.magnitude;

                if (magn > 0)
                {
                    dir /= magn;
                }

                float newMagn = radius;                //magn > radius ? radius : magn;
                dir *= newMagn;

                GraphHitInfo hit;

                int tests = 0;
                while (true)
                {
                    Vector3 pt = center + dir;

                    if (g.Linecast(center, pt, nn.node, out hit))
                    {
                        if (hit.point == Vector3.zero)
                        {
                            // Oops, linecast actually failed completely
                            // try again unless we have tried lots of times
                            // then we just continue anyway
                            tests++;
                            if (tests > 8)
                            {
                                previousPoints[i] = pt;
                                break;
                            }
                        }
                        else
                        {
                            pt = hit.point;
                        }
                    }

                    bool worked = false;

                    for (float q = 0.1f; q <= 1.0f; q += 0.05f)
                    {
                        Vector3 qt = Vector3.Lerp(center, pt, q);
                        worked = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius)
                            {
                                worked = false;
                                break;
                            }
                        }

                        // Abort after 8 tests or when we have found a valid point
                        if (worked || tests > 8)
                        {
                            worked            = true;
                            previousPoints[i] = qt;
                            break;
                        }
                    }

                    // Break out of nested loop
                    if (worked)
                    {
                        break;
                    }

                    // If we could not find a valid point, reduce the clearance radius slightly to improve
                    // the chances next time
                    clearanceRadius *= 0.9f;
                    // This will pick points in 2D closer to the edge of the circle with a higher probability
                    dir   = UnityEngine.Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5);
                    dir.y = 0;
                    tests++;
                }
            }
        }
Ejemplo n.º 2
0
 private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
 {
     BBTree.BBTreeBox bbtreeBox = this.tree[boxi];
     if (bbtreeBox.IsLeaf)
     {
         TriangleMeshNode[] array = this.nodeLookup;
         for (int i = 0; i < 4; i++)
         {
             if (array[bbtreeBox.nodeOffset + i] == null)
             {
                 return;
             }
             TriangleMeshNode triangleMeshNode = array[bbtreeBox.nodeOffset + i];
             if (constraint == null || constraint.Suitable(triangleMeshNode))
             {
                 Vector3 vector = triangleMeshNode.ClosestPointOnNodeXZ(p);
                 float   num    = (vector.x - p.x) * (vector.x - p.x) + (vector.z - p.z) * (vector.z - p.z);
                 if (nnInfo.constrainedNode == null || num < closestSqrDist - 1E-06f || (num <= closestSqrDist + 1E-06f && Mathf.Abs(vector.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y)))
                 {
                     nnInfo.constrainedNode      = triangleMeshNode;
                     nnInfo.constClampedPosition = vector;
                     closestSqrDist = num;
                 }
             }
         }
     }
     else
     {
         int   left  = bbtreeBox.left;
         int   right = bbtreeBox.right;
         float num2;
         float num3;
         this.GetOrderedChildren(ref left, ref right, out num2, out num3, p);
         if (num2 <= closestSqrDist)
         {
             this.SearchBoxClosestXZ(left, p, ref closestSqrDist, constraint, ref nnInfo);
         }
         if (num3 <= closestSqrDist)
         {
             this.SearchBoxClosestXZ(right, p, ref closestSqrDist, constraint, ref nnInfo);
         }
     }
 }
Ejemplo n.º 3
0
        void SearchBoxClosest(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
        {
            BBTreeBox box = tree[boxi];

            if (box.IsLeaf)
            {
                var nodes = nodeLookup;
                for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++)
                {
                    var     node    = nodes[box.nodeOffset + i];
                    Vector3 closest = node.ClosestPointOnNode(p);
                    float   dist    = (closest - p).sqrMagnitude;
                    if (dist < closestSqrDist)
                    {
                        DrawDebugNode(node, 0.2f, Color.red);

                        if (constraint == null || constraint.Suitable(node))
                        {
                            // Update the NNInfo
                            nnInfo.constrainedNode      = node;
                            nnInfo.constClampedPosition = closest;
                            closestSqrDist = dist;
                        }
                    }
                    else
                    {
                        DrawDebugNode(node, 0.0f, Color.blue);
                    }
                }
            }
            else
            {
                DrawDebugRect(box.rect);
                int   first = box.left, second = box.right;
                float firstDist, secondDist;
                GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);

                // Search children (closest box first to improve performance)
                if (firstDist < closestSqrDist)
                {
                    SearchBoxClosest(first, p, ref closestSqrDist, constraint, ref nnInfo);
                }

                if (secondDist < closestSqrDist)
                {
                    SearchBoxClosest(second, p, ref closestSqrDist, constraint, ref nnInfo);
                }
            }
        }
Ejemplo n.º 4
0
        public static NNInfoInternal GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
        {
            Int3      pos                   = (Int3)position;
            float     minDist               = -1f;
            GraphNode minNode               = null;
            float     minConstDist          = -1f;
            GraphNode minConstNode          = null;
            float     maxDistSqr            = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr;
            GraphNodeDelegateCancelable del = delegate(GraphNode _node)
            {
                TriangleMeshNode triangleMeshNode3 = _node as TriangleMeshNode;
                if (accurateNearestNode)
                {
                    Vector3 b            = triangleMeshNode3.ClosestPointOnNode(position);
                    float   sqrMagnitude = ((Vector3)pos - b).sqrMagnitude;
                    if (minNode == null || sqrMagnitude < minDist)
                    {
                        minDist = sqrMagnitude;
                        minNode = triangleMeshNode3;
                    }
                    if (sqrMagnitude < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude < minConstDist))
                    {
                        minConstDist = sqrMagnitude;
                        minConstNode = triangleMeshNode3;
                    }
                }
                else if (!triangleMeshNode3.ContainsPoint((Int3)position))
                {
                    float sqrMagnitude2 = (triangleMeshNode3.position - pos).sqrMagnitude;
                    if (minNode == null || sqrMagnitude2 < minDist)
                    {
                        minDist = sqrMagnitude2;
                        minNode = triangleMeshNode3;
                    }
                    if (sqrMagnitude2 < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude2 < minConstDist))
                    {
                        minConstDist = sqrMagnitude2;
                        minConstNode = triangleMeshNode3;
                    }
                }
                else
                {
                    int num = Math.Abs(triangleMeshNode3.position.y - pos.y);
                    if (minNode == null || (float)num < minDist)
                    {
                        minDist = (float)num;
                        minNode = triangleMeshNode3;
                    }
                    if ((float)num < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (float)num < minConstDist))
                    {
                        minConstDist = (float)num;
                        minConstNode = triangleMeshNode3;
                    }
                }
                return(true);
            };

            graph.GetNodes(del);
            NNInfoInternal result = new NNInfoInternal(minNode);

            if (result.node != null)
            {
                TriangleMeshNode triangleMeshNode = result.node as TriangleMeshNode;
                Vector3          clampedPosition  = triangleMeshNode.ClosestPointOnNode(position);
                result.clampedPosition = clampedPosition;
            }
            result.constrainedNode = minConstNode;
            if (result.constrainedNode != null)
            {
                TriangleMeshNode triangleMeshNode2    = result.constrainedNode as TriangleMeshNode;
                Vector3          constClampedPosition = triangleMeshNode2.ClosestPointOnNode(position);
                result.constClampedPosition = constClampedPosition;
            }
            return(result);
        }
Ejemplo n.º 5
0
        /** This performs a linear search through all polygons returning the closest one.
         * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
         * complying with the NNConstraint.
         * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
         */
        public static NNInfoInternal GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
        {
            var pos = (Int3)position;

            float     minDist = -1;
            GraphNode minNode = null;

            float     minConstDist = -1;
            GraphNode minConstNode = null;

            float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;

            GraphNodeDelegateCancelable del = delegate(GraphNode _node) {
                var node = _node as TriangleMeshNode;

                if (accurateNearestNode)
                {
                    Vector3 closest = node.ClosestPointOnNode(position);
                    float   dist    = ((Vector3)pos - closest).sqrMagnitude;

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

                    if (dist < maxDistSqr && constraint.Suitable(node))
                    {
                        if (minConstNode == null || dist < minConstDist)
                        {
                            minConstDist = dist;
                            minConstNode = node;
                        }
                    }
                }
                else
                {
                    if (!node.ContainsPoint((Int3)position))
                    {
                        float dist = (node.position - pos).sqrMagnitude;
                        if (minNode == null || dist < minDist)
                        {
                            minDist = dist;
                            minNode = node;
                        }

                        if (dist < maxDistSqr && constraint.Suitable(node))
                        {
                            if (minConstNode == null || dist < minConstDist)
                            {
                                minConstDist = dist;
                                minConstNode = node;
                            }
                        }
                    }
                    else
                    {
                        int dist = System.Math.Abs(node.position.y - pos.y);

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

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

            graph.GetNodes(del);

            var nninfo = new NNInfoInternal(minNode);

            //Find the point closest to the nearest triangle

            if (nninfo.node != null)
            {
                var node = nninfo.node as TriangleMeshNode;                //minNode2 as MeshNode;

                Vector3 clP = node.ClosestPointOnNode(position);

                nninfo.clampedPosition = clP;
            }

            nninfo.constrainedNode = minConstNode;
            if (nninfo.constrainedNode != null)
            {
                var node = nninfo.constrainedNode as TriangleMeshNode;                //minNode2 as MeshNode;

                Vector3 clP = node.ClosestPointOnNode(position);

                nninfo.constClampedPosition = clP;
            }

            return(nninfo);
        }
Ejemplo n.º 6
0
        /// <summary>
        /// Queries the tree for the closest node to p constrained by the NNConstraint trying to improve an existing solution.
        /// Note that this function will only fill in the constrained node.
        /// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
        /// </summary>
        /// <param name="p">Point to search around</param>
        /// <param name="constraint">Optionally set to constrain which nodes to return</param>
        /// <param name="distance">The best distance for the 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>
        /// <param name="previous">This search will start from the previous NNInfo and improve it if possible.
        /// Even if the search fails on this call, the solution will never be worse than previous.</param>
        public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous)
        {
            var sqrDistance     = distance * distance;
            var origSqrDistance = sqrDistance;

            if (count > 0 && SquaredRectPointDistance(tree[0].rect, p) < sqrDistance)
            {
                SearchBoxClosest(0, p, ref sqrDistance, constraint, ref previous);
                // Only update the distance if the squared distance changed as otherwise #distance
                // might change due to rounding errors even if no better solution was found
                if (sqrDistance < origSqrDistance)
                {
                    distance = Mathf.Sqrt(sqrDistance);
                }
            }
            return(previous);
        }
Ejemplo n.º 7
0
 private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfoInternal nnInfo)
 {
     BBTree.BBTreeBox bbtreeBox = this.arr[boxi];
     if (bbtreeBox.node != null)
     {
         if (BBTree.NodeIntersectsCircle(bbtreeBox.node, p, radius))
         {
             Vector3 vector       = bbtreeBox.node.ClosestPointOnNode(p);
             float   sqrMagnitude = (vector - p).sqrMagnitude;
             if (nnInfo.node == null)
             {
                 nnInfo.node            = bbtreeBox.node;
                 nnInfo.clampedPosition = vector;
             }
             else if (sqrMagnitude < (nnInfo.clampedPosition - p).sqrMagnitude)
             {
                 nnInfo.node            = bbtreeBox.node;
                 nnInfo.clampedPosition = vector;
             }
             if ((constraint == null || constraint.Suitable(bbtreeBox.node)) && (nnInfo.constrainedNode == null || sqrMagnitude < (nnInfo.constClampedPosition - p).sqrMagnitude))
             {
                 nnInfo.constrainedNode      = bbtreeBox.node;
                 nnInfo.constClampedPosition = vector;
             }
         }
         return;
     }
     if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, radius))
     {
         this.SearchBoxCircle(bbtreeBox.left, p, radius, constraint, ref nnInfo);
     }
     if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, radius))
     {
         this.SearchBoxCircle(bbtreeBox.right, p, radius, constraint, ref nnInfo);
     }
 }
Ejemplo n.º 8
0
        public static void GetPointsAroundPoint(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new ArgumentNullException("g");
            }
            NavGraph navGraph = g as NavGraph;

            if (navGraph == null)
            {
                throw new ArgumentException("g is not a NavGraph");
            }
            NNInfoInternal nearestForce = navGraph.GetNearestForce(p, NNConstraint.Default);

            p = nearestForce.clampedPosition;
            if (nearestForce.node == null)
            {
                return;
            }
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count));
            clearanceRadius *= clearanceRadius;
            for (int i = 0; i < previousPoints.Count; i++)
            {
                Vector3 vector    = previousPoints[i];
                float   magnitude = vector.magnitude;
                if (magnitude > 0f)
                {
                    vector /= magnitude;
                }
                float num = radius;
                vector *= num;
                bool flag = false;
                int  num2 = 0;
                do
                {
                    Vector3      vector2 = p + vector;
                    GraphHitInfo graphHitInfo;
                    if (g.Linecast(p, vector2, nearestForce.node, out graphHitInfo))
                    {
                        vector2 = graphHitInfo.point;
                    }
                    for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f)
                    {
                        Vector3 vector3 = (vector2 - p) * num3 + p;
                        flag = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius)
                            {
                                flag = false;
                                break;
                            }
                        }
                        if (flag)
                        {
                            previousPoints[i] = vector3;
                            break;
                        }
                    }
                    if (!flag)
                    {
                        if (num2 > 8)
                        {
                            flag = true;
                        }
                        else
                        {
                            clearanceRadius *= 0.9f;
                            vector           = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5));
                            vector.y         = 0f;
                            num2++;
                        }
                    }
                }while (!flag);
            }
        }
Ejemplo n.º 9
0
 public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous)
 {
     if (this.count == 0)
     {
         return(previous);
     }
     this.SearchBoxClosest(0, p, ref distance, constraint, ref previous);
     return(previous);
 }
Ejemplo n.º 10
0
 private void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
 {
     BBTree.BBTreeBox bbtreeBox = this.arr[boxi];
     if (bbtreeBox.node != null)
     {
         if (BBTree.NodeIntersectsCircle(bbtreeBox.node, p, closestDist))
         {
             Vector3 vector = bbtreeBox.node.ClosestPointOnNode(p);
             if (constraint == null || constraint.Suitable(bbtreeBox.node))
             {
                 float sqrMagnitude = (vector - p).sqrMagnitude;
                 if (nnInfo.constrainedNode == null || sqrMagnitude < closestDist * closestDist)
                 {
                     nnInfo.constrainedNode      = bbtreeBox.node;
                     nnInfo.constClampedPosition = vector;
                     closestDist = (float)Math.Sqrt((double)sqrMagnitude);
                 }
             }
         }
     }
     else
     {
         if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, closestDist))
         {
             this.SearchBoxClosest(bbtreeBox.left, p, ref closestDist, constraint, ref nnInfo);
         }
         if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, closestDist))
         {
             this.SearchBoxClosest(bbtreeBox.right, p, ref closestDist, constraint, ref nnInfo);
         }
     }
 }
Ejemplo n.º 11
0
 private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
 {
     BBTree.BBTreeBox bbtreeBox = this.arr[boxi];
     if (bbtreeBox.node != null)
     {
         Vector3 constClampedPosition = bbtreeBox.node.ClosestPointOnNodeXZ(p);
         if (constraint == null || constraint.Suitable(bbtreeBox.node))
         {
             float num = (constClampedPosition.x - p.x) * (constClampedPosition.x - p.x) + (constClampedPosition.z - p.z) * (constClampedPosition.z - p.z);
             if (nnInfo.constrainedNode == null || num < closestDist * closestDist)
             {
                 nnInfo.constrainedNode      = bbtreeBox.node;
                 nnInfo.constClampedPosition = constClampedPosition;
                 closestDist = (float)Math.Sqrt((double)num);
             }
         }
     }
     else
     {
         if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, closestDist))
         {
             this.SearchBoxClosestXZ(bbtreeBox.left, p, ref closestDist, constraint, ref nnInfo);
         }
         if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, closestDist))
         {
             this.SearchBoxClosestXZ(bbtreeBox.right, p, ref closestDist, constraint, ref nnInfo);
         }
     }
 }
Ejemplo n.º 12
0
        void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfoInternal nnInfo)           //, int intendentLevel = 0) {
        {
            BBTreeBox box = arr[boxi];

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

                    DrawDebugNode(box.node, 0.0f, Color.red);

                    Vector3 closest = box.node.ClosestPointOnNode(p);                     //NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
                    float   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 || dist < (nnInfo.constClampedPosition - p).sqrMagnitude)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                        }
                    }
                }
                else
                {
                    DrawDebugNode(box.node, 0.0f, Color.blue);
                }
                return;
            }

            DrawDebugRect(box.rect);

            //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.º 13
0
        void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
        {
            BBTreeBox box = arr[boxi];

            if (box.node != null)
            {
                //Leaf node
                if (NodeIntersectsCircle(box.node, p, closestDist))
                {
                    DrawDebugNode(box.node, 0.2f, Color.red);

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

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

                        //Update the NNInfo
                        if (nnInfo.constrainedNode == null || dist < closestDist * closestDist)
                        {
                            nnInfo.constrainedNode      = box.node;
                            nnInfo.constClampedPosition = closest;
                            closestDist = (float)Math.Sqrt(dist);
                        }
                    }
                }
                else
                {
                    DrawDebugNode(box.node, 0.0f, Color.blue);
                }
            }
            else
            {
                DrawDebugRect(box.rect);

                //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.º 14
0
        public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous)
        {
            float num  = distance * distance;
            float num2 = num;

            if (this.count > 0 && BBTree.SquaredRectPointDistance(this.tree[0].rect, p) < num)
            {
                this.SearchBoxClosest(0, p, ref num, constraint, ref previous);
                if (num < num2)
                {
                    distance = Mathf.Sqrt(num);
                }
            }
            return(previous);
        }
        // Token: 0x060024F1 RID: 9457 RVA: 0x0019C554 File Offset: 0x0019A754
        public override NNInfoInternal GetNearestForce(Vector3 position, NNConstraint constraint)
        {
            if (this.nodes == null || this.depth * this.width * this.layerCount != this.nodes.Length || this.layerCount == 0)
            {
                return(default(NNInfoInternal));
            }
            Vector3 vector = position;

            position = base.transform.InverseTransform(position);
            float         x             = position.x;
            float         z             = position.z;
            int           num           = Mathf.Clamp((int)x, 0, this.width - 1);
            int           num2          = Mathf.Clamp((int)z, 0, this.depth - 1);
            float         num3          = float.PositiveInfinity;
            int           num4          = 2;
            LevelGridNode levelGridNode = this.GetNearestNode(vector, num, num2, constraint);

            if (levelGridNode != null)
            {
                num3 = ((Vector3)levelGridNode.position - vector).sqrMagnitude;
            }
            if (levelGridNode != null && num4 > 0)
            {
                num4--;
            }
            float num5 = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
            float num6 = num5 * num5;
            int   num7 = 1;

            for (;;)
            {
                int i = num2 + num7;
                if (this.nodeSize * (float)num7 > num5)
                {
                    break;
                }
                int j;
                for (j = num - num7; j <= num + num7; j++)
                {
                    if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
                    {
                        LevelGridNode nearestNode = this.GetNearestNode(vector, j, i, constraint);
                        if (nearestNode != null)
                        {
                            float sqrMagnitude = ((Vector3)nearestNode.position - vector).sqrMagnitude;
                            if (sqrMagnitude < num3 && sqrMagnitude < num6)
                            {
                                num3          = sqrMagnitude;
                                levelGridNode = nearestNode;
                            }
                        }
                    }
                }
                i = num2 - num7;
                for (j = num - num7; j <= num + num7; j++)
                {
                    if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
                    {
                        LevelGridNode nearestNode2 = this.GetNearestNode(vector, j, i, constraint);
                        if (nearestNode2 != null)
                        {
                            float sqrMagnitude2 = ((Vector3)nearestNode2.position - vector).sqrMagnitude;
                            if (sqrMagnitude2 < num3 && sqrMagnitude2 < num6)
                            {
                                num3          = sqrMagnitude2;
                                levelGridNode = nearestNode2;
                            }
                        }
                    }
                }
                j = num - num7;
                for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++)
                {
                    if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
                    {
                        LevelGridNode nearestNode3 = this.GetNearestNode(vector, j, i, constraint);
                        if (nearestNode3 != null)
                        {
                            float sqrMagnitude3 = ((Vector3)nearestNode3.position - vector).sqrMagnitude;
                            if (sqrMagnitude3 < num3 && sqrMagnitude3 < num6)
                            {
                                num3          = sqrMagnitude3;
                                levelGridNode = nearestNode3;
                            }
                        }
                    }
                }
                j = num + num7;
                for (i = num2 - num7 + 1; i <= num2 + num7 - 1; i++)
                {
                    if (j >= 0 && i >= 0 && j < this.width && i < this.depth)
                    {
                        LevelGridNode nearestNode4 = this.GetNearestNode(vector, j, i, constraint);
                        if (nearestNode4 != null)
                        {
                            float sqrMagnitude4 = ((Vector3)nearestNode4.position - vector).sqrMagnitude;
                            if (sqrMagnitude4 < num3 && sqrMagnitude4 < num6)
                            {
                                num3          = sqrMagnitude4;
                                levelGridNode = nearestNode4;
                            }
                        }
                    }
                }
                if (levelGridNode != null)
                {
                    if (num4 == 0)
                    {
                        break;
                    }
                    num4--;
                }
                num7++;
            }
            NNInfoInternal result = new NNInfoInternal(levelGridNode);

            if (levelGridNode != null)
            {
                int xcoordinateInGrid = levelGridNode.XCoordinateInGrid;
                int zcoordinateInGrid = levelGridNode.ZCoordinateInGrid;
                result.clampedPosition = base.transform.Transform(new Vector3(Mathf.Clamp(x, (float)xcoordinateInGrid, (float)xcoordinateInGrid + 1f), base.transform.InverseTransform((Vector3)levelGridNode.position).y, Mathf.Clamp(z, (float)zcoordinateInGrid, (float)zcoordinateInGrid + 1f)));
            }
            return(result);
        }
Ejemplo n.º 16
0
 private void SearchBoxClosest(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
 {
     BBTree.BBTreeBox bbtreeBox = this.tree[boxi];
     if (bbtreeBox.IsLeaf)
     {
         TriangleMeshNode[] array = this.nodeLookup;
         for (int i = 0; i < 4; i++)
         {
             if (array[bbtreeBox.nodeOffset + i] == null)
             {
                 return;
             }
             TriangleMeshNode triangleMeshNode = array[bbtreeBox.nodeOffset + i];
             Vector3          vector           = triangleMeshNode.ClosestPointOnNode(p);
             float            sqrMagnitude     = (vector - p).sqrMagnitude;
             if (sqrMagnitude < closestSqrDist && (constraint == null || constraint.Suitable(triangleMeshNode)))
             {
                 nnInfo.constrainedNode      = triangleMeshNode;
                 nnInfo.constClampedPosition = vector;
                 closestSqrDist = sqrMagnitude;
             }
         }
     }
     else
     {
         int   left  = bbtreeBox.left;
         int   right = bbtreeBox.right;
         float num;
         float num2;
         this.GetOrderedChildren(ref left, ref right, out num, out num2, p);
         if (num < closestSqrDist)
         {
             this.SearchBoxClosest(left, p, ref closestSqrDist, constraint, ref nnInfo);
         }
         if (num2 < closestSqrDist)
         {
             this.SearchBoxClosest(right, p, ref closestSqrDist, constraint, ref nnInfo);
         }
     }
 }
Ejemplo n.º 17
0
        void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
        {
            BBTreeBox box = tree[boxi];

            if (box.IsLeaf)
            {
                var nodes = nodeLookup;
                for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++)
                {
                    var node = nodes[box.nodeOffset + i];
                    // Update the NNInfo
                    DrawDebugNode(node, 0.2f, Color.red);

                    if (constraint == null || constraint.Suitable(node))
                    {
                        Vector3 closest = node.ClosestPointOnNodeXZ(p);
                        // XZ squared distance
                        float dist = (closest.x - p.x) * (closest.x - p.x) + (closest.z - p.z) * (closest.z - p.z);

                        // There's a theoretical case when the closest point is on the edge of a node which may cause the
                        // closest point's xz coordinates to not line up perfectly with p's xz coordinates even though they should
                        // (because floating point errors are annoying). So use a tiny margin to cover most of those cases.
                        const float fuzziness = 0.000001f;
                        if (nnInfo.constrainedNode == null || dist < closestSqrDist - fuzziness || (dist <= closestSqrDist + fuzziness && Mathf.Abs(closest.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y)))
                        {
                            nnInfo.constrainedNode      = node;
                            nnInfo.constClampedPosition = closest;
                            closestSqrDist = dist;
                        }
                    }
                }
            }
            else
            {
                DrawDebugRect(box.rect);

                int   first = box.left, second = box.right;
                float firstDist, secondDist;
                GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);

                // Search children (closest box first to improve performance)
                if (firstDist <= closestSqrDist)
                {
                    SearchBoxClosestXZ(first, p, ref closestSqrDist, constraint, ref nnInfo);
                }

                if (secondDist <= closestSqrDist)
                {
                    SearchBoxClosestXZ(second, p, ref closestSqrDist, constraint, ref nnInfo);
                }
            }
        }
Ejemplo n.º 18
0
 public NNInfo(NNInfoInternal internalInfo)
 {
     node     = internalInfo.node;
     position = internalInfo.clampedPosition;
 }
Ejemplo n.º 19
0
        public static NNInfoInternal GetNearest(NavMeshGraph graph, GraphNode[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
        {
            if (nodes == null || nodes.Length == 0)
            {
                Debug.LogError("NavGraph hasn't been generated yet or does not contain any nodes");
                return(new NNInfoInternal());
            }

            if (constraint == null)
            {
                constraint = NNConstraint.None;
            }


            Int3[] vertices = graph.vertices;

            //Query BBTree

            if (graph.bbTree == null)
            {
                /** \todo Change method to require a navgraph */
                return(GetNearestForce(graph, graph, position, constraint, accurateNearestNode));
            }

            //Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
            float w = (graph.bbTree.Size.width + graph.bbTree.Size.height) * 0.5F * 0.02F;

            NNInfoInternal query = graph.bbTree.QueryCircle(position, w, constraint);            //graph.bbTree.Query (position,constraint);

            if (query.node == null)
            {
                for (int i = 1; i <= 8; i++)
                {
                    query = graph.bbTree.QueryCircle(position, i * i * w, constraint);

                    if (query.node != null || (i - 1) * (i - 1) * w > AstarPath.active.maxNearestNodeDistance * 2)             // *2 for a margin
                    {
                        break;
                    }
                }
            }

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

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

            return(query);
        }