path: root/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp

/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// QuadTreeSpace by Erwin de Vries.
// With math corrections by Oleh Derevenko. ;)

#include <ode/common.h>
#include <ode/collision_space.h>
#include <ode/collision.h>
#include "config.h"
#include "matrix.h"
#include "collision_kernel.h"

#include "collision_space_internal.h"


#define AXIS0 0
#define AXIS1 1
#define UP 2

//#define DRAWBLOCKS

const int SPLITAXIS = 2;
const int SPLITS = SPLITAXIS * SPLITAXIS;

#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)

class Block{
public:
    dReal mMinX, mMaxX;
    dReal mMinZ, mMaxZ;

    dGeomID mFirst;
    int mGeomCount;

    Block* mParent;
    Block* mChildren;

    void Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks);

    void Collide(void* UserData, dNearCallback* Callback);
    void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback);

    void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback);

    void AddObject(dGeomID Object);
    void DelObject(dGeomID Object);
    void Traverse(dGeomID Object);

    bool Inside(const dReal* AABB);

    Block* GetBlock(const dReal* AABB);
    Block* GetBlockChild(const dReal* AABB);
};


#ifdef DRAWBLOCKS
#include "..\..\Include\drawstuff\\drawstuff.h"

static void DrawBlock(Block* Block){
    dVector3 v[8];
    v[0][AXIS0] = Block->mMinX;
    v[0][UP] = REAL(-1.0);
    v[0][AXIS1] = Block->mMinZ;

    v[1][AXIS0] = Block->mMinX;
    v[1][UP] = REAL(-1.0);
    v[1][AXIS1] = Block->mMaxZ;

    v[2][AXIS0] = Block->mMaxX;
    v[2][UP] = REAL(-1.0);
    v[2][AXIS1] = Block->mMinZ;

    v[3][AXIS0] = Block->mMaxX;
    v[3][UP] = REAL(-1.0);
    v[3][AXIS1] = Block->mMaxZ;

    v[4][AXIS0] = Block->mMinX;
    v[4][UP] = REAL(1.0);
    v[4][AXIS1] = Block->mMinZ;

    v[5][AXIS0] = Block->mMinX;
    v[5][UP] = REAL(1.0);
    v[5][AXIS1] = Block->mMaxZ;

    v[6][AXIS0] = Block->mMaxX;
    v[6][UP] = REAL(1.0);
    v[6][AXIS1] = Block->mMinZ;

    v[7][AXIS0] = Block->mMaxX;
    v[7][UP] = REAL(1.0);
    v[7][AXIS1] = Block->mMaxZ;

    // Bottom
    dsDrawLine(v[0], v[1]);
    dsDrawLine(v[1], v[3]);
    dsDrawLine(v[3], v[2]);
    dsDrawLine(v[2], v[0]);

    // Top
    dsDrawLine(v[4], v[5]);
    dsDrawLine(v[5], v[7]);
    dsDrawLine(v[7], v[6]);
    dsDrawLine(v[6], v[4]);

    // Sides
    dsDrawLine(v[0], v[4]);
    dsDrawLine(v[1], v[5]);
    dsDrawLine(v[2], v[6]);
    dsDrawLine(v[3], v[7]);
}
#endif	//DRAWBLOCKS


void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){
    dIASSERT(MinX <= MaxX);
    dIASSERT(MinZ <= MaxZ);

    mGeomCount = 0;
    mFirst = 0;

    mMinX = MinX;
    mMaxX = MaxX;

    mMinZ = MinZ;
    mMaxZ = MaxZ;

    this->mParent = Parent;

    if (Depth > 0){
        mChildren = Blocks;
        Blocks += SPLITS;

        const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS;
        const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS;

        const int ChildDepth = Depth - 1;
        int Index = 0;

        dReal ChildRightX = MinX;
        for (int i = 0; i < SPLITAXIS; i++){
            const dReal ChildLeftX = ChildRightX;
            ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;

            dReal ChildRightZ = MinZ;
            for (int j = 0; j < SPLITAXIS; j++){
                const dReal ChildLeftZ = ChildRightZ;
                ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;

                mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks);
                ++Index;
            }
        }
    }
    else mChildren = 0;
}

void Block::Collide(void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
    DrawBlock(this);
#endif
    // Collide the local list
    dxGeom* g = mFirst;
    while (g){
        if (GEOM_ENABLED(g)){
            Collide(g, g->next_ex, UserData, Callback);
        }
        g = g->next_ex;
    }

    // Recurse for children
    if (mChildren){
        for (int i = 0; i < SPLITS; i++){
            Block &CurrentChild = mChildren[i];
            if (CurrentChild.mGeomCount <= 1){	// Early out
                continue;
            }
            CurrentChild.Collide(UserData, Callback);
        }
    }
}

// Note: g2 is assumed to be in this Block
void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
    DrawBlock(this);
