Unity C# GenericQuadTree

using UnityEngine;
using System.Collections.Generic;

/** Quadtree for quick nearest neighbour search of agents.
 */
public class GenericQuadtree<T> where T : UnityEngine.Component {
    const int LeafSize = 8;

    struct Node {
        public int child00;
        public int child01;
        public int child10;
        public int child11;
        public byte count;
        public List<T> linkedList;

        public void Add(T agent) {
            if (linkedList == null) linkedList = new List<T>();
            linkedList.Add(agent);
            count++;
        }

        public void Distribute(Node[] nodes, Rect r) {
            if (linkedList == null) return;

            Vector2 c = r.center;
            for (int i = 0; i < linkedList.Count; i++) {
                T obj = linkedList[i];

                if (obj.transform.position.x > c.x) {
                    if (obj.transform.position.z > c.y) {
                        nodes[child11].Add(obj);
                    }
                    else {
                        nodes[child10].Add(obj);
                    }
                }
                else {
                    if (obj.transform.position.z > c.y) {
                        nodes[child01].Add(obj);
                    }
                    else {
                        nodes[child00].Add(obj);
                    }
                }
            }
            linkedList.Clear();
            count = 0;
        }
    }

    Node[] nodes = new Node[32];
    int filledNodes = 1;
    bool dirty = false;
    Rect bounds;

    public void Clear() {
        nodes[0].count = 0;
        nodes[0].child00 = 0;
        if (nodes[0].linkedList != null) nodes[0].linkedList.Clear();
        filledNodes = 1;
    }

    System.Collections.Generic.LinkedList<T> allNodes = new LinkedList<T>();

    void ReserveFour() {
        if (filledNodes + 4 >= nodes.Length) {
            var nds = new Node[nodes.Length * 2];
            for (int i = 0; i < nodes.Length; i++) nds[i] = nodes[i];
            nodes = nds;
        }
    }

    int GetNodeIndex() {
        if (nodes[filledNodes].linkedList != null) nodes[filledNodes].linkedList.Clear();
        nodes[filledNodes].count = 0;
        nodes[filledNodes].child00 = filledNodes;
        filledNodes++;
        return filledNodes - 1;
    }

    void SetBounds(Rect r) {
        bounds = r;
    }

    public void SetDirty() {
        dirty = true;
    }

    void Rebuild() {
#if UNITY_EDITOR
        System.Console.WriteLine("Rebuilding Quadtree for " + typeof(T).Name + "... ");
#endif
        dirty = false;

        // Remove all previous data
        Clear();

        // Calculate the total bounds
        var c = allNodes.First;

        if (c == null) {
            // Nothing in the tree!
            return;
        }

        bounds = new Rect(c.Value.transform.position.x, c.Value.transform.position.z, 0, 0);
        while (c != null) {
            Vector2 p = new Vector2(c.Value.transform.position.x, c.Value.transform.position.z);
            bounds = Rect.MinMaxRect(Mathf.Min(bounds.xMin, p.x), Mathf.Min(bounds.yMin, p.y), Mathf.Max(bounds.xMax, p.x), Mathf.Max(bounds.yMax, p.y));
            c = c.Next;
        }
        SetBounds(bounds);

        // Insert all nodes
        c = allNodes.First;
        while (c != null) {
            Update(c.Value);
            c = c.Next;
        }
    }

    public LinkedListNode<T> Insert(T agent) {
        Vector2 p = new Vector2(agent.transform.position.x, agent.transform.position.z);

        if (!bounds.Contains(p)) {
            SetDirty();
        }
        else {
            if (dirty) {
                Rebuild();
            }

            Update(agent);
        }

        return allNodes.AddLast(agent);
    }

