From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode-0.9/ode/src/collision_quadtreespace.cpp | 584 --------------------- 1 file changed, 584 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_quadtreespace.cpp (limited to 'libraries/ode-0.9\/ode/src/collision_quadtreespace.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp deleted file mode 100644 index 86a1833..0000000 --- a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp +++ /dev/null @@ -1,584 +0,0 @@ -/************************************************************************* - * * - * 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 <ode/common.h> -#include <ode/matrix.h> -#include <ode/collision_space.h> -#include <ode/collision.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_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); -}; - - -#ifdef DRAWBLOCKS -#include "..\..\Include\drawstuff\\drawstuff.h" - -static void DrawBlock(Block* Block){ - dVector3 v[8]; - v[0][AXIS0] = Block->MinX; - v[0][UP] = REAL(-1.0); - v[0][AXIS1] = Block->MinZ; - - v[1][AXIS0] = Block->MinX; - v[1][UP] = REAL(-1.0); - v[1][AXIS1] = Block->MaxZ; - - v[2][AXIS0] = Block->MaxX; - v[2][UP] = REAL(-1.0); - v[2][AXIS1] = Block->MinZ; - - v[3][AXIS0] = Block->MaxX; - v[3][UP] = REAL(-1.0); - v[3][AXIS1] = Block->MaxZ; - - v[4][AXIS0] = Block->MinX; - v[4][UP] = REAL(1.0); - v[4][AXIS1] = Block->MinZ; - - v[5][AXIS0] = Block->MinX; - v[5][UP] = REAL(1.0); - v[5][AXIS1] = Block->MaxZ; - - v[6][AXIS0] = Block->MaxX; - v[6][UP] = REAL(1.0); - v[6][AXIS1] = Block->MinZ; - - v[7][AXIS0] = Block->MaxX; - v[7][UP] = REAL(1.0); - v[7][AXIS1] = Block->MaxZ; - - // 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 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){ -#ifdef DRAWBLOCKS - DrawBlock(this); -#endif - // 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){ -#ifdef DRAWBLOCKS - DrawBlock(this); -#endif - // 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<dxGeom*> 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((dReal)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((dReal)SPLITS, i); - } - - 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; - 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; // 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->parent_space == 0 && g->next == 0, "geom is already in a space"); - - 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){ - CHECK_NOT_LOCKED(this); - dAASSERT(g); - dUASSERT(g->parent_space == this,"object is not in this space"); - - // 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){ - dAASSERT(Callback); - - lock_count++; - cleanGeoms(); - - Blocks[0].Collide(UserData, Callback); - - lock_count--; -} - -void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){ - dAASSERT(g1 && 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--; -} - -dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){ - return new dxQuadTreeSpace(space, Center, Extents, Depth); -} -- cgit v1.1