#endif
    // Collide against local list
    while (g2){
        if (GEOM_ENABLED(g2)){
            collideAABBs (g1, g2, UserData, Callback);
        }
        g2 = g2->next_ex;
    }

    // Collide against children
    if (mChildren){
        for (int i = 0; i < SPLITS; i++){
            Block &CurrentChild = mChildren[i];
            // Early out for empty blocks
            if (CurrentChild.mGeomCount == 0){
                continue;
            }

            // Does the geom's AABB collide with the block?
            // Don't do AABB tests for single geom blocks.
            if (CurrentChild.mGeomCount == 1){
                //
            }
            else if (true){
                if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
                    g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
                    g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
                    g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue;
            }
            CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
        }
    }
}

void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
    // Collide against local list
    dxGeom* g1 = mFirst;
    while (g1){
        if (GEOM_ENABLED(g1)){
            collideAABBs (g1, g2, UserData, Callback);
        }
        g1 = g1->next_ex;
    }
}

void Block::AddObject(dGeomID Object){
    // Add the geom
    Object->next_ex = mFirst;
    mFirst = Object;
    Object->tome_ex = (dxGeom**)this;

    // Now traverse upwards to tell that we have a geom
    Block* Block = this;
    do{
        Block->mGeomCount++;
        Block = Block->mParent;
    }
    while (Block);
}

void Block::DelObject(dGeomID Object){
    // Del the geom
    dxGeom* g = mFirst;
    dxGeom* Last = 0;
    while (g){
        if (g == Object){
            if (Last){
                Last->next_ex = g->next_ex;
            }
            else mFirst = g->next_ex;

            break;
        }
        Last = g;
        g = g->next_ex;
    }

    Object->tome_ex = 0;

    // Now traverse upwards to tell that we have lost a geom
    Block* Block = this;
    do{
        Block->mGeomCount--;
        Block = Block->mParent;
    }
    while (Block);
}

void Block::Traverse(dGeomID Object){
    Block* NewBlock = GetBlock(Object->aabb);

    if (NewBlock != this){
        // Remove the geom from the old block and add it to the new block.
        // This could be more optimal, but the loss should be very small.
        DelObject(Object);
        NewBlock->AddObject(Object);
    }
}

bool Block::Inside(const dReal* AABB){
    return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ;
}

Block* Block::GetBlock(const dReal* AABB){
    if (Inside(AABB)){
        return GetBlockChild(AABB);	// Child or this will have a good block
    }
    else if (mParent){
        return mParent->GetBlock(AABB);	// Parent has a good block
    }
    else return this;	// We are at the root, so we have little choice
}

Block* Block::GetBlockChild(const dReal* AABB){
    if (mChildren){
        for (int i = 0; i < SPLITS; i++){
            Block &CurrentChild = mChildren[i];
            if (CurrentChild.Inside(AABB)){
                return CurrentChild.GetBlockChild(AABB);	// Child will have good block
            }
        }
    }
    return this;	// This is the best block
}

//****************************************************************************
// quadtree space

struct dxQuadTreeSpace : public dxSpace{
    Block* Blocks;	// Blocks[0] is the root

    dArray<dxGeom*> DirtyList;

    dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth);
    ~dxQuadTreeSpace();

    dxGeom* getGeom(int i);

    void add(dxGeom* g);
    void remove(dxGeom* g);
    void dirty(dxGeom* g);

    void computeAABB();

    void cleanGeoms();
    void collide(void* UserData, dNearCallback* Callback);
    void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);

    // Temp data
    Block* CurrentBlock;	// Only used while enumerating
    int* CurrentChild;	// Only used while enumerating
    int CurrentLevel;	// Only used while enumerating
    dxGeom* CurrentObject;	// Only used while enumerating
    int CurrentIndex;
};

namespace {

    inline
    sizeint numNodes(int depth) 
    {
        // A 4-ary tree has (4^(depth+1) - 1)/3 nodes
        // Note: split up into multiple constant expressions for readability
        const int k = depth+1;
        const sizeint fourToNthPlusOne = (sizeint)1 << (2*k); // 4^k = 2^(2k)
        return (fourToNthPlusOne - 1) / 3;
    }

}



dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
    type = dQuadTreeSpaceClass;

    sizeint BlockCount = numNodes(Depth);

    Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
    Block* Blocks = this->Blocks + 1;	// This pointer gets modified!

    dReal MinX = Center[AXIS0] - Extents[AXIS0];
    dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
    dReal MinZ = Center[AXIS1] - Extents[AXIS1];
    dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
    this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);

    CurrentBlock = 0;
    CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
    CurrentLevel = 0;
    CurrentObject = 0;
    CurrentIndex = -1;

    // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
    aabb[0] = -dInfinity;
    aabb[1] = dInfinity;
    aabb[2] = -dInfinity;
    aabb[3] = dInfinity;
    aabb[4] = -dInfinity;
    aabb[5] = dInfinity;
}

