ode/src/collision_quadtreespace.cpp

   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 // With math corrections by Oleh Derevenko. ;)
  25
  26 #include <ode/common.h>
  27 #include <ode/collision_space.h>
  28 #include <ode/collision.h>
  29 #include "config.h"
  30 #include "matrix.h"
  31 #include "collision_kernel.h"
  32
  33 #include "collision_space_internal.h"
  34
  35
  36 #define AXIS0 0
  37 #define AXIS1 1
  38 #define UP 2
  39
  40 //#define DRAWBLOCKS
  41
  42 const int SPLITAXIS = 2;
  43 const int SPLITS = SPLITAXIS * SPLITAXIS;
  44
  45 #define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
  46
  47 class Block{
  48 public:
  49     dReal mMinX, mMaxX;
  50     dReal mMinZ, mMaxZ;
  51
  52     dGeomID mFirst;
  53     int mGeomCount;
  54
  55     Block* mParent;
  56     Block* mChildren;
  57
  58     void Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks);
  59
  60     void Collide(void* UserData, dNearCallback* Callback);
  61     void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback);
  62
  63     void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback);
  64
  65     void AddObject(dGeomID Object);
  66     void DelObject(dGeomID Object);
  67     void Traverse(dGeomID Object);
  68
  69     bool Inside(const dReal* AABB);
  70
  71     Block* GetBlock(const dReal* AABB);
  72     Block* GetBlockChild(const dReal* AABB);
  73 };
  74
  75
  76 #ifdef DRAWBLOCKS
  77 #include "..\..\Include\drawstuff\\drawstuff.h"
  78
  79 static void DrawBlock(Block* Block){
  80     dVector3 v[8];
  81     v[0][AXIS0] = Block->mMinX;
  82     v[0][UP] = REAL(-1.0);
  83     v[0][AXIS1] = Block->mMinZ;
  84
  85     v[1][AXIS0] = Block->mMinX;
  86     v[1][UP] = REAL(-1.0);
  87     v[1][AXIS1] = Block->mMaxZ;
  88
  89     v[2][AXIS0] = Block->mMaxX;
  90     v[2][UP] = REAL(-1.0);
  91     v[2][AXIS1] = Block->mMinZ;
  92
  93     v[3][AXIS0] = Block->mMaxX;
  94     v[3][UP] = REAL(-1.0);
  95     v[3][AXIS1] = Block->mMaxZ;
  96
  97     v[4][AXIS0] = Block->mMinX;
  98     v[4][UP] = REAL(1.0);
  99     v[4][AXIS1] = Block->mMinZ;
 100
 101     v[5][AXIS0] = Block->mMinX;
 102     v[5][UP] = REAL(1.0);
 103     v[5][AXIS1] = Block->mMaxZ;
 104
 105     v[6][AXIS0] = Block->mMaxX;
 106     v[6][UP] = REAL(1.0);
 107     v[6][AXIS1] = Block->mMinZ;
 108
 109     v[7][AXIS0] = Block->mMaxX;
 110     v[7][UP] = REAL(1.0);
 111     v[7][AXIS1] = Block->mMaxZ;
 112
 113     // Bottom
 114     dsDrawLine(v[0], v[1]);
 115     dsDrawLine(v[1], v[3]);
 116     dsDrawLine(v[3], v[2]);
 117     dsDrawLine(v[2], v[0]);
 118
 119     // Top
 120     dsDrawLine(v[4], v[5]);
 121     dsDrawLine(v[5], v[7]);
 122     dsDrawLine(v[7], v[6]);
 123     dsDrawLine(v[6], v[4]);
 124
 125     // Sides
 126     dsDrawLine(v[0], v[4]);
 127     dsDrawLine(v[1], v[5]);
 128     dsDrawLine(v[2], v[6]);
 129     dsDrawLine(v[3], v[7]);
 130 }
 131 #endif  //DRAWBLOCKS
 132
 133
 134 void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){
 135     dIASSERT(MinX <= MaxX);
 136     dIASSERT(MinZ <= MaxZ);
 137
 138     mGeomCount = 0;
 139     mFirst = 0;
 140
 141     mMinX = MinX;
 142     mMaxX = MaxX;
 143
 144     mMinZ = MinZ;
 145     mMaxZ = MaxZ;
 146
 147     this->mParent = Parent;
 148
 149     if (Depth > 0){
 150         mChildren = Blocks;
 151         Blocks += SPLITS;
 152
 153         const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS;
 154         const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS;
 155
 156         const int ChildDepth = Depth - 1;
 157         int Index = 0;
 158
 159         dReal ChildRightX = MinX;
 160         for (int i = 0; i < SPLITAXIS; i++){
 161             const dReal ChildLeftX = ChildRightX;
 162             ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;
 163
 164             dReal ChildRightZ = MinZ;
 165             for (int j = 0; j < SPLITAXIS; j++){
 166                 const dReal ChildLeftZ = ChildRightZ;
 167                 ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;
 168
 169                 mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks);
 170                 ++Index;
 171             }
 172         }
 173     }
 174     else mChildren = 0;
 175 }
 176
 177 void Block::Collide(void* UserData, dNearCallback* Callback){
 178 #ifdef DRAWBLOCKS
 179     DrawBlock(this);
 180 #endif
 181     // Collide the local list
 182     dxGeom* g = mFirst;
 183     while (g){
 184         if (GEOM_ENABLED(g)){
 185             Collide(g, g->next_ex, UserData, Callback);
 186         }
 187         g = g->next_ex;
 188     }
 189
 190     // Recurse for children
 191     if (mChildren){
 192         for (int i = 0; i < SPLITS; i++){
 193             Block &CurrentChild = mChildren[i];
 194             if (CurrentChild.mGeomCount <= 1){  // Early out
 195                 continue;
 196             }
 197             CurrentChild.Collide(UserData, Callback);
 198         }
 199     }
 200 }
 201
 202 // Note: g2 is assumed to be in this Block
 203 void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
 204 #ifdef DRAWBLOCKS
 205     DrawBlock(this);
 206 #endif
 207     // Collide against local list
 208     while (g2){
 209         if (GEOM_ENABLED(g2)){
 210             collideAABBs (g1, g2, UserData, Callback);
 211         }
 212         g2 = g2->next_ex;
 213     }
 214
 215     // Collide against children
 216     if (mChildren){
 217         for (int i = 0; i < SPLITS; i++){
 218             Block &CurrentChild = mChildren[i];
 219             // Early out for empty blocks
 220             if (CurrentChild.mGeomCount == 0){
 221                 continue;
 222             }
 223
 224             // Does the geom's AABB collide with the block?
 225             // Don't do AABB tests for single geom blocks.
 226             if (CurrentChild.mGeomCount == 1){
 227                 //
 228             }
 229             else if (true){
 230                 if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
 231                     g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
 232                     g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
 233                     g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue;
 234             }
 235             CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
 236         }
 237     }
 238 }
 239
 240 void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
 241     // Collide against local list
 242     dxGeom* g1 = mFirst;
 243     while (g1){
 244         if (GEOM_ENABLED(g1)){
 245             collideAABBs (g1, g2, UserData, Callback);
 246         }
 247         g1 = g1->next_ex;
 248     }
 249 }
 250
 251 void Block::AddObject(dGeomID Object){
 252     // Add the geom
 253     Object->next_ex = mFirst;
 254     mFirst = Object;
 255     Object->tome_ex = (dxGeom**)this;
 256
 257     // Now traverse upwards to tell that we have a geom
 258     Block* Block = this;
 259     do{
 260         Block->mGeomCount++;
 261         Block = Block->mParent;
 262     }
 263     while (Block);
 264 }
 265
 266 void Block::DelObject(dGeomID Object){
 267     // Del the geom
 268     dxGeom* g = mFirst;
 269     dxGeom* Last = 0;
 270     while (g){
 271         if (g == Object){
 272             if (Last){
 273                 Last->next_ex = g->next_ex;
 274             }
 275             else mFirst = g->next_ex;
 276
 277             break;
 278         }
 279         Last = g;
 280         g = g->next_ex;
 281     }
 282
 283     Object->tome_ex = 0;
 284
 285     // Now traverse upwards to tell that we have lost a geom
 286     Block* Block = this;
 287     do{
 288         Block->mGeomCount--;
 289         Block = Block->mParent;
 290     }
 291     while (Block);
 292 }
 293
 294 void Block::Traverse(dGeomID Object){
 295     Block* NewBlock = GetBlock(Object->aabb);
 296
 297     if (NewBlock != this){
 298         // Remove the geom from the old block and add it to the new block.
 299         // This could be more optimal, but the loss should be very small.
 300         DelObject(Object);
 301         NewBlock->AddObject(Object);
 302     }
 303 }
 304
 305 bool Block::Inside(const dReal* AABB){
 306     return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ;
 307 }
 308
 309 Block* Block::GetBlock(const dReal* AABB){
 310     if (Inside(AABB)){
 311         return GetBlockChild(AABB);     // Child or this will have a good block
 312     }
 313     else if (mParent){
 314         return mParent->GetBlock(AABB); // Parent has a good block
 315     }
 316     else return this;   // We are at the root, so we have little choice
 317 }
 318
 319 Block* Block::GetBlockChild(const dReal* AABB){
 320     if (mChildren){
 321         for (int i = 0; i < SPLITS; i++){
 322             Block &CurrentChild = mChildren[i];
 323             if (CurrentChild.Inside(AABB)){
 324                 return CurrentChild.GetBlockChild(AABB);        // Child will have good block
 325             }
 326         }
 327     }
 328     return this;        // This is the best block
 329 }
 330
 331 //****************************************************************************
 332 // quadtree space
 333
 334 struct dxQuadTreeSpace : public dxSpace{
 335     Block* Blocks;      // Blocks[0] is the root
 336
 337     dArray<dxGeom*> DirtyList;
 338
 339     dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth);
 340     ~dxQuadTreeSpace();
 341
 342     dxGeom* getGeom(int i);
 343
 344     void add(dxGeom* g);
 345     void remove(dxGeom* g);
 346     void dirty(dxGeom* g);
 347
 348     void computeAABB();
 349
 350     void cleanGeoms();
 351     void collide(void* UserData, dNearCallback* Callback);
 352     void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
 353
 354     // Temp data
 355     Block* CurrentBlock;        // Only used while enumerating
 356     int* CurrentChild;  // Only used while enumerating
 357     int CurrentLevel;   // Only used while enumerating
 358     dxGeom* CurrentObject;      // Only used while enumerating
 359     int CurrentIndex;
 360 };
 361
 362 namespace {
 363
 364     inline
 365     sizeint numNodes(int depth)
 366     {
 367         // A 4-ary tree has (4^(depth+1) - 1)/3 nodes
 368         // Note: split up into multiple constant expressions for readability
 369         const int k = depth+1;
 370         const sizeint fourToNthPlusOne = (sizeint)1 << (2*k); // 4^k = 2^(2k)
 371         return (fourToNthPlusOne - 1) / 3;
 372     }
 373
 374 }
 375
 376
 377
 378 dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
 379     type = dQuadTreeSpaceClass;
 380
 381     sizeint BlockCount = numNodes(Depth);
 382
 383     Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
 384     Block* Blocks = this->Blocks + 1;   // This pointer gets modified!
 385
 386     dReal MinX = Center[AXIS0] - Extents[AXIS0];
 387     dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
 388     dReal MinZ = Center[AXIS1] - Extents[AXIS1];
 389     dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
 390     this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);
 391
 392     CurrentBlock = 0;
 393     CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
 394     CurrentLevel = 0;
 395     CurrentObject = 0;
 396     CurrentIndex = -1;
 397
 398     // 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
 399     aabb[0] = -dInfinity;
 400     aabb[1] = dInfinity;
 401     aabb[2] = -dInfinity;
 402     aabb[3] = dInfinity;
 403     aabb[4] = -dInfinity;
 404     aabb[5] = dInfinity;
 405 }
 406
 407 dxQuadTreeSpace::~dxQuadTreeSpace(){
 408     int Depth = 0;
 409     Block* Current = &Blocks[0];
 410     while (Current){
 411         Depth++;
 412         Current = Current->mChildren;
 413     }
 414
 415     sizeint BlockCount = numNodes(Depth);
 416
 417     dFree(Blocks, BlockCount * sizeof(Block));
 418     dFree(CurrentChild, (Depth + 1) * sizeof(int));
 419 }
 420
 421 dxGeom* dxQuadTreeSpace::getGeom(int Index){
 422     dUASSERT(Index >= 0 && Index < count, "index out of range");
 423
 424     //@@@
 425     dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
 426
 427     return 0;
 428
 429     // This doesnt work
 430 /*
 431     if (CurrentIndex == Index){
 432         // Loop through all objects in the local list
 433 CHILDRECURSE:
 434         if (CurrentObject){
 435             dGeomID g = CurrentObject;
 436             CurrentObject = CurrentObject->next_ex;
 437             CurrentIndex++;
 438
 439 #ifdef DRAWBLOCKS
 440             DrawBlock(CurrentBlock);
 441 #endif  //DRAWBLOCKS
 442             return g;
 443         }
 444         else{
 445             // Now lets loop through our children. Starting at index 0.
 446             if (CurrentBlock->Children){
 447                 CurrentChild[CurrentLevel] = 0;
 448 PARENTRECURSE:
 449                 for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
 450                     if (CurrentBlock->Children[i].GeomCount == 0){
 451                         continue;
 452                     }
 453                     CurrentBlock = &CurrentBlock->Children[i];
 454                     CurrentObject = CurrentBlock->First;
 455
 456                     i++;
 457
 458                     CurrentLevel++;
 459                     goto CHILDRECURSE;
 460                 }
 461             }
 462         }
 463
 464         // Now lets go back to the parent so it can continue processing its other children.
 465         if (CurrentBlock->Parent){
 466             CurrentBlock = CurrentBlock->Parent;
 467             CurrentLevel--;
 468             goto PARENTRECURSE;
 469         }
 470     }
 471     else{
 472         CurrentBlock = &Blocks[0];
 473         CurrentLevel = 0;
 474         CurrentObject = CurrentObject;
 475         CurrentIndex = 0;
 476
 477         // Other states are already set
 478         CurrentObject = CurrentBlock->First;
 479     }
 480
 481
 482     if (current_geom && current_index == Index - 1){
 483         //current_geom = current_geom->next_ex; // next
 484         current_index = Index;
 485         return current_geom;
 486     }
 487     else for (int i = 0; i < Index; i++){       // this will be verrrrrrry slow
 488         getGeom(i);
 489     }
 490 */
 491
 492     return 0;
 493 }
 494
 495 void dxQuadTreeSpace::add(dxGeom* g){
 496     CHECK_NOT_LOCKED (this);
 497     dAASSERT(g);
 498     dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
 499
 500     DirtyList.push(g);
 501     Blocks[0].GetBlock(g->aabb)->AddObject(g);  // Add to best block
 502
 503     dxSpace::add(g);
 504 }
 505
 506 void dxQuadTreeSpace::remove(dxGeom* g){
 507     CHECK_NOT_LOCKED(this);
 508     dAASSERT(g);
 509     dUASSERT(g->parent_space == this,"object is not in this space");
 510
 511     // remove
 512     ((Block*)g->tome_ex)->DelObject(g);
 513
 514     for (int i = 0; i < DirtyList.size(); i++){
 515         if (DirtyList[i] == g){
 516             DirtyList.remove(i);
 517             // (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
 518             --i;
 519         }
 520     }
 521
 522     dxSpace::remove(g);
 523 }
 524
 525 void dxQuadTreeSpace::dirty(dxGeom* g){
 526     DirtyList.push(g);
 527 }
 528
 529 void dxQuadTreeSpace::computeAABB(){
 530     //
 531 }
 532
 533 void dxQuadTreeSpace::cleanGeoms(){
 534     // compute the AABBs of all dirty geoms, and clear the dirty flags
 535     lock_count++;
 536
 537     for (int i = 0; i < DirtyList.size(); i++){
 538         dxGeom* g = DirtyList[i];
 539         if (IS_SPACE(g)){
 540             ((dxSpace*)g)->cleanGeoms();
 541         }
 542
 543         g->recomputeAABB();
 544         dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
 545
 546         g->gflags &= ~GEOM_DIRTY;
 547
 548         ((Block*)g->tome_ex)->Traverse(g);
 549     }
 550     DirtyList.setSize(0);
 551
 552     lock_count--;
 553 }
 554
 555 void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
 556     dAASSERT(Callback);
 557
 558     lock_count++;
 559     cleanGeoms();
 560
 561     Blocks[0].Collide(UserData, Callback);
 562
 563     lock_count--;
 564 }
 565
 566
 567 struct DataCallback {
 568     void *data;
 569     dNearCallback *callback;
 570 };
 571 // Invokes the callback with arguments swapped
 572 static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
 573 {
 574     DataCallback *dc = (DataCallback*)data;
 575     dc->callback(dc->data, g2, g1);
 576 }
 577
 578
 579 void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){
 580     dAASSERT(g2 && Callback);
 581
 582     lock_count++;
 583     cleanGeoms();
 584     g2->recomputeAABB();
 585
 586     if (g2->parent_space == this){
 587         // The block the geom is in
 588         Block* CurrentBlock = (Block*)g2->tome_ex;
 589
 590         // Collide against block and its children
 591         DataCallback dc = {UserData, Callback};
 592         CurrentBlock->Collide(g2, CurrentBlock->mFirst, &dc, swap_callback);
 593
 594         // Collide against parents
 595         while ((CurrentBlock = CurrentBlock->mParent))
 596             CurrentBlock->CollideLocal(g2, UserData, Callback);
 597
 598     }
 599     else {
 600         DataCallback dc = {UserData, Callback};
 601         Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback);
 602     }
 603
 604     lock_count--;
 605 }
 606
 607 dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){
 608     return new dxQuadTreeSpace(space, Center, Extents, Depth);
 609 }