1 /*************************************************************************

     2  *                                                                       *

     3  * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *

     4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

     5  *                                                                       *

     6  * This library is free software; you can redistribute it and/or         *

     7  * modify it under the terms of EITHER:                                  *

     8  *   (1) The GNU Lesser General Public License as published by the Free  *

     9  *       Software Foundation; either version 2.1 of the License, or (at  *

    10  *       your option) any later version. The text of the GNU Lesser      *

    11  *       General Public License is included with this library in the     *

    12  *       file LICENSE.TXT.                                               *

    13  *   (2) The BSD-style license that is included with this library in     *

    14  *       the file LICENSE-BSD.TXT.                                       *

    15  *                                                                       *

    16  * This library is distributed in the hope that it will be useful,       *

    17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

    18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

    19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

    20  *                                                                       *

    21  *************************************************************************/

    22 

    23 // QuadTreeSpace by Erwin de Vries.

    24 

    25 #include <ode/common.h>

    26 #include <ode/matrix.h>

    27 #include <ode/collision_space.h>

    28 #include <ode/collision.h>

    29 #include "collision_kernel.h"

    30 

    31 #include "collision_space_internal.h"

    32 

    33 

    34 #define AXIS0 0

    35 #define AXIS1 1

    36 #define UP 2

    37 

    38 const int SPLITAXIS = 2;

    39 const int SPLITS = SPLITAXIS * SPLITAXIS;

    40 

    41 #define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED

    42 

    43 class Block{

    44 public:

    45 	dReal MinX, MaxX;

    46 	dReal MinZ, MaxZ;

    47 

    48 	dGeomID First;

    49 	int GeomCount;

    50 

    51 	Block* Parent;

    52 	Block* Children;

    53 

    54 	void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);

    55 

    56 	void Collide(void* UserData, dNearCallback* Callback);

    57 	void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);

    58 

    59 	void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);

    60 	

    61 	void AddObject(dGeomID Object);

    62 	void DelObject(dGeomID Object);

    63 	void Traverse(dGeomID Object);

    64 

    65 	bool Inside(const dReal* AABB);

    66 	

    67 	Block* GetBlock(const dReal* AABB);

    68 	Block* GetBlockChild(const dReal* AABB);

    69 };

    70 

    71 

    72 void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){

    73 	GeomCount = 0;

    74 	First = 0;

    75 

    76 	MinX = Center[AXIS0] - Extents[AXIS0];

    77 	MaxX = Center[AXIS0] + Extents[AXIS0];

    78 

    79 	MinZ = Center[AXIS1] - Extents[AXIS1];

    80 	MaxZ = Center[AXIS1] + Extents[AXIS1];

    81 

    82 	this->Parent = Parent;

    83 	if (Depth > 0){

    84 		Children = Blocks;

    85 		Blocks += SPLITS;

    86 

    87 		dVector3 ChildExtents;

    88 		ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;

    89 		ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;

    90 		ChildExtents[UP] = Extents[UP];

    91 

    92 		for (int i = 0; i < SPLITAXIS; i++){

    93 			for (int j = 0; j < SPLITAXIS; j++){

    94 				int Index = i * SPLITAXIS + j;

    95 

    96 				dVector3 ChildCenter;

    97 				ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);

    98 				ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);

    99 				ChildCenter[UP] = Center[UP];

   100 				

   101 				Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);

   102 			}

   103 		}

   104 	}

   105 	else Children = 0;

   106 }

   107 

   108 void Block::Collide(void* UserData, dNearCallback* Callback){

   109 

   110 	// Collide the local list

   111 	dxGeom* g = First;

   112 	while (g){

   113 		if (GEOM_ENABLED(g)){

   114 			Collide(g, g->next, UserData, Callback);

   115 		}

   116 		g = g->next;

   117 	}

   118 

   119 	// Recurse for children

   120 	if (Children){

   121 		for (int i = 0; i < SPLITS; i++){

   122 			if (Children[i].GeomCount <= 1){	// Early out

   123 				continue;

   124 			}

   125 			Children[i].Collide(UserData, Callback);

   126 		}

   127 	}

   128 }

   129 

   130 void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){

   131 

   132 	// Collide against local list

   133 	while (g2){

   134 		if (GEOM_ENABLED(g2)){

   135 			collideAABBs (g1, g2, UserData, Callback);

   136 		}

   137 		g2 = g2->next;

   138 	}

   139 

   140 	// Collide against children

   141 	if (Children){

   142 		for (int i = 0; i < SPLITS; i++){

   143 			// Early out for empty blocks

   144 			if (Children[i].GeomCount == 0){

   145 				continue;

   146 			}

   147 

   148 			// Does the geom's AABB collide with the block?

   149 			// Dont do AABB tests for single geom blocks.

   150 			if (Children[i].GeomCount == 1 && Children[i].First){

   151 				//

   152 			}

   153 			else if (true){

   154 				if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||

   155 					g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||

   156 					g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||

   157 					g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;

   158 			}

   159 			Children[i].Collide(g1, Children[i].First, UserData, Callback);

   160 		}

   161 	}

   162 }

   163 

   164 void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){

   165 	// Collide against local list

   166 	dxGeom* g2 = First;

   167 	while (g2){

   168 		if (GEOM_ENABLED(g2)){

   169 			collideAABBs (g1, g2, UserData, Callback);

   170 		}

   171 		g2 = g2->next;

   172 	}

   173 }

   174 

   175 void Block::AddObject(dGeomID Object){

   176 	// Add the geom

   177 	Object->next = First;

   178 	First = Object;

   179 	Object->tome = (dxGeom**)this;

   180 

   181 	// Now traverse upwards to tell that we have a geom

   182 	Block* Block = this;

   183 	do{

   184 		Block->GeomCount++;

   185 		Block = Block->Parent;

   186 	}

   187 	while (Block);

   188 }

   189 

   190 void Block::DelObject(dGeomID Object){

   191 	// Del the geom

   192 	dxGeom* g = First;

   193 	dxGeom* Last = 0;

   194 	while (g){

   195 		if (g == Object){

   196 			if (Last){

   197 				Last->next = g->next;

   198 			}

   199 			else First = g->next;

   200 

   201 			break;

   202 		}

   203 		Last = g;

   204 		g = g->next;

   205 	}

   206 

   207 	Object->tome = 0;

   208 

   209 	// Now traverse upwards to tell that we have lost a geom

   210 	Block* Block = this;

   211 	do{

   212 		Block->GeomCount--;

   213 		Block = Block->Parent;

   214 	}

   215 	while (Block);

   216 }

   217 

   218 void Block::Traverse(dGeomID Object){

   219 	Block* NewBlock = GetBlock(Object->aabb);

   220 

   221 	if (NewBlock != this){

   222 		// Remove the geom from the old block and add it to the new block.

   223 		// This could be more optimal, but the loss should be very small.

   224 		DelObject(Object);

   225 		NewBlock->AddObject(Object);

   226 	}

   227 }

   228 

   229 bool Block::Inside(const dReal* AABB){

   230 	return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;

   231 }

   232 

   233 Block* Block::GetBlock(const dReal* AABB){

   234 	if (Inside(AABB)){

   235 		return GetBlockChild(AABB);	// Child or this will have a good block

   236 	}

   237 	else if (Parent){

   238 		return Parent->GetBlock(AABB);	// Parent has a good block

   239 	}

   240 	else return this;	// We are at the root, so we have little choice

   241 }

   242 

   243 Block* Block::GetBlockChild(const dReal* AABB){

   244 	if (Children){

   245 		for (int i = 0; i < SPLITS; i++){

   246 			if (Children[i].Inside(AABB)){

   247 				return Children[i].GetBlockChild(AABB);	// Child will have good block

   248 			}

   249 		}

   250 	}

   251 	return this;	// This is the best block

   252 }

   253 

   254 //****************************************************************************

   255 // quadtree space

   256 

   257 struct dxQuadTreeSpace : public dxSpace{

   258 	Block* Blocks;	// Blocks[0] is the root

   259 

   260 	dArray<dxGeom*> DirtyList;

   261 

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

   263 	~dxQuadTreeSpace();

   264 

   265 	dxGeom* getGeom(int i);

   266 	

   267 	void add(dxGeom* g);

   268 	void remove(dxGeom* g);

   269 	void dirty(dxGeom* g);

   270 

   271 	void computeAABB();

   272 	

   273 	void cleanGeoms();

   274 	void collide(void* UserData, dNearCallback* Callback);

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

   276 

   277 	// Temp data

   278 	Block* CurrentBlock;	// Only used while enumerating

   279 	int* CurrentChild;	// Only used while enumerating

   280 	int CurrentLevel;	// Only used while enumerating

   281 	dxGeom* CurrentObject;	// Only used while enumerating

   282 	int CurrentIndex;

   283 };

   284 

   285 dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){

   286 	type = dQuadTreeSpaceClass;

   287 

   288 	int BlockCount = 0;

   289 	for (int i = 0; i <= Depth; i++){

   290 		BlockCount += (int)pow(SPLITS, i);

   291 	}

   292 

   293 	Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));

   294 	Block* Blocks = this->Blocks + 1;	// This pointer gets modified!

   295 

   296 	this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);

   297 

   298 	CurrentBlock = 0;

   299 	CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));

   300 	CurrentLevel = 0;

   301 	CurrentObject = 0;

   302 	CurrentIndex = -1;

   303 

   304 	// 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

   305 	aabb[0] = -dInfinity;

   306 	aabb[1] = dInfinity;

   307 	aabb[2] = -dInfinity;

   308 	aabb[3] = dInfinity;

   309 	aabb[4] = -dInfinity;

   310 	aabb[5] = dInfinity;

   311 }

   312 

   313 dxQuadTreeSpace::~dxQuadTreeSpace(){

   314 	int Depth = 0;

   315 	Block* Current = &Blocks[0];

   316 	while (Current){

   317 		Depth++;

   318 		Current = Current->Children;

   319 	}

   320 

   321 	int BlockCount = 0;

   322 	for (int i = 0; i < Depth; i++){

   323 		BlockCount += (int)pow(SPLITS, i);

   324 	}

   325 

   326 	dFree(Blocks, BlockCount * sizeof(Block));

   327 	dFree(CurrentChild, (Depth + 1) * sizeof(int));

   328 }

   329 

   330 dxGeom* dxQuadTreeSpace::getGeom(int /*Index*/){

   331 

   332 	//@@@

   333 

   334 	return 0;

   335 }

   336 

   337 void dxQuadTreeSpace::add(dxGeom* g){

   338 

   339 

   340 	g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;

   341 	DirtyList.push(g);

   342 

   343 	// add

   344 	g->parent_space = this;

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

   346 	count++;

   347 	

   348 	// enumerator has been invalidated

   349 	current_geom = 0;

   350 	

   351 	dGeomMoved(this);

   352 }

   353 

   354 void dxQuadTreeSpace::remove(dxGeom* g){

   355 	

   356 	// remove

   357 	((Block*)g->tome)->DelObject(g);

   358 	count--;

   359 

   360 	for (int i = 0; i < DirtyList.size(); i++){

   361 		if (DirtyList[i] == g){

   362 			DirtyList.remove(i);

   363 			// (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

   364 			--i;

   365 		}

   366 	}

   367 	

   368 	// safeguard

   369 	g->next = 0;

   370 	g->tome = 0;

   371 	g->parent_space = 0;

   372 	

   373 	// enumerator has been invalidated

   374 	current_geom = 0;

   375 	

   376 	// the bounding box of this space (and that of all the parents) may have

   377 	// changed as a consequence of the removal.

   378 	dGeomMoved(this);

   379 }

   380 

   381 void dxQuadTreeSpace::dirty(dxGeom* g){

   382 	DirtyList.push(g);

   383 }

   384 

   385 void dxQuadTreeSpace::computeAABB(){

   386 	//

   387 }

   388 

   389 void dxQuadTreeSpace::cleanGeoms(){

   390 	// compute the AABBs of all dirty geoms, and clear the dirty flags

   391 	lock_count++;

   392 	

   393 	for (int i = 0; i < DirtyList.size(); i++){

   394 		dxGeom* g = DirtyList[i];

   395 		if (IS_SPACE(g)){

   396 			((dxSpace*)g)->cleanGeoms();

   397 		}

   398 		g->recomputeAABB();

   399 		g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));

   400 

   401 		((Block*)g->tome)->Traverse(g);

   402 	}

   403 	DirtyList.setSize(0);

   404 

   405 	lock_count--;

   406 }

   407 

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

   409 

   410 

   411   lock_count++;

   412   cleanGeoms();

   413 

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

   415 

   416   lock_count--;

   417 }

   418 

   419 void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){

   420 

   421   lock_count++;

   422   cleanGeoms();

   423   g1->recomputeAABB();

   424 

   425   if (g1->parent_space == this){

   426 	  // The block the geom is in

   427 	  Block* CurrentBlock = (Block*)g1->tome;

   428 	  

   429 	  // Collide against block and its children

   430 	  CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);

   431 	  

   432 	  // Collide against parents

   433 	  while (true){

   434 		  CurrentBlock = CurrentBlock->Parent;

   435 		  if (!CurrentBlock){

   436 			  break;

   437 		  }

   438 		  CurrentBlock->CollideLocal(g1, UserData, Callback);

   439 	  }

   440   }

   441   else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);

   442 

   443   lock_count--;

   444 }

   445 

   446 EXPORT_C dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){

   447 	return new dxQuadTreeSpace(space, Center, Extents, Depth);

   448 }