dxQuadTreeSpace::~dxQuadTreeSpace(){
    int Depth = 0;
    Block* Current = &Blocks[0];
    while (Current){
        Depth++;
        Current = Current->mChildren;
    }

    sizeint BlockCount = numNodes(Depth);

    dFree(Blocks, BlockCount * sizeof(Block));
    dFree(CurrentChild, (Depth + 1) * sizeof(int));
}

dxGeom* dxQuadTreeSpace::getGeom(int Index){
    dUASSERT(Index >= 0 && Index < count, "index out of range");

    //@@@
    dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");

    return 0;

    // This doesnt work
/*
    if (CurrentIndex == Index){
        // Loop through all objects in the local list
CHILDRECURSE:
        if (CurrentObject){
            dGeomID g = CurrentObject;
            CurrentObject = CurrentObject->next_ex;
            CurrentIndex++;

#ifdef DRAWBLOCKS
            DrawBlock(CurrentBlock);
#endif	//DRAWBLOCKS
            return g;
        }
        else{
            // Now lets loop through our children. Starting at index 0.
            if (CurrentBlock->Children){
                CurrentChild[CurrentLevel] = 0;
PARENTRECURSE:
                for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
                    if (CurrentBlock->Children[i].GeomCount == 0){
                        continue;
                    }
                    CurrentBlock = &CurrentBlock->Children[i];
                    CurrentObject = CurrentBlock->First;

                    i++;

                    CurrentLevel++;
                    goto CHILDRECURSE;
                }
            }
        }

        // Now lets go back to the parent so it can continue processing its other children.
        if (CurrentBlock->Parent){
            CurrentBlock = CurrentBlock->Parent;
            CurrentLevel--;
            goto PARENTRECURSE;
        }
    }
    else{
        CurrentBlock = &Blocks[0];
        CurrentLevel = 0;
        CurrentObject = CurrentObject;
        CurrentIndex = 0;

        // Other states are already set
        CurrentObject = CurrentBlock->First;
    }


    if (current_geom && current_index == Index - 1){
        //current_geom = current_geom->next_ex; // next
        current_index = Index;
        return current_geom;
    }
    else for (int i = 0; i < Index; i++){	// this will be verrrrrrry slow
        getGeom(i);
    }
*/

    return 0;
}

void dxQuadTreeSpace::add(dxGeom* g){
    CHECK_NOT_LOCKED (this);
    dAASSERT(g);
    dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");

    DirtyList.push(g);
    Blocks[0].GetBlock(g->aabb)->AddObject(g);	// Add to best block

    dxSpace::add(g);
}

void dxQuadTreeSpace::remove(dxGeom* g){
    CHECK_NOT_LOCKED(this);
    dAASSERT(g);
    dUASSERT(g->parent_space == this,"object is not in this space");

    // remove
    ((Block*)g->tome_ex)->DelObject(g);

    for (int i = 0; i < DirtyList.size(); i++){
        if (DirtyList[i] == g){
            DirtyList.remove(i);
            // (mg) there can be multiple instances of a dirty object on stack  be sure to remove ALL and not just first, for this we decrement i
            --i;
        }
    }

    dxSpace::remove(g);
}

void dxQuadTreeSpace::dirty(dxGeom* g){
    DirtyList.push(g);
}

void dxQuadTreeSpace::computeAABB(){
    //
}

void dxQuadTreeSpace::cleanGeoms(){
    // compute the AABBs of all dirty geoms, and clear the dirty flags
    lock_count++;

    for (int i = 0; i < DirtyList.size(); i++){
        dxGeom* g = DirtyList[i];
        if (IS_SPACE(g)){
            ((dxSpace*)g)->cleanGeoms();
        }
        
        g->recomputeAABB();
        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);

        g->gflags &= ~GEOM_DIRTY;

        ((Block*)g->tome_ex)->Traverse(g);
    }
    DirtyList.setSize(0);

    lock_count--;
}

void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
    dAASSERT(Callback);

    lock_count++;
    cleanGeoms();

    Blocks[0].Collide(UserData, Callback);

    lock_count--;
}


struct DataCallback {
    void *data;
    dNearCallback *callback;
};
// Invokes the callback with arguments swapped
static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
{
    DataCallback *dc = (DataCallback*)data;
    dc->callback(dc->data, g2, g1);
}


void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){
    dAASSERT(g2 && Callback);

    lock_count++;
    cleanGeoms();
    g2->recomputeAABB();

    if (g2->parent_space == this){
        // The block the geom is in
        Block* CurrentBlock = (Block*)g2->tome_ex;

        // Collide against block and its children
        DataCallback dc = {UserData, Callback};
        CurrentBlock->Collide(g2, CurrentBlock->mFirst, &dc, swap_callback);

        // Collide against parents
        while ((CurrentBlock = CurrentBlock->mParent))
            CurrentBlock->CollideLocal(g2, UserData, Callback);

    }
    else {
        DataCallback dc = {UserData, Callback};
        Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback);
    }

    lock_count--;
}

dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){
    return new dxQuadTreeSpace(space, Center, Extents, Depth);
}