    void Update(T agent) {
        int i = 0;
        Rect r = bounds;

        Vector2 p = new Vector2(agent.transform.position.x, agent.transform.position.z);

        int depth = 0;

        while (true) {
            depth++;

            if (nodes[i].child00 == i) {
                // Leaf node. Break at depth 10 in case lots of agents ( > LeafSize ) are in the same spot
                if (nodes[i].count < LeafSize || depth > 10) {
                    nodes[i].Add(agent);
                    break;
                }
                else {
                    // Split
                    Node node = nodes[i];
                    ReserveFour();
                    node.child00 = GetNodeIndex();
                    node.child01 = GetNodeIndex();
                    node.child10 = GetNodeIndex();
                    node.child11 = GetNodeIndex();
                    nodes[i] = node;

                    nodes[i].Distribute(nodes, r);
                }
            }
            // Note, no else
            if (nodes[i].child00 != i) {
                // Not a leaf node
                Vector2 c = r.center;
                if (p.x > c.x) {
                    if (p.y > c.y) {
                        i = nodes[i].child11;
                        r = Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax);
                    }
                    else {
                        i = nodes[i].child10;
                        r = Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y);
                    }
                }
                else {
                    if (p.y > c.y) {
                        i = nodes[i].child01;
                        r = Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax);
                    }
                    else {
                        i = nodes[i].child00;
                        r = Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y);
                    }
                }
            }
        }
    }

    public void Remove(LinkedListNode<T> node) {
        allNodes.Remove(node);
        SetDirty();
    }

    public void Query(Vector2 p, float radius, List<T> results) {
        if (dirty) {
            Rebuild();
        }

        QueryRec(0, p, radius, bounds, results);
    }

    void QueryRec(int i, Vector2 p, float radius, Rect r, List<T> results) {
        if (nodes[i].child00 == i) {
            // Leaf node
            if (nodes[i].linkedList != null) {
                for (int j = 0; j < nodes[i].linkedList.Count; j++) {
                    var a = nodes[i].linkedList[j];

                    //Debug.DrawLine (a.transform.position, new Vector3(p.x,0,p.y),Color.black);
                    float dist = (new Vector2(a.transform.position.x, a.transform.position.z) - p).sqrMagnitude;
                    if (dist < radius * radius) {
                        results.Add(a);
                    }
                }
            }
        }
        else {
            // Not a leaf node
            Vector2 c = r.center;
            if (p.x - radius < c.x) {
                if (p.y - radius < c.y) {
                    QueryRec(nodes[i].child00, p, radius, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y), results);
                }
                if (p.y + radius > c.y) {
                    QueryRec(nodes[i].child01, p, radius, Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax), results);
                }
            }

            if (p.x + radius > c.x) {
                if (p.y - radius < c.y) {
                    QueryRec(nodes[i].child10, p, radius, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y), results);
                }
                if (p.y + radius > c.y) {
                    QueryRec(nodes[i].child11, p, radius, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax), results);
                }
            }
        }
    }

    public void DebugDraw() {
        DebugDrawRec(0, bounds);
    }

    void DebugDrawRec(int i, Rect r) {
        Debug.DrawLine(new Vector3(r.xMin, 0, r.yMin), new Vector3(r.xMax, 0, r.yMin), Color.white);
        Debug.DrawLine(new Vector3(r.xMax, 0, r.yMin), new Vector3(r.xMax, 0, r.yMax), Color.white);
        Debug.DrawLine(new Vector3(r.xMax, 0, r.yMax), new Vector3(r.xMin, 0, r.yMax), Color.white);
        Debug.DrawLine(new Vector3(r.xMin, 0, r.yMax), new Vector3(r.xMin, 0, r.yMin), Color.white);

        if (nodes[i].child00 != i) {
            // Not a leaf node
            Vector2 c = r.center;
            DebugDrawRec(nodes[i].child11, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
            DebugDrawRec(nodes[i].child10, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
            DebugDrawRec(nodes[i].child01, Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax));
            DebugDrawRec(nodes[i].child00, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
        }

        if (nodes[i].linkedList != null) {
            for (int j = 0; j < nodes[i].linkedList.Count; j++) {
                var a = nodes[i].linkedList[j];
                Debug.DrawLine(nodes[i].linkedList[0].transform.position + Vector3.up, a.transform.position + Vector3.up, new Color(1, 1, 0, 0.5f));
            }
        }
    }
}