evilsatellites/EvilSatellites/Quadtree/QuadtreeNode.cs

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;

namespace EvilSatellites.Quadtree
{
    public class QuadtreeNode<T> where T : Positional
    {
        public enum Direction
        {
            UpperTopLeft,
            UpperTopRight,
            UpperBottomLeft,
            UpperBottomRight,
            LowerTopLeft,
            LowerTopRight,
            LowerBottomLeft,
            LowerBottomRight,
            Here
        }
        public List<T> Objects;
        public List<QuadtreeNode<T>> Nodes;
        public Vector3 Position;
        public Vector3 Dimensions;
        public int CurrentLevel;
        public int MaxObjects;
        public int MaxLevels;
        public QuadtreeNode(Vector3 position, Vector3 dimensions, int maxLevels, int maxObjects, int currentLevel)
        {
            this.Position = position;
            this.Dimensions = dimensions;
            Objects = new List<T>();
            Nodes = new List<QuadtreeNode<T>>();
            this.CurrentLevel = currentLevel;
            this.MaxLevels = maxLevels;
            this.MaxObjects = maxObjects;
        }

        public void Add(T obj)
        {
            if (Nodes.Count > 0)
            {
                var indecies = GetIndecies(obj.Position, obj.Dimensions);
                foreach (var index in indecies)
                {
                    if (Nodes[index].Intersects(obj.Position, obj.Dimensions))
                    {
                        Nodes[index].Add(obj);
                    }
                }
                return;
            }
            Objects.Add(obj);
            if (Objects.Count > this.MaxObjects)
            {
                if (Nodes.Count > 0)
                {
                    DistributeObjects();
                }
                else
                {
                    Split();
                }

            }
        }

