Quadtree for AABB collision boxes

Question

I have a whole bunch of entities constantly being processed and testing for collisions. I've tried to optimize this by stopping certain entity types from checking each other, dead entities not being processed and entities not checking themselves, but with the more entities I had, the slower the processing goes.

What I need is a good, clean way of implementing AABB (axis aligned bounding box) quadtree support. A way to insert an AABB of any length, width and position into the quadtree, and queering an area.

Right now I have a very basic quadtree that only takes singular points but it's buggy and can only store points. If I have a wall entity, with an AABB that extends across the whole window, then the quadtree will only store it's X and Y value in one corner of the window so collisions on the other side of the window don't occur.

Right now, my quadtree is as follows:

Creation:

//--------------Quad Tree-----------------------
quadTree = new QuadTree(screenBoundry->X() + screenBoundry->W() / 2, //X
                        screenBoundry->Y() + screenBoundry->H() / 2, //Y
                        screenBoundry->W() / 2,                      //W
                        screenBoundry->H() / 2,                      //H
                        4);                                          //Capacity

//----------------------------

QuadTree::QuadTree(double x, double y, double w, double h, int capacity)
    : mo_boundry(0)
    , mo_i_capacity(0)
    , mo_v_points()
    , mo_b_divided(false)
    , mo_i_arrayCounter(0)
    , northEast()
    , northWest()
    , southEast()
    , southWest()
{
    mo_boundry = new Rectangle(x, y, w, h);
    mo_i_capacity = capacity;
    mo_b_divided = false;
}
QuadTree::~QuadTree()
{
    delete mo_boundry;
    mo_boundry = 0;
}

Insertion:

void QuadTree::ParseEntityList(std::vector<Entity*> entityList)
{
    for (int i = 0; i < static_cast<signed int>(entityList.size()); ++i) {
        Insert(entityList.at(i), entityList.at(i)->GetPos());
    }
}

void
QuadTree::Insert(Entity* entityToReference, Vector2* entityPoint) {

    //Check if point exists in boundry
    if (!mo_boundry->Contains(entityPoint)) {
        return;
    }

    //If points are under the limit then push
    if (mo_i_arrayCounter < mo_i_capacity) {
        mo_v_points[entityToReference] = entityPoint; //Add to map
        mo_i_arrayCounter++;
    }
    else {
        //If not divided then divide
        if (!mo_b_divided) {
            SubDivide();
        }

        northEast->Insert(entityToReference, entityPoint);
        northWest->Insert(entityToReference, entityPoint);
        southEast->Insert(entityToReference, entityPoint);
        southWest->Insert(entityToReference, entityPoint);
    }
}

Subdivision:

void
QuadTree::SubDivide() {
    double x = mo_boundry->X();
    double y = mo_boundry->Y();
    double w = mo_boundry->W() / 2;
    double h = mo_boundry->H() / 2;

    //Northwest boundry (x + w, y - h, w, h)
    //Northeast boundry (x - w, y - h, w, h)
    //Southwest boundry (x + w, y + h, w, h)
    //Southeast boundry (x - w, y + h, w, h)

    northEast = new QuadTree(x + w, y - h, w, h, mo_i_capacity);
    northWest = new QuadTree(x - w, y - h, w, h, mo_i_capacity);
    southEast = new QuadTree(x + w, y + h, w, h, mo_i_capacity);
    southWest = new QuadTree(x - w, y + h, w, h, mo_i_capacity);

    mo_b_divided = true;
}

Querying:

std::vector<Entity*>
QuadTree::Query(Rectangle* range, std::vector<Entity*> found)
{
    //The current range doesn't overlap this boundry
    if (!range->Overlaps(this->mo_boundry)) {
        return found;
    }

    //For each point in this current quad
    for (std::map<Entity*, Vector2*>::iterator it = mo_v_points.begin(); it != mo_v_points.end(); ++it) {
        if (range->Contains(it->second)) {
            found.push_back(it->first);
        }
    }

    if (mo_b_divided) {
        this->northEast->Query(range, found);
        this->northWest->Query(range, found);
        this->southEast->Query(range, found);
        this->southWest->Query(range, found);
    }

    return found;
}

Clearing per frame:

void 
QuadTree::ClearTree()
{
    mo_v_points.clear();

    if (mo_b_divided) {
        this->northEast->ClearTree();
        delete northEast;
        northEast = 0;

        this->northWest->ClearTree();
        delete northWest;
        northWest = 0;

        this->southEast->ClearTree();
        delete southEast;
        southEast = 0;

        this->southWest->ClearTree();
        delete southWest;
        southWest = 0;
    }

    mo_i_arrayCounter = 0;
    mo_b_divided = false;
}

My Current solutions has a bunch of issue though.

1. Memory leaks like crazy.
2. For some reason, only stores points on the right side of the screen.
3. Doesn't support, or store AABB information for collision detection.

If anyone can give some direct answers, or some links to exactly this issue, then that would be great. I've searched around for a while now, and haven't found a solution for my exact problem or the ones I have found are usually extremely unreadable or very difficult to implement into my current solution.

Thanks heaps for any answers/help I can get

Answer 1

There are 2 ways to deal with boxes straddling a boundary. One is to keep the box in the vector of the node where it straddles the boundary and don't push it down. The other is to add the box to all subtrees it overlaps with.

For the second option all you need to do is change first if in insert to:

//Check if box overlaps boundary
if (!mo_boundry.Overlaps(entityAABB)) {
    return;
}

As an optimization when a large box gets inserted the overlaps the boundary you can try to move one of the smaller boxes in the local vector to the subtrees. Especially when the smaller box it's already fully contained within one of the subtrees. This does not need to happen during subdivision but can be done as you insert more Entities.

For the first you need to additionally check if the rectangle would overlap the boundary and if so don't push to the subtree. Here that previous optimization is mandatory.

---

A few more remarks:

you don't use the Mapness of your mo_v_points. Change it to a std::vector<std::pair<Entity*, Rectangle>> (You could even remove the Rectangle and get it from Entity in the loop). It's much more performant than the cache thrashing that std::map forces on you. Even if you need it as a map doing a quick linear search (std::find_if) on the vector is going to be faster than std::map::find for this use case.

Rectangle doesn't need to be held by pointer. Store it by value instead. (I suspect it's the same for the position in Entity).

I also see a lot of passing by pointer, for the pointer to Entity that is logical, for the position and range it isn't. Replace it with passing by const ref or by value for the small trivial structs.

Also you pass the result vector by value, this causes quite a few issues. For one it means that the subtrees don't actually add their results to the result vector but instead to a private copy. Instead pass the result vector by pointer. Or move the vector in to avoid the deep copy and capture the result:

found = this->northEast->Query(range, std::move(found));
found = this->northWest->Query(range, std::move(found));
found = this->southEast->Query(range, std::move(found));
found = this->southWest->Query(range, std::move(found));

To fix your leaks replace the raw pointer member fields with std::unique_ptr where you need the owning pointer semantics and by value where you don't need pointer semantics. Non-owning pointer can remain raw pointers (like the per-node vector holding pointers to Entities).