private RectangleWS GenerateRectangle(GameObject obj)
        {
            RectangleWS    rectObj = new RectangleWS();
            SphereCollider tempSphereCollider;
            BoxCollider    tempBoxCollider;

            if (obj.MyCollider is BoxCollider)
            {
                tempBoxCollider = (BoxCollider)obj.MyCollider;

                rectObj.Min   = tempBoxCollider.Box.Min;
                rectObj.Max   = tempBoxCollider.Box.Max;
                rectObj.Min.Y = 0.0f;
                rectObj.Max.Y = 0.0f;
                //if(rectObj.Min.Z > rectObj.Max.Z)
                //{
                //    rectObj.Max.Z = -rectObj.Max.Z;
                //    rectObj.Min.Z = -rectObj.Min.Z;
                //}
            }
            else if (obj.MyCollider is SphereCollider)
            {
                tempSphereCollider = (SphereCollider)obj.MyCollider;
                rectObj.Min        = Vector3.Zero;
                rectObj.Max        = Vector3.Zero;

                rectObj.Min.X = tempSphereCollider.Sphere.Center.X - tempSphereCollider.Sphere.Radius;
                rectObj.Min.Z = tempSphereCollider.Sphere.Center.Z - tempSphereCollider.Sphere.Radius;
                rectObj.Max.X = tempSphereCollider.Sphere.Center.X + tempSphereCollider.Sphere.Radius;
                rectObj.Max.Z = tempSphereCollider.Sphere.Center.Z + tempSphereCollider.Sphere.Radius;
            }
            else
            {
                Debug.Log("QUADTREE: Collider not found for object: " + obj.Name + ", ID " + obj.UniqueID.ToString());
                rectObj.Min = root.Rect.Min;
                rectObj.Max = root.Rect.Max;

                return(rectObj);
            }

            //float diffX = rectObj.Max.X - rectObj.Min.X;
            //float diffZ = rectObj.Max.Z - rectObj.Min.Z;

            //if (diffX > diffZ)
            //{
            //    float diffTotal = (diffX - diffZ) / 2.0f;
            //    rectObj.Min.Z -= diffTotal;
            //    rectObj.Max.Z += diffTotal;
            //}
            //else if (diffZ > diffX)
            //{
            //    float diffTotal = (diffZ - diffX) / 2.0f;
            //    rectObj.Min.X -= diffTotal;
            //    rectObj.Max.X += diffTotal;
            //}

            return(rectObj);
        }
 public QuadTreeNode(QuadTreeNode parent, RectangleWS rect)
 {
     this.Parent  = parent;
     this.Objects = new List <GameObject>();
     this.Rect    = rect;
     this.ChildTL = null;
     this.ChildTR = null;
     this.ChildBR = null;
     this.ChildBL = null;
 }
 private bool CheckIfObjectFits(ref RectangleWS rectOut, ref RectangleWS rectIn)
 {
     if (rectIn.Min.X >= rectOut.Min.X &&
         rectIn.Min.Z >= rectOut.Min.Z &&
         rectIn.Max.X <= rectOut.Max.X &&
         rectIn.Max.Z <= rectOut.Max.Z)
     {
         return(true);
     }
     else
     {
         return(false);
     }
 }
        private bool CheckIntersection(ref Plane plane, ref RectangleWS rect)
        {
            Vector3 planePoint = plane.Normal * plane.D;
            Vector3 vecMin     = planePoint - rect.Min;
            Vector3 vecMax     = planePoint - rect.Max;

            vecMin = Vector3.Normalize(vecMin);
            vecMax = Vector3.Normalize(vecMax);

            float angleMin = Math.Abs((float)Math.Acos((double)Vector3.Dot(plane.Normal, vecMin)));
            float angleMax = Math.Abs((float)Math.Acos((double)Vector3.Dot(plane.Normal, vecMax)));

            bool ifMin = angleMin >= MathHelper.PiOver2;
            bool ifMax = angleMax >= MathHelper.PiOver2;

            if (angleMin >= MathHelper.PiOver2 || angleMax >= MathHelper.PiOver2)
            {
                return(true);
            }
            else
            {
                return(false);
            }
        }
        public void Add(GameObject obj)
        {
            QuadTreeNode current;
            QuadTreeNode parent;
            bool         finished;

            current  = root;
            parent   = null;
            finished = false;

            RectangleWS rectObj = GenerateRectangle(obj);

            uint ctr = 0;

            while (!finished)
            {
                ++ctr;

                // generate rects for leaf nodes
                RectangleWS childTLRect = current.GetRectForChildTL();
                RectangleWS childTRRect = current.GetRectForChildTR();
                RectangleWS childBRRect = current.GetRectForChildBR();
                RectangleWS childBLRect = current.GetRectForChildBL();

                // check if we fit in any
                if (CheckIfObjectFits(ref childTLRect, ref rectObj))
                {
                    // yay we fit, let's generate that child if necessary and proceed further
                    if (current.ChildTL == null)
                    {
                        current.ChildTL        = new QuadTreeNode(current, childTLRect);
                        current.ChildTL.Parent = current;
                    }
                    parent  = current;
                    current = current.ChildTL;
                    continue;
                }
                else if (CheckIfObjectFits(ref childTRRect, ref rectObj))
                {
                    if (current.ChildTR == null)
                    {
                        current.ChildTR        = new QuadTreeNode(current, childTRRect);
                        current.ChildTR.Parent = current;
                    }
                    parent  = current;
                    current = current.ChildTR;
                    continue;
                }
                else if (CheckIfObjectFits(ref childBRRect, ref rectObj))
                {
                    if (current.ChildBR == null)
                    {
                        current.ChildBR        = new QuadTreeNode(current, childBRRect);
                        current.ChildBR.Parent = current;
                    }
                    parent  = current;
                    current = current.ChildBR;
                    continue;
                }
                else if (CheckIfObjectFits(ref childBLRect, ref rectObj))
                {
                    if (current.ChildBL == null)
                    {
                        current.ChildBL        = new QuadTreeNode(current, childBLRect);
                        current.ChildBL.Parent = current;
                    }
                    parent  = current;
                    current = current.ChildBL;
                    continue;
                }
                else
                {
                    // holy shit we dont fit in any so let's add ourselves to current node
                    current.Objects.Add(obj);
                    obj.MyNode = current;
                    if (obj.Dynamic)
                    {
                        dynamicObjects.Add(obj);
                    }
                    finished = true;
                    Debug.Log("Object " + obj.Name + " is on level " + ctr.ToString());

                    foreach (GameObject child in obj.GetChildren())
                    {
                        Add(child);
                    }

                    break;
                }
            }
        }
        public void Update()
        {
            int          count = dynamicObjects.Count;
            RectangleWS  currentRect;
            QuadTreeNode current;

            for (int i = 0; i < count; ++i)
            {
                if (dynamicObjects[i].MyNode == null)
                {
                    this.Add(dynamicObjects[i]);
                }

                currentRect = GenerateRectangle(dynamicObjects[i]);
                current     = dynamicObjects[i].MyNode;

                if (CheckIfObjectFits(ref current.Rect, ref currentRect))
                {
                    // we still fit - continue
                    continue;
                }
                else
                {
                    if (current.Parent == null)
                    {
                        continue;
                    }
                    // remove ourselves
                    current.Objects.Remove(dynamicObjects[i]);
                    dynamicObjects[i].MyNode = null;

                    // check parents
                    nodesToCheck.Clear();
                    nodesToCheck.Push(current.Parent);
                    while (nodesToCheck.Count > 0)
                    {
                        current = nodesToCheck.Pop();

                        // check if we fit
                        if (current != null && CheckIfObjectFits(ref current.Rect, ref currentRect))
                        {
                            // YESS! but let's check if we fit in children first
                            if (current.ChildBL != null)
                            {
                                if (CheckIfObjectFits(ref current.ChildBL.Rect, ref currentRect))
                                {
                                    nodesToCheck.Push(current.ChildBL);
                                    continue;
                                }
                            }
                            else
                            {
                                RectangleWS tempRect = current.GetRectForChildBL();
                                if (CheckIfObjectFits(ref tempRect, ref currentRect))
                                {
                                    current.ChildBL = new QuadTreeNode(current, tempRect);
                                    nodesToCheck.Push(current.ChildBL);
                                    continue;
                                }
                            }

                            if (current.ChildBR != null)
                            {
                                if (CheckIfObjectFits(ref current.ChildBR.Rect, ref currentRect))
                                {
                                    nodesToCheck.Push(current.ChildBR);
                                    continue;
                                }
                            }
                            else
                            {
                                RectangleWS tempRect = current.GetRectForChildBR();
                                if (CheckIfObjectFits(ref tempRect, ref currentRect))
                                {
                                    current.ChildBR = new QuadTreeNode(current, tempRect);
                                    nodesToCheck.Push(current.ChildBR);
                                    continue;
                                }
                            }

                            if (current.ChildTL != null)
                            {
                                if (CheckIfObjectFits(ref current.ChildTL.Rect, ref currentRect))
                                {
                                    nodesToCheck.Push(current.ChildTL);
                                    continue;
                                }
                            }
                            else
                            {
                                RectangleWS tempRect = current.GetRectForChildTL();
                                if (CheckIfObjectFits(ref tempRect, ref currentRect))
                                {
                                    current.ChildTL = new QuadTreeNode(current, tempRect);
                                    nodesToCheck.Push(current.ChildTL);
                                    continue;
                                }
                            }

                            if (current.ChildTR != null)
                            {
                                if (CheckIfObjectFits(ref current.ChildTR.Rect, ref currentRect))
                                {
                                    nodesToCheck.Push(current.ChildTR);
                                    continue;
                                }
                            }
                            else
                            {
                                RectangleWS tempRect = current.GetRectForChildTR();
                                if (CheckIfObjectFits(ref tempRect, ref currentRect))
                                {
                                    current.ChildTR = new QuadTreeNode(current, tempRect);
                                    nodesToCheck.Push(current.ChildTR);
                                    continue;
                                }
                            }

                            // we don't fit in children, so that's the end of our beautiful journey
                            current.Objects.Add(dynamicObjects[i]);
                            dynamicObjects[i].MyNode = current;
                            break;
                        }
                        else if (current != null)
                        {
                            // nope, let's push parent
                            nodesToCheck.Push(current.Parent);
                        }
                    }
                }
            }
        }