diff -r 000000000000 -r 2f259fa3e83a ode/src/collision_quadtreespace.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ode/src/collision_quadtreespace.cpp Tue Feb 02 01:00:49 2010 +0200 @@ -0,0 +1,448 @@ +/************************************************************************* + * * + * 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. + +#include +#include +#include +#include +#include "collision_kernel.h" + +#include "collision_space_internal.h" + + +#define AXIS0 0 +#define AXIS1 1 +#define UP 2 + +const int SPLITAXIS = 2; +const int SPLITS = SPLITAXIS * SPLITAXIS; + +#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED + +class Block{ +public: + dReal MinX, MaxX; + dReal MinZ, MaxZ; + + dGeomID First; + int GeomCount; + + Block* Parent; + Block* Children; + + void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks); + + void Collide(void* UserData, dNearCallback* Callback); + void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback); + + void CollideLocal(dGeomID Object, 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); +}; + + +void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){ + GeomCount = 0; + First = 0; + + MinX = Center[AXIS0] - Extents[AXIS0]; + MaxX = Center[AXIS0] + Extents[AXIS0]; + + MinZ = Center[AXIS1] - Extents[AXIS1]; + MaxZ = Center[AXIS1] + Extents[AXIS1]; + + this->Parent = Parent; + if (Depth > 0){ + Children = Blocks; + Blocks += SPLITS; + + dVector3 ChildExtents; + ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS; + ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS; + ChildExtents[UP] = Extents[UP]; + + for (int i = 0; i < SPLITAXIS; i++){ + for (int j = 0; j < SPLITAXIS; j++){ + int Index = i * SPLITAXIS + j; + + dVector3 ChildCenter; + ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2); + ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2); + ChildCenter[UP] = Center[UP]; + + Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks); + } + } + } + else Children = 0; +} + +void Block::Collide(void* UserData, dNearCallback* Callback){ + + // Collide the local list + dxGeom* g = First; + while (g){ + if (GEOM_ENABLED(g)){ + Collide(g, g->next, UserData, Callback); + } + g = g->next; + } + + // Recurse for children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].GeomCount <= 1){ // Early out + continue; + } + Children[i].Collide(UserData, Callback); + } + } +} + +void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){ + + // Collide against local list + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } + + // Collide against children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + // Early out for empty blocks + if (Children[i].GeomCount == 0){ + continue; + } + + // Does the geom's AABB collide with the block? + // Dont do AABB tests for single geom blocks. + if (Children[i].GeomCount == 1 && Children[i].First){ + // + } + else if (true){ + if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX || + g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX || + g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ || + g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue; + } + Children[i].Collide(g1, Children[i].First, UserData, Callback); + } + } +} + +void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){ + // Collide against local list + dxGeom* g2 = First; + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } +} + +void Block::AddObject(dGeomID Object){ + // Add the geom + Object->next = First; + First = Object; + Object->tome = (dxGeom**)this; + + // Now traverse upwards to tell that we have a geom + Block* Block = this; + do{ + Block->GeomCount++; + Block = Block->Parent; + } + while (Block); +} + +void Block::DelObject(dGeomID Object){ + // Del the geom + dxGeom* g = First; + dxGeom* Last = 0; + while (g){ + if (g == Object){ + if (Last){ + Last->next = g->next; + } + else First = g->next; + + break; + } + Last = g; + g = g->next; + } + + Object->tome = 0; + + // Now traverse upwards to tell that we have lost a geom + Block* Block = this; + do{ + Block->GeomCount--; + Block = Block->Parent; + } + 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] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ; +} + +Block* Block::GetBlock(const dReal* AABB){ + if (Inside(AABB)){ + return GetBlockChild(AABB); // Child or this will have a good block + } + else if (Parent){ + return Parent->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 (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].Inside(AABB)){ + return Children[i].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 DirtyList; + + dxQuadTreeSpace(dSpaceID _space, dVector3 Center, 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; +}; + +dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){ + type = dQuadTreeSpaceClass; + + int BlockCount = 0; + for (int i = 0; i <= Depth; i++){ + BlockCount += (int)pow(SPLITS, i); + } + + Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); + Block* Blocks = this->Blocks + 1; // This pointer gets modified! + + this->Blocks[0].Create(Center, Extents, 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->Children; + } + + int BlockCount = 0; + for (int i = 0; i < Depth; i++){ + BlockCount += (int)pow(SPLITS, i); + } + + dFree(Blocks, BlockCount * sizeof(Block)); + dFree(CurrentChild, (Depth + 1) * sizeof(int)); +} + +dxGeom* dxQuadTreeSpace::getGeom(int /*Index*/){ + + //@@@ + + return 0; +} + +void dxQuadTreeSpace::add(dxGeom* g){ + + + g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + DirtyList.push(g); + + // add + g->parent_space = this; + Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block + count++; + + // enumerator has been invalidated + current_geom = 0; + + dGeomMoved(this); +} + +void dxQuadTreeSpace::remove(dxGeom* g){ + + // remove + ((Block*)g->tome)->DelObject(g); + count--; + + 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; + } + } + + // safeguard + g->next = 0; + g->tome = 0; + g->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved(this); +} + +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(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + + ((Block*)g->tome)->Traverse(g); + } + DirtyList.setSize(0); + + lock_count--; +} + +void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){ + + + lock_count++; + cleanGeoms(); + + Blocks[0].Collide(UserData, Callback); + + lock_count--; +} + +void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){ + + lock_count++; + cleanGeoms(); + g1->recomputeAABB(); + + if (g1->parent_space == this){ + // The block the geom is in + Block* CurrentBlock = (Block*)g1->tome; + + // Collide against block and its children + CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback); + + // Collide against parents + while (true){ + CurrentBlock = CurrentBlock->Parent; + if (!CurrentBlock){ + break; + } + CurrentBlock->CollideLocal(g1, UserData, Callback); + } + } + else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback); + + lock_count--; +} + +EXPORT_C dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){ + return new dxQuadTreeSpace(space, Center, Extents, Depth); +}