dali-physics/third-party/bullet3/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 ///btDbvtBroadphase implementation by Nathanael Presson
  17
  18 #include "btDbvtBroadphase.h"
  19 #include "LinearMath/btThreads.h"
  20 btScalar gDbvtMargin = btScalar(0.05);
  21 //
  22 // Profiling
  23 //
  24
  25 #if DBVT_BP_PROFILE || DBVT_BP_ENABLE_BENCHMARK
  26 #include <stdio.h>
  27 #endif
  28
  29 #if DBVT_BP_PROFILE
  30 struct ProfileScope
  31 {
  32         __forceinline ProfileScope(btClock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
  33         {
  34         }
  35         __forceinline ~ProfileScope()
  36         {
  37                 (*m_value) += m_clock->getTimeMicroseconds() - m_base;
  38         }
  39         btClock* m_clock;
  40         unsigned long* m_value;
  41         unsigned long m_base;
  42 };
  43 #define SPC(_value_) ProfileScope spc_scope(m_clock, _value_)
  44 #else
  45 #define SPC(_value_)
  46 #endif
  47
  48 //
  49 // Helpers
  50 //
  51
  52 //
  53 template <typename T>
  54 static inline void listappend(T* item, T*& list)
  55 {
  56         item->links[0] = 0;
  57         item->links[1] = list;
  58         if (list) list->links[0] = item;
  59         list = item;
  60 }
  61
  62 //
  63 template <typename T>
  64 static inline void listremove(T* item, T*& list)
  65 {
  66         if (item->links[0])
  67                 item->links[0]->links[1] = item->links[1];
  68         else
  69                 list = item->links[1];
  70         if (item->links[1]) item->links[1]->links[0] = item->links[0];
  71 }
  72
  73 //
  74 template <typename T>
  75 static inline int listcount(T* root)
  76 {
  77         int n = 0;
  78         while (root)
  79         {
  80                 ++n;
  81                 root = root->links[1];
  82         }
  83         return (n);
  84 }
  85
  86 //
  87 template <typename T>
  88 static inline void clear(T& value)
  89 {
  90         static const struct ZeroDummy : T
  91         {
  92         } zerodummy;
  93         value = zerodummy;
  94 }
  95
  96 //
  97 // Colliders
  98 //
  99
 100 /* Tree collider        */
 101 struct btDbvtTreeCollider : btDbvt::ICollide
 102 {
 103         btDbvtBroadphase* pbp;
 104         btDbvtProxy* proxy;
 105         btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
 106         void Process(const btDbvtNode* na, const btDbvtNode* nb)
 107         {
 108                 if (na != nb)
 109                 {
 110                         btDbvtProxy* pa = (btDbvtProxy*)na->data;
 111                         btDbvtProxy* pb = (btDbvtProxy*)nb->data;
 112 #if DBVT_BP_SORTPAIRS
 113                         if (pa->m_uniqueId > pb->m_uniqueId)
 114                                 btSwap(pa, pb);
 115 #endif
 116                         pbp->m_paircache->addOverlappingPair(pa, pb);
 117                         ++pbp->m_newpairs;
 118                 }
 119         }
 120         void Process(const btDbvtNode* n)
 121         {
 122                 Process(n, proxy->leaf);
 123         }
 124 };
 125
 126 //
 127 // btDbvtBroadphase
 128 //
 129
 130 //
 131 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 132 {
 133         m_deferedcollide = false;
 134         m_needcleanup = true;
 135         m_releasepaircache = (paircache != 0) ? false : true;
 136         m_prediction = 0;
 137         m_stageCurrent = 0;
 138         m_fixedleft = 0;
 139         m_fupdates = 1;
 140         m_dupdates = 0;
 141         m_cupdates = 10;
 142         m_newpairs = 1;
 143         m_updates_call = 0;
 144         m_updates_done = 0;
 145         m_updates_ratio = 0;
 146         m_paircache = paircache ? paircache : new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache();
 147         m_gid = 0;
 148         m_pid = 0;
 149         m_cid = 0;
 150         for (int i = 0; i <= STAGECOUNT; ++i)
 151         {
 152                 m_stageRoots[i] = 0;
 153         }
 154 #if BT_THREADSAFE
 155         m_rayTestStacks.resize(BT_MAX_THREAD_COUNT);
 156 #else
 157         m_rayTestStacks.resize(1);
 158 #endif
 159 #if DBVT_BP_PROFILE
 160         clear(m_profiling);
 161 #endif
 162 }
 163
 164 //
 165 btDbvtBroadphase::~btDbvtBroadphase()
 166 {
 167         if (m_releasepaircache)
 168         {
 169                 m_paircache->~btOverlappingPairCache();
 170                 btAlignedFree(m_paircache);
 171         }
 172 }
 173
 174 //
 175 btBroadphaseProxy* btDbvtBroadphase::createProxy(const btVector3& aabbMin,
 176                                                                                                  const btVector3& aabbMax,
 177                                                                                                  int /*shapeType*/,
 178                                                                                                  void* userPtr,
 179                                                                                                  int collisionFilterGroup,
 180                                                                                                  int collisionFilterMask,
 181                                                                                                  btDispatcher* /*dispatcher*/)
 182 {
 183         btDbvtProxy* proxy = new (btAlignedAlloc(sizeof(btDbvtProxy), 16)) btDbvtProxy(aabbMin, aabbMax, userPtr,
 184                                                                                                                                                                    collisionFilterGroup,
 185                                                                                                                                                                    collisionFilterMask);
 186
 187         btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
 188
 189         //bproxy->aabb                  =       btDbvtVolume::FromMM(aabbMin,aabbMax);
 190         proxy->stage = m_stageCurrent;
 191         proxy->m_uniqueId = ++m_gid;
 192         proxy->leaf = m_sets[0].insert(aabb, proxy);
 193         listappend(proxy, m_stageRoots[m_stageCurrent]);
 194         if (!m_deferedcollide)
 195         {
 196                 btDbvtTreeCollider collider(this);
 197                 collider.proxy = proxy;
 198                 m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
 199                 m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
 200         }
 201         return (proxy);
 202 }
 203
 204 //
 205 void btDbvtBroadphase::destroyProxy(btBroadphaseProxy* absproxy,
 206                                                                         btDispatcher* dispatcher)
 207 {
 208         btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
 209         if (proxy->stage == STAGECOUNT)
 210                 m_sets[1].remove(proxy->leaf);
 211         else
 212                 m_sets[0].remove(proxy->leaf);
 213         listremove(proxy, m_stageRoots[proxy->stage]);
 214         m_paircache->removeOverlappingPairsContainingProxy(proxy, dispatcher);
 215         btAlignedFree(proxy);
 216         m_needcleanup = true;
 217 }
 218
 219 void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy, btVector3& aabbMin, btVector3& aabbMax) const
 220 {
 221         btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
 222         aabbMin = proxy->m_aabbMin;
 223         aabbMax = proxy->m_aabbMax;
 224 }
 225
 226 struct BroadphaseRayTester : btDbvt::ICollide
 227 {
 228         btBroadphaseRayCallback& m_rayCallback;
 229         BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
 230                 : m_rayCallback(orgCallback)
 231         {
 232         }
 233         void Process(const btDbvtNode* leaf)
 234         {
 235                 btDbvtProxy* proxy = (btDbvtProxy*)leaf->data;
 236                 m_rayCallback.process(proxy);
 237         }
 238 };
 239
 240 void btDbvtBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
 241 {
 242         BroadphaseRayTester callback(rayCallback);
 243         btAlignedObjectArray<const btDbvtNode*>* stack = &m_rayTestStacks[0];
 244 #if BT_THREADSAFE
 245         // for this function to be threadsafe, each thread must have a separate copy
 246         // of this stack.  This could be thread-local static to avoid dynamic allocations,
 247         // instead of just a local.
 248         int threadIndex = btGetCurrentThreadIndex();
 249         btAlignedObjectArray<const btDbvtNode*> localStack;
 250         //todo(erwincoumans, "why do we get tsan issue here?")
 251         if (0)//threadIndex < m_rayTestStacks.size())
 252         //if (threadIndex < m_rayTestStacks.size())
 253         {
 254                 // use per-thread preallocated stack if possible to avoid dynamic allocations
 255                 stack = &m_rayTestStacks[threadIndex];
 256         }
 257         else
 258         {
 259                 stack = &localStack;
 260         }
 261 #endif
 262
 263         m_sets[0].rayTestInternal(m_sets[0].m_root,
 264                                                           rayFrom,
 265                                                           rayTo,
 266                                                           rayCallback.m_rayDirectionInverse,
 267                                                           rayCallback.m_signs,
 268                                                           rayCallback.m_lambda_max,
 269                                                           aabbMin,
 270                                                           aabbMax,
 271                                                           *stack,
 272                                                           callback);
 273
 274         m_sets[1].rayTestInternal(m_sets[1].m_root,
 275                                                           rayFrom,
 276                                                           rayTo,
 277                                                           rayCallback.m_rayDirectionInverse,
 278                                                           rayCallback.m_signs,
 279                                                           rayCallback.m_lambda_max,
 280                                                           aabbMin,
 281                                                           aabbMax,
 282                                                           *stack,
 283                                                           callback);
 284 }
 285
 286 struct BroadphaseAabbTester : btDbvt::ICollide
 287 {
 288         btBroadphaseAabbCallback& m_aabbCallback;
 289         BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
 290                 : m_aabbCallback(orgCallback)
 291         {
 292         }
 293         void Process(const btDbvtNode* leaf)
 294         {
 295                 btDbvtProxy* proxy = (btDbvtProxy*)leaf->data;
 296                 m_aabbCallback.process(proxy);
 297         }
 298 };
 299
 300 void btDbvtBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& aabbCallback)
 301 {
 302         BroadphaseAabbTester callback(aabbCallback);
 303
 304         const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(aabbMin, aabbMax);
 305         //process all children, that overlap with  the given AABB bounds
 306         m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
 307         m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
 308 }
 309
 310 //
 311 void btDbvtBroadphase::setAabb(btBroadphaseProxy* absproxy,
 312                                                            const btVector3& aabbMin,
 313                                                            const btVector3& aabbMax,
 314                                                            btDispatcher* /*dispatcher*/)
 315 {
 316         btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
 317         ATTRIBUTE_ALIGNED16(btDbvtVolume)
 318         aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
 319 #if DBVT_BP_PREVENTFALSEUPDATE
 320         if (NotEqual(aabb, proxy->leaf->volume))
 321 #endif
 322         {
 323                 bool docollide = false;
 324                 if (proxy->stage == STAGECOUNT)
 325                 { /* fixed -> dynamic set       */
 326                         m_sets[1].remove(proxy->leaf);
 327                         proxy->leaf = m_sets[0].insert(aabb, proxy);
 328                         docollide = true;
 329                 }
 330                 else
 331                 { /* dynamic set                                */
 332                         ++m_updates_call;
 333                         if (Intersect(proxy->leaf->volume, aabb))
 334                         { /* Moving                             */
 335
 336                                 const btVector3 delta = aabbMin - proxy->m_aabbMin;
 337                                 btVector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
 338                                 if (delta[0] < 0) velocity[0] = -velocity[0];
 339                                 if (delta[1] < 0) velocity[1] = -velocity[1];
 340                                 if (delta[2] < 0) velocity[2] = -velocity[2];
 341                                 if (
 342                                         m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
 343
 344                                 )
 345                                 {
 346                                         ++m_updates_done;
 347                                         docollide = true;
 348                                 }
 349                         }
 350                         else
 351                         { /* Teleporting                        */
 352                                 m_sets[0].update(proxy->leaf, aabb);
 353                                 ++m_updates_done;
 354                                 docollide = true;
 355                         }
 356                 }
 357                 listremove(proxy, m_stageRoots[proxy->stage]);
 358                 proxy->m_aabbMin = aabbMin;
 359                 proxy->m_aabbMax = aabbMax;
 360                 proxy->stage = m_stageCurrent;
 361                 listappend(proxy, m_stageRoots[m_stageCurrent]);
 362                 if (docollide)
 363                 {
 364                         m_needcleanup = true;
 365                         if (!m_deferedcollide)
 366                         {
 367                                 btDbvtTreeCollider collider(this);
 368                                 m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
 369                                 m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 370                         }
 371                 }
 372         }
 373 }
 374
 375 //
 376 void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy,
 377                                                                                   const btVector3& aabbMin,
 378                                                                                   const btVector3& aabbMax,
 379                                                                                   btDispatcher* /*dispatcher*/)
 380 {
 381         btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
 382         ATTRIBUTE_ALIGNED16(btDbvtVolume)
 383         aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
 384         bool docollide = false;
 385         if (proxy->stage == STAGECOUNT)
 386         { /* fixed -> dynamic set       */
 387                 m_sets[1].remove(proxy->leaf);
 388                 proxy->leaf = m_sets[0].insert(aabb, proxy);
 389                 docollide = true;
 390         }
 391         else
 392         { /* dynamic set                                */
 393                 ++m_updates_call;
 394                 /* Teleporting                  */
 395                 m_sets[0].update(proxy->leaf, aabb);
 396                 ++m_updates_done;
 397                 docollide = true;
 398         }
 399         listremove(proxy, m_stageRoots[proxy->stage]);
 400         proxy->m_aabbMin = aabbMin;
 401         proxy->m_aabbMax = aabbMax;
 402         proxy->stage = m_stageCurrent;
 403         listappend(proxy, m_stageRoots[m_stageCurrent]);
 404         if (docollide)
 405         {
 406                 m_needcleanup = true;
 407                 if (!m_deferedcollide)
 408                 {
 409                         btDbvtTreeCollider collider(this);
 410                         m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
 411                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 412                 }
 413         }
 414 }
 415
 416 //
 417 void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 418 {
 419         collide(dispatcher);
 420 #if DBVT_BP_PROFILE
 421         if (0 == (m_pid % DBVT_BP_PROFILING_RATE))
 422         {
 423                 printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
 424                 unsigned int total = m_profiling.m_total;
 425                 if (total <= 0) total = 1;
 426                 printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / DBVT_BP_PROFILING_RATE);
 427                 printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / DBVT_BP_PROFILING_RATE);
 428                 printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / DBVT_BP_PROFILING_RATE);
 429                 printf("total:     %uus\r\n", total / DBVT_BP_PROFILING_RATE);
 430                 const unsigned long sum = m_profiling.m_ddcollide +
 431                                                                   m_profiling.m_fdcollide +
 432                                                                   m_profiling.m_cleanup;
 433                 printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / DBVT_BP_PROFILING_RATE);
 434                 printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * DBVT_BP_PROFILING_RATE));
 435                 clear(m_profiling);
 436                 m_clock.reset();
 437         }
 438 #endif
 439
 440         performDeferredRemoval(dispatcher);
 441 }
 442
 443 void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
 444 {
 445         if (m_paircache->hasDeferredRemoval())
 446         {
 447                 btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
 448
 449                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 450                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 451
 452                 int invalidPair = 0;
 453
 454                 int i;
 455
 456                 btBroadphasePair previousPair;
 457                 previousPair.m_pProxy0 = 0;
 458                 previousPair.m_pProxy1 = 0;
 459                 previousPair.m_algorithm = 0;
 460
 461                 for (i = 0; i < overlappingPairArray.size(); i++)
 462                 {
 463                         btBroadphasePair& pair = overlappingPairArray[i];
 464
 465                         bool isDuplicate = (pair == previousPair);
 466
 467                         previousPair = pair;
 468
 469                         bool needsRemoval = false;
 470
 471                         if (!isDuplicate)
 472                         {
 473                                 //important to perform AABB check that is consistent with the broadphase
 474                                 btDbvtProxy* pa = (btDbvtProxy*)pair.m_pProxy0;
 475                                 btDbvtProxy* pb = (btDbvtProxy*)pair.m_pProxy1;
 476                                 bool hasOverlap = Intersect(pa->leaf->volume, pb->leaf->volume);
 477
 478                                 if (hasOverlap)
 479                                 {
 480                                         needsRemoval = false;
 481                                 }
 482                                 else
 483                                 {
 484                                         needsRemoval = true;
 485                                 }
 486                         }
 487                         else
 488                         {
 489                                 //remove duplicate
 490                                 needsRemoval = true;
 491                                 //should have no algorithm
 492                                 btAssert(!pair.m_algorithm);
 493                         }
 494
 495                         if (needsRemoval)
 496                         {
 497                                 m_paircache->cleanOverlappingPair(pair, dispatcher);
 498
 499                                 pair.m_pProxy0 = 0;
 500                                 pair.m_pProxy1 = 0;
 501                                 invalidPair++;
 502                         }
 503                 }
 504
 505                 //perform a sort, to sort 'invalid' pairs to the end
 506                 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 507                 overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
 508         }
 509 }
 510
 511 //
 512 void btDbvtBroadphase::collide(btDispatcher* dispatcher)
 513 {
 514         /*printf("---------------------------------------------------------\n");
 515         printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
 516         printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
 517         printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
 518         {
 519                 int i;
 520                 for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
 521                 {
 522                         printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
 523                                 getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
 524                 }
 525                 printf("\n");
 526         }
 527 */
 528
 529         SPC(m_profiling.m_total);
 530         /* optimize                             */
 531         m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
 532         if (m_fixedleft)
 533         {
 534                 const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
 535                 m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
 536                 m_fixedleft = btMax<int>(0, m_fixedleft - count);
 537         }
 538         /* dynamic -> fixed set */
 539         m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
 540         btDbvtProxy* current = m_stageRoots[m_stageCurrent];
 541         if (current)
 542         {
 543 #if DBVT_BP_ACCURATESLEEPING
 544                 btDbvtTreeCollider collider(this);
 545 #endif
 546                 do
 547                 {
 548                         btDbvtProxy* next = current->links[1];
 549                         listremove(current, m_stageRoots[current->stage]);
 550                         listappend(current, m_stageRoots[STAGECOUNT]);
 551 #if DBVT_BP_ACCURATESLEEPING
 552                         m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
 553                         collider.proxy = current;
 554                         btDbvt::collideTV(m_sets[0].m_root, current->aabb, collider);
 555                         btDbvt::collideTV(m_sets[1].m_root, current->aabb, collider);
 556 #endif
 557                         m_sets[0].remove(current->leaf);
 558                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
 559                         curAabb = btDbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
 560                         current->leaf = m_sets[1].insert(curAabb, current);
 561                         current->stage = STAGECOUNT;
 562                         current = next;
 563                 } while (current);
 564                 m_fixedleft = m_sets[1].m_leaves;
 565                 m_needcleanup = true;
 566         }
 567         /* collide dynamics             */
 568         {
 569                 btDbvtTreeCollider collider(this);
 570                 if (m_deferedcollide)
 571                 {
 572                         SPC(m_profiling.m_fdcollide);
 573                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
 574                 }
 575                 if (m_deferedcollide)
 576                 {
 577                         SPC(m_profiling.m_ddcollide);
 578                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
 579                 }
 580         }
 581         /* clean up                             */
 582         if (m_needcleanup)
 583         {
 584                 SPC(m_profiling.m_cleanup);
 585                 btBroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
 586                 if (pairs.size() > 0)
 587                 {
 588                         int ni = btMin(pairs.size(), btMax<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
 589                         for (int i = 0; i < ni; ++i)
 590                         {
 591                                 btBroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
 592                                 btDbvtProxy* pa = (btDbvtProxy*)p.m_pProxy0;
 593                                 btDbvtProxy* pb = (btDbvtProxy*)p.m_pProxy1;
 594                                 if (!Intersect(pa->leaf->volume, pb->leaf->volume))
 595                                 {
 596 #if DBVT_BP_SORTPAIRS
 597                                         if (pa->m_uniqueId > pb->m_uniqueId)
 598                                                 btSwap(pa, pb);
 599 #endif
 600                                         m_paircache->removeOverlappingPair(pa, pb, dispatcher);
 601                                         --ni;
 602                                         --i;
 603                                 }
 604                         }
 605                         if (pairs.size() > 0)
 606                                 m_cid = (m_cid + ni) % pairs.size();
 607                         else
 608                                 m_cid = 0;
 609                 }
 610         }
 611         ++m_pid;
 612         m_newpairs = 1;
 613         m_needcleanup = false;
 614         if (m_updates_call > 0)
 615         {
 616                 m_updates_ratio = m_updates_done / (btScalar)m_updates_call;
 617         }
 618         else
 619         {
 620                 m_updates_ratio = 0;
 621         }
 622         m_updates_done /= 2;
 623         m_updates_call /= 2;
 624 }
 625
 626 //
 627 void btDbvtBroadphase::optimize()
 628 {
 629         m_sets[0].optimizeTopDown();
 630         m_sets[1].optimizeTopDown();
 631 }
 632
 633 //
 634 btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
 635 {
 636         return (m_paircache);
 637 }
 638
 639 //
 640 const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
 641 {
 642         return (m_paircache);
 643 }
 644
 645 //
 646 void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
 647 {
 648         ATTRIBUTE_ALIGNED16(btDbvtVolume)
 649         bounds;
 650
 651         if (!m_sets[0].empty())
 652                 if (!m_sets[1].empty())
 653                         Merge(m_sets[0].m_root->volume,
 654                                   m_sets[1].m_root->volume, bounds);
 655                 else
 656                         bounds = m_sets[0].m_root->volume;
 657         else if (!m_sets[1].empty())
 658                 bounds = m_sets[1].m_root->volume;
 659         else
 660                 bounds = btDbvtVolume::FromCR(btVector3(0, 0, 0), 0);
 661         aabbMin = bounds.Mins();
 662         aabbMax = bounds.Maxs();
 663 }
 664
 665 void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
 666 {
 667         int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
 668         if (!totalObjects)
 669         {
 670                 //reset internal dynamic tree data structures
 671                 m_sets[0].clear();
 672                 m_sets[1].clear();
 673
 674                 m_deferedcollide = false;
 675                 m_needcleanup = true;
 676                 m_stageCurrent = 0;
 677                 m_fixedleft = 0;
 678                 m_fupdates = 1;
 679                 m_dupdates = 0;
 680                 m_cupdates = 10;
 681                 m_newpairs = 1;
 682                 m_updates_call = 0;
 683                 m_updates_done = 0;
 684                 m_updates_ratio = 0;
 685
 686                 m_gid = 0;
 687                 m_pid = 0;
 688                 m_cid = 0;
 689                 for (int i = 0; i <= STAGECOUNT; ++i)
 690                 {
 691                         m_stageRoots[i] = 0;
 692                 }
 693         }
 694 }
 695
 696 //
 697 void btDbvtBroadphase::printStats()
 698 {
 699 }
 700
 701 //
 702 #if DBVT_BP_ENABLE_BENCHMARK
 703
 704 struct btBroadphaseBenchmark
 705 {
 706         struct Experiment
 707         {
 708                 const char* name;
 709                 int object_count;
 710                 int update_count;
 711                 int spawn_count;
 712                 int iterations;
 713                 btScalar speed;
 714                 btScalar amplitude;
 715         };
 716         struct Object
 717         {
 718                 btVector3 center;
 719                 btVector3 extents;
 720                 btBroadphaseProxy* proxy;
 721                 btScalar time;
 722                 void update(btScalar speed, btScalar amplitude, btBroadphaseInterface* pbi)
 723                 {
 724                         time += speed;
 725                         center[0] = btCos(time * (btScalar)2.17) * amplitude +
 726                                                 btSin(time) * amplitude / 2;
 727                         center[1] = btCos(time * (btScalar)1.38) * amplitude +
 728                                                 btSin(time) * amplitude;
 729                         center[2] = btSin(time * (btScalar)0.777) * amplitude;
 730                         pbi->setAabb(proxy, center - extents, center + extents, 0);
 731                 }
 732         };
 733         static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
 734         static btScalar UnitRand() { return (UnsignedRand(16384) / (btScalar)16384); }
 735         static void OutputTime(const char* name, btClock& c, unsigned count = 0)
 736         {
 737                 const unsigned long us = c.getTimeMicroseconds();
 738                 const unsigned long ms = (us + 500) / 1000;
 739                 const btScalar sec = us / (btScalar)(1000 * 1000);
 740                 if (count > 0)
 741                         printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
 742                 else
 743                         printf("%s : %u us (%u ms)\r\n", name, us, ms);
 744         }
 745 };
 746
 747 void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
 748 {
 749         static const btBroadphaseBenchmark::Experiment experiments[] =
 750                 {
 751                         {"1024o.10%", 1024, 10, 0, 8192, (btScalar)0.005, (btScalar)100},
 752                         /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
 753                 {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
 754                 };
 755         static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
 756         btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
 757         btClock wallclock;
 758         /* Begin                        */
 759         for (int iexp = 0; iexp < nexperiments; ++iexp)
 760         {
 761                 const btBroadphaseBenchmark::Experiment& experiment = experiments[iexp];
 762                 const int object_count = experiment.object_count;
 763                 const int update_count = (object_count * experiment.update_count) / 100;
 764                 const int spawn_count = (object_count * experiment.spawn_count) / 100;
 765                 const btScalar speed = experiment.speed;
 766                 const btScalar amplitude = experiment.amplitude;
 767                 printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
 768                 printf("\tObjects: %u\r\n", object_count);
 769                 printf("\tUpdate: %u\r\n", update_count);
 770                 printf("\tSpawn: %u\r\n", spawn_count);
 771                 printf("\tSpeed: %f\r\n", speed);
 772                 printf("\tAmplitude: %f\r\n", amplitude);
 773                 srand(180673);
 774                 /* Create objects       */
 775                 wallclock.reset();
 776                 objects.reserve(object_count);
 777                 for (int i = 0; i < object_count; ++i)
 778                 {
 779                         btBroadphaseBenchmark::Object* po = new btBroadphaseBenchmark::Object();
 780                         po->center[0] = btBroadphaseBenchmark::UnitRand() * 50;
 781                         po->center[1] = btBroadphaseBenchmark::UnitRand() * 50;
 782                         po->center[2] = btBroadphaseBenchmark::UnitRand() * 50;
 783                         po->extents[0] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
 784                         po->extents[1] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
 785                         po->extents[2] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
 786                         po->time = btBroadphaseBenchmark::UnitRand() * 2000;
 787                         po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
 788                         objects.push_back(po);
 789                 }
 790                 btBroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
 791                 /* First update         */
 792                 wallclock.reset();
 793                 for (int i = 0; i < objects.size(); ++i)
 794                 {
 795                         objects[i]->update(speed, amplitude, pbi);
 796                 }
 797                 btBroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
 798                 /* Updates                      */
 799                 wallclock.reset();
 800                 for (int i = 0; i < experiment.iterations; ++i)
 801                 {
 802                         for (int j = 0; j < update_count; ++j)
 803                         {
 804                                 objects[j]->update(speed, amplitude, pbi);
 805                         }
 806                         pbi->calculateOverlappingPairs(0);
 807                 }
 808                 btBroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
 809                 /* Clean up                     */
 810                 wallclock.reset();
 811                 for (int i = 0; i < objects.size(); ++i)
 812                 {
 813                         pbi->destroyProxy(objects[i]->proxy, 0);
 814                         delete objects[i];
 815                 }
 816                 objects.resize(0);
 817                 btBroadphaseBenchmark::OutputTime("\tRelease", wallclock);
 818         }
 819 }
 820 #else
 821 void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
 822 {
 823 }
 824 #endif
 825
 826 #if DBVT_BP_PROFILE
 827 #undef SPC
 828 #endif