Quadtree.cpp

#include "Quadtree.h"

int Quadtree::MAX_OBJECTS = 3;

Quadtree::Quadtree(sf::FloatRect bounds, Quadtree* parent)
{
    mParent = parent;
    mAABB = bounds;
    mDebugRect.setFillColor(sf::Color::Transparent);
    mDebugRect.setOutlineThickness(1.0f);
    mDebugRect.setOutlineColor(sf::Color::Blue);
    mDebugRect.setSize(sf::Vector2f(bounds.width - 4, bounds.height - 4));
    mDebugRect.setPosition(bounds.left +2, bounds.top + 2);
}

void Quadtree::drawDebugRect(sf::RenderTarget& target)
{
    target.draw(mDebugRect);

    for (auto qtree : mChildren)
    {
        qtree->drawDebugRect(target);
    }
}

void Quadtree::insertObject(Boundable* object)
{
    mObjects.push_back(object);

    if (getLocalObjectCount() > MAX_OBJECTS && mChildren.empty())
        subdivide();
}

void Quadtree::removeObject(Boundable* object)
{
    bool isFound = false;

    if (!mObjects.empty())
    {
        auto found = std::find_if(mObjects.begin(), mObjects.end(), [&](Boundable* Qobj) {
            if (object == Qobj)
            {
                isFound = true;
                return true;
            }
            else
                return false;
        });

        if (isFound)
            mObjects.erase(found);

    }

    if (!isFound)
    {
        for (auto& qtree : mChildren)
        {
            qtree->removeObject(object);
        }
    }
}

std::set<Quadtree::Pair> Quadtree::collisionPairs()
{
    std::set<Pair> collisionSet;

    getCollisionPairs(collisionSet);

    return collisionSet;
}

int Quadtree::count()
{
    int count = 0;
    getTotalObjectCount(count);
    return count;
}

int Quadtree::getObjectLimit()
{
    return Quadtree::MAX_OBJECTS;
}

void Quadtree::setObjectLimit(int limit)
{
    Quadtree::MAX_OBJECTS = limit;
}

void Quadtree::manage()
{
    updateAll();
}

void Quadtree::clear()
{
    mObjects.clear();
    mChildren.clear();
}

void Quadtree::updateAll()
{
    for (auto it = mObjects.begin(); it != mObjects.end(); ++it)
    {

        int index = getObjIndex((*it));

        if (index == -2)
        {
            mParent->insertObject((*it));
        }
        else if (index > -1)
        {
            switch (index)
            {
            case 0:
                mChildren[0]->insertObject((*it));
                break;
            case 1:
                mChildren[1]->insertObject((*it));
                break;
            case 2:
                mChildren[2]->insertObject((*it));
                break;
            case 3:
                mChildren[3]->insertObject((*it));
                break;
            default:
                break;
            }
        }
    }

    mObjects.erase(std::remove_if(mObjects.begin(), mObjects.end(), [this](Boundable* obj){ return getObjIndex(obj) == -2 || getObjIndex(obj) > -1; }), mObjects.end());

    if (isMarkedForRemoval())
    {
        Quadtree* oldestParent = new Quadtree;
        getOldestParent(oldestParent);
        oldestParent->removeWrecks();
    }

    for (auto& c : mChildren)
    {
        c->updateAll();
    }
}

void Quadtree::subdivide()
{
    float height = mAABB.height / 2;
    float width = mAABB.width / 2;
    float x = mAABB.left;
    float y = mAABB.top;

    mChildren.push_back(new Quadtree(sf::FloatRect(x, y, width, height), this));
    mChildren.push_back(new Quadtree(sf::FloatRect(x + width, y, width, height), this));
    mChildren.push_back(new Quadtree(sf::FloatRect(x, y + height, width, height), this));
    mChildren.push_back(new Quadtree(sf::FloatRect(x + width, y + height, width, height), this));
}

void Quadtree::removeWrecks()
{
    for (auto& qtree : mChildren)
    {
        qtree->removeWrecks();
    }


    if (isMarkedForRemoval())
    {

        for (auto it = mChildren.begin(); it != mChildren.end(); ++it)
        {
            for (auto& object : (*it)->mObjects)
            {
                (*it)->mParent->insertObject(object);
            }

            (*it)->mObjects.clear();
        }

        mChildren.clear();
    }
}

bool Quadtree::isMarkedForRemoval()
{
    int count = 0;
    getTotalObjectCount(count);
    return count <= MAX_OBJECTS && !mChildren.empty();
}

bool Quadtree::intersects(Boundable* object)
{
    return mAABB.intersects(object->getBoundingRect());
}

int Quadtree::getObjIndex(Boundable* obj)
{

    // -2 = parent
    // -1 = this
    // 0 = upleft
    // 1 = upright
    // 2 = downleft
    // 3 = downright



    std::vector<int> counter;

    if (!mChildren.empty())
    {
        if (mChildren[0]->intersects(obj))
            counter.push_back(0);
        if (mChildren[1]->intersects(obj))
            counter.push_back(1);
        if (mChildren[2]->intersects(obj))
            counter.push_back(2);
        if (mChildren[3]->intersects(obj))
            counter.push_back(3);
    }

    int index;

    if (counter.size() == 1)
        index = counter[0];
    else
        index = -1;

    if (mParent == nullptr)
        return index;

    int indexFromParent;
    indexFromParent = mParent->getObjIndex(obj);

    if (indexFromParent <= -1)
        return -2;
    else
        return index;

}

int Quadtree::getLocalObjectCount()
{
    return mObjects.size();
}

void Quadtree::getTotalObjectCount(int& count)
{
    count += getLocalObjectCount();

    for (auto& qtree : mChildren)
    {
        qtree->getTotalObjectCount(count);
    }
}

void Quadtree::getOldestParent(Quadtree*& parent)
{
    if (mParent != nullptr)
    {
        parent = mParent;

        mParent->getOldestParent(parent);
    }
    else
        parent = this;
}

void Quadtree::getCollisionPairs(std::set<Quadtree::Pair> & collisionSet)
{
    for (auto& obj1 : mObjects)
    {

        for (auto& obj2 : mObjects)
        {
            if (isColliding(obj1, obj2) && obj1 != obj2)
            collisionSet.insert(std::minmax(obj1, obj2));
        }


        getCollisionPairsFromChildren(collisionSet, obj1);

    }

    for (auto& qtree : mChildren)
    {
        qtree->getCollisionPairs(collisionSet);
    }
}

void Quadtree::getCollisionPairsFromChildren(std::set<Quadtree::Pair> & collisionSet, Boundable* obj)
{
    for (auto& qtree : mChildren)
    {
        for (auto& cObj : qtree->mObjects)
        {
            if (isColliding(obj, cObj))
            collisionSet.insert(std::minmax(obj, cObj));
        }
    }

    for (auto& qtree : mChildren)
        qtree->getCollisionPairsFromChildren(collisionSet, obj);
}


bool isColliding(Boundable* obj1, Boundable* obj2)
{
    return obj1->getBoundingRect().intersects(obj2->getBoundingRect());
}