A Quadtree implementation in D with unit tests

quadtree.d

import std.traits: hasMember;

/*
    Quadtree implementation that allows the collision resolver 
    to only scan a small subset of the possible colliding objects

    See Also:
    http://gamedevelopment.tutsplus.com/tutorials/quick-tip-use-quadtrees-to-detect-likely-collisions-in-2d-space--gamedev-374
*/
struct Quadtree(T, uint MAX_OBJECTS = 10, uint MAX_LEVELS = 5) if (
    hasMember!(T, "x") &&
    hasMember!(T, "y") &&
    hasMember!(T, "width") &&
    hasMember!(T, "height")
) {
    private int level;
    private T bounds;
    T[] objects = [];
    Quadtree[] nodes = [];


    this(const int p_level, const T p_bounds) pure nothrow {
        this.level = p_level;
        this.bounds = p_bounds;

        if (p_level == 0) {
            this.split();
        }
    }


    /*
        Clears the quadtree
    */
    void clear() pure nothrow @nogc @safe {
        this.objects = [];

        foreach (ref node; this.nodes) {
            node.clear();
        }
    }


    /*
        Splits the node into 4 subnodes
    */
    void split() pure nothrow {
        immutable int sub_width = this.bounds.width / 2;
        immutable int sub_height = this.bounds.height / 2;
        immutable int x = this.bounds.x;
        immutable int y = this.bounds.y;

        this.nodes ~= Quadtree(
            this.level+1,
            T(x + sub_width, y, sub_width, sub_height)
        );
        this.nodes ~= Quadtree(
            this.level+1,
            T(x, y, sub_width, sub_height)
        );
        this.nodes ~= Quadtree(
            this.level+1,
            T(x, y + sub_height, sub_width, sub_height)
        );
        this.nodes ~= Quadtree(
            this.level+1,
            T(x + sub_width, y + sub_height, sub_width, sub_height)
        );
    }


    /*
        Determine which node the object belongs to. -1 means
        object cannot completely fit within a child node and is part
        of the parent node
    */
    int getIndex(const T p_rect) const pure nothrow @nogc @safe {
        int index = -1;
        immutable double vertical_midpoint = bounds.x + (bounds.width / 2);
        immutable double horizontal_midpoint = bounds.y + (bounds.height / 2);

        // Object can completely fit within the top quadrants
        immutable bool top_quadrant = (p_rect.y < horizontal_midpoint && p_rect.y + p_rect.height < horizontal_midpoint);
        // Object can completely fit within the bottom quadrants
        immutable bool bottom_quadrant = (p_rect.y > horizontal_midpoint);

        // Object can completely fit within the left quadrants
        if (p_rect.x < vertical_midpoint && p_rect.x + p_rect.width < vertical_midpoint) {
            if (top_quadrant) {
                index = 1;
            }
            else if (bottom_quadrant) {
                index = 2;
            }
        }
        // Object can completely fit within the right quadrants
        else if (p_rect.x > vertical_midpoint) {
            if (top_quadrant) {
                index = 0;
             }
            else if (bottom_quadrant) {
                index = 3;
            }
        }

       return index;
    }


    /*
        Insert the object into the quadtree. If the node
        exceeds the capacity, it will split and add all
        objects to their corresponding nodes.
    */
    void insert(const T p_rect) pure {
        import std.algorithm : remove;

        if (this.nodes.length != 0) {
            immutable int index = this.getIndex(p_rect);

            if (index != -1) {
                this.nodes[index].insert(p_rect);
                return;
            }
        }

        this.objects ~= p_rect;

        if (this.objects.length > MAX_OBJECTS && this.level < MAX_LEVELS) {
            if (this.nodes.length == 0) { 
                this.split(); 
            }

            int i = 0;

            while (i < this.objects.length) {
                immutable int index = this.getIndex(this.objects[i]);

                if (index != -1) {
                    this.nodes[index].insert(this.objects[i]);
                    this.objects = this.objects.remove(i);
                } else {
                    i++;
                }
            }
        }
    }


    /*
        Return all objects that could collide with the given object
    */
    T[] retrieve(const T p_rect) pure nothrow @safe {
        immutable int index = this.getIndex(p_rect);
        T[] return_objects = [];

        if (index != -1 && this.nodes.length != 0) {
            return_objects ~= this.nodes[index].retrieve(p_rect);
        }
        
        return_objects ~= this.objects;

        return return_objects;
    }
} unittest {
    struct Rect {
    	int x;
    	int y;
    	int width;
    	int height;
    }

    auto a = Quadtree!Rect(0, Rect(0, 0, 100, 100));
    // Did the tree correctly initialize?
    assert(a.nodes.length == 4);

    auto rects = [
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10),
        Rect(10, 10, 10, 10)
    ];
    auto rects2 = [
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10),
        Rect(80, 80, 10, 10)
    ];

    foreach (rect; rects) {
        a.insert(rect);
    }
    foreach (rect; rects2) {
        a.insert(rect);
    }

    // Did the child node create four new nodes?
    assert(a.objects.length == 0);
    assert(a.nodes[1].nodes.length == 4);

    // Does the retrieve method correctly only return those
    // Rect's in the second node?
    assert(a.retrieve(Rect(5, 5, 5, 5)) == rects);

    a.clear();
    // Were all of the objects actually cleared from the tree?
    assert(a.objects.length == 0);
    assert(a.nodes[1].objects.length == 0);
}