        private void Split()
        {
            if (CurrentLevel >= MaxLevels)
            {
                return;
            }
            var halfWidth = Dimensions.X / 2;
            var halfHeight = Dimensions.Y / 2;
            var halfDepth = Dimensions.Z / 2;
            var upperTopLeft = new Vector3(Position.X, Position.Y + halfHeight, Position.Z+halfDepth);
            var upperTopRight = new Vector3(Position.X + halfWidth, Position.Y + halfHeight, Position.Z + halfDepth);
            var upperBottomLeft = new Vector3(Position.X, Position.Y, Position.Z + halfDepth);
            var upperBottomRight = new Vector3(Position.X + halfWidth, Position.Y, Position.Z + halfDepth);
            var lowerTopLeft = new Vector3(Position.X, Position.Y + halfHeight, Position.Z);
            var lowerTopRight = new Vector3(Position.X + halfWidth, Position.Y + halfHeight, Position.Z);
            var lowerBottomLeft = new Vector3(Position.X, Position.Y, Position.Z);
            var lowerBottomRight = new Vector3(Position.X + halfWidth, Position.Y, Position.Z);
            var upperTopLeftNode = new QuadtreeNode<T>(
                upperTopLeft,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var upperTopRightNode = new QuadtreeNode<T>(
                upperTopRight,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var upperBottomLeftNode = new QuadtreeNode<T>(
                upperBottomLeft,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var upperBottomRightNode = new QuadtreeNode<T>(
                upperBottomRight,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var lowerTopLeftNode = new QuadtreeNode<T>(
    lowerTopLeft,
    Dimensions / 2,
    MaxLevels,
    MaxObjects,
    CurrentLevel + 1
);
            var lowerTopRightNode = new QuadtreeNode<T>(
                lowerTopRight,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var lowerBottomLeftNode = new QuadtreeNode<T>(
                lowerBottomLeft,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            var lowerBottomRightNode = new QuadtreeNode<T>(
                lowerBottomRight,
                Dimensions / 2,
                MaxLevels,
                MaxObjects,
                CurrentLevel + 1
            );
            Nodes.Add(upperTopLeftNode);
            Nodes.Add(upperTopRightNode);
            Nodes.Add(upperBottomLeftNode);
            Nodes.Add(upperBottomRightNode);
            Nodes.Add(lowerTopLeftNode);
            Nodes.Add(lowerTopRightNode);
            Nodes.Add(lowerBottomLeftNode);
            Nodes.Add(lowerBottomRightNode);
            DistributeObjects();
        }

        private void DistributeObjects()
        {
            List<T> movedObjects = new List<T>();
            for (var i = 0; i < Objects.Count; i++)
            {
                var indecies = GetIndecies(Objects[i].Position, Objects[i].Dimensions);
                foreach (var index in indecies)
                {
                    if (Nodes[index].Intersects(Objects[i].Position, Objects[i].Dimensions))
                    {
                    Nodes[index].Add(Objects[i]);
                    movedObjects.Add(Objects[i]);
                    }
                }

            }
            foreach (var obj in movedObjects)
            {
                Objects.Remove(obj);
            }
            movedObjects.Clear();
        }

        public int GetIndex(Vector3 position)
        {
            if (Nodes.Count == 0)
            {
                return -1;
            }

            var halfwidth = Dimensions.X / 2;
            var halfHeight = Dimensions.Y / 2;
            var halfDepth = Dimensions.Z / 2;
            var isTop = (position.Y > this.Position.Y + halfHeight);
            var isRight = (position.X > this.Position.X + halfwidth);
            var isUpper = (position.X > this.Position.X + halfwidth);
            if (isUpper)
            {
                if (isTop)
                {
                    if (isRight)
                    {
                        return 1;
                    }
                    else
                    {
                        return 0;
                    }
                }
                else
                {
                    if (isRight)
                    {
                        return 3;
                    }
                    else
                    {
                        return 2;
                    }
                }
            }
            else
            {
                if (isTop)
                {
                    if (isRight)
                    {
                        return 5;
                    }
                    else
                    {
                        return 4;
                    }
                }
                else
                {
                    if (isRight)
                    {
                        return 7;
                    }
                    else
                    {
                        return 6;
                    }
                }
            }
        }

        public List<int> GetIndecies(Vector3 position, Vector3 dimensions)
        {
            List<int> indecies = new List<int>();
            for (var i = 0; i < Nodes.Count; i++)
            {
                if (Nodes[i].Intersects(position, dimensions)) indecies.Add(i);
            }
            return indecies;
        }

        public List<T> Find(Vector3 position, Vector3 dimensions)
        {
            var results = new List<T>();
            if (!Intersects(position, dimensions))
            {
                return results;
            }

            var index = GetIndex(position);
            if (index == -1)
            {
                return Objects;
            }

            if (!Nodes[index].Intersects(position, dimensions))
            {
                results.AddRange(Objects);
                for (var i = 0; i < Nodes.Count; i++)
                {
                    if (Nodes[i].Intersects(position, dimensions))
                    {
                        results.AddRange(Nodes[i].GetAllObjects());
                    }
                }
            }
            else
            {
                results.AddRange(Nodes[index].Find(position, dimensions));
            }
            return results;
        }

        public bool Intersects(Vector3 position, Vector3 dimensions)
        {
            return (position.X <= (this.Position.X + this.Dimensions.X) && (position.X + dimensions.X) >= this.Position.X) && (position.Y <= (this.Position.Y + this.Dimensions.Y) && (position.Y + dimensions.Y) >= this.Position.Y) && (position.Z <= (this.Position.Z + this.Dimensions.Z) && (position.Z + dimensions.Z) >= this.Position.Z);
        }

        public bool Fits(Vector3 position, Vector3 dimensions)
        {
            return (
                (position.X >= this.Position.X && position.X <= this.Position.X + this.Dimensions.X && position.Y >= this.Position.Y && position.Y <= this.Position.Y + this.Dimensions.Y) &&
                (position.X + dimensions.X >= this.Position.X && position.X + dimensions.X <= this.Position.X + this.Dimensions.X && position.Y + dimensions.Y >= this.Position.Y && position.Y + dimensions.Y <= this.Position.Y + this.Dimensions.Y)
            );
        }

        public List<T> GetAllObjects()
        {
            List<T> objs = new List<T>();
            objs.AddRange(Objects);
            if (Nodes.Count > 0)
            {
                for (var i = 0; i < Nodes.Count; i++)
                {
                    objs.AddRange(Nodes[i].GetAllObjects());
                }
            }
            return objs;
        }
    }
}