Example #1
0
        void OnDrawGizmos(int boxi, int depth)
        {
            BBTreeBox box = arr[boxi];

            var min = (Vector3) new Int3(box.rect.xmin, 0, box.rect.ymin);
            var max = (Vector3) new Int3(box.rect.xmax, 0, box.rect.ymax);

            Vector3 center = (min + max) * 0.5F;
            Vector3 size   = (max - center) * 2;

            size      = new Vector3(size.x, 1, size.z);
            center.y += depth * 2;

            Gizmos.color = AstarMath.IntToColor(depth, 1f);             //new Color (0,0,0,0.2F);
            Gizmos.DrawCube(center, size);

            if (box.node != null)
            {
            }
            else
            {
                OnDrawGizmos(box.left, depth + 1);
                OnDrawGizmos(box.right, depth + 1);
            }
        }
Example #2
0
		int GetBox ( IntRect rect ) {
			if ( count >= arr.Length ) EnsureCapacity ( count+1 );

			arr[count] = new BBTreeBox ( rect );
			count++;
			return count-1;
		}
Example #3
0
		int GetBox ( MeshNode node ) {
			if ( count >= arr.Length ) EnsureCapacity ( count+1 );

			arr[count] = new BBTreeBox ( node );
			count++;
			return count-1;
		}
Example #4
0
		void EnsureCapacity ( int c ) {
			if ( arr.Length < c ) {
				var narr = new BBTreeBox[Math.Max ( c , (int)(arr.Length*1.5f))];
				for ( int i = 0; i < count; i++ ) {
					narr[i] = arr[i];
				}
				arr = narr;
			}
		}
Example #5
0
        TriangleMeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint)
        {
            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];
                    if (node.ContainsPoint((Int3)p))
                    {
#if !SERVER
                        DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif

                        if (constraint == null || constraint.Suitable(node))
                        {
                            return(node);
                        }
                    }
                    else
                    {
#if !SERVER
                        DrawDebugNode(node, 0.0f, UnityEngine.Color.blue);
#endif
                    }
                }
            }
            else
            {
#if !SERVER
                DrawDebugRect(box.rect);
#endif

                //Search children
                if (tree[box.left].Contains(p))
                {
                    var result = SearchBoxInside(box.left, p, constraint);
                    if (result != null)
                    {
                        return(result);
                    }
                }

                if (tree[box.right].Contains(p))
                {
                    var result = SearchBoxInside(box.right, p, constraint);
                    if (result != null)
                    {
                        return(result);
                    }
                }
            }

            return(null);
        }
Example #6
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
#if !SERVER
                    DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif

                    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
            {
#if !SERVER
                DrawDebugRect(box.rect);
#endif

                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);
                }
            }
        }
Example #7
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)
                    {
#if !SERVER
                        DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif

                        if (constraint == null || constraint.Suitable(node))
                        {
                            // Update the NNInfo
                            nnInfo.constrainedNode      = node;
                            nnInfo.constClampedPosition = closest;
                            closestSqrDist = dist;
                        }
                    }
                    else
                    {
#if !SERVER
                        DrawDebugNode(node, 0.0f, UnityEngine.Color.blue);
#endif
                    }
                }
            }
            else
            {
#if !SERVER
                DrawDebugRect(box.rect);
#endif
                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);
                }
            }
        }
Example #8
0
        int GetBox(IntRect rect)
        {
            if (count >= arr.Length)
            {
                EnsureCapacity(count + 1);
            }

            arr[count] = new BBTreeBox(rect);
            count++;
            return(count - 1);
        }
Example #9
0
        int GetBox(NavMeshNode node, IntRect bounds)
        {
            if (count >= arr.Length)
            {
                EnsureCapacity(count + 1);
            }

            arr[count] = new BBTreeBox(node, bounds);
            count++;
            return(count - 1);
        }
Example #10
0
 void EnsureCapacity(int c)
 {
     if (arr.Length < c)
     {
         var narr = new BBTreeBox[Math.Max(c, (int)(arr.Length * 1.5f))];
         for (int i = 0; i < count; i++)
         {
             narr[i] = arr[i];
         }
         arr = narr;
     }
 }
Example #11
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);
            }
        }
Example #12
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);
            }
        }
Example #13
0
        NavMeshNode SearchBoxInside(int boxi, Int3 p, NNConstraint constraint)
        {
            BBTreeBox box = arr[boxi];

            if (box.node != null)
            {
                if (box.node.ContainsPoint(p))
                {
                    //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


                    if (constraint == null || constraint.Suitable(box.node))
                    {
                        return(box.node);
                    }
                }
                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
                NavMeshNode g;
                if (arr[box.left].Contains(p))
                {
                    g = SearchBoxInside(box.left, p, constraint);
                    if (g != null)
                    {
                        return(g);
                    }
                }

                if (arr[box.right].Contains(p))
                {
                    g = SearchBoxInside(box.right, p, constraint);
                    if (g != null)
                    {
                        return(g);
                    }
                }
            }

            return(null);
        }
Example #14
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);
                }
            }
        }