dali-physics/third-party/bullet3/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2013 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 ///b3DynamicBvhBroadphase implementation by Nathanael Presson
  17
  18 #include "b3DynamicBvhBroadphase.h"
  19 #include "b3OverlappingPair.h"
  20
  21 //
  22 // Profiling
  23 //
  24
  25 #if B3_DBVT_BP_PROFILE || B3_DBVT_BP_ENABLE_BENCHMARK
  26 #include <stdio.h>
  27 #endif
  28
  29 #if B3_DBVT_BP_PROFILE
  30 struct b3ProfileScope
  31 {
  32         __forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
  33         {
  34         }
  35         __forceinline ~b3ProfileScope()
  36         {
  37                 (*m_value) += m_clock->getTimeMicroseconds() - m_base;
  38         }
  39         b3Clock* m_clock;
  40         unsigned long* m_value;
  41         unsigned long m_base;
  42 };
  43 #define b3SPC(_value_) b3ProfileScope spc_scope(m_clock, _value_)
  44 #else
  45 #define b3SPC(_value_)
  46 #endif
  47
  48 //
  49 // Helpers
  50 //
  51
  52 //
  53 template <typename T>
  54 static inline void b3ListAppend(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 b3ListRemove(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 b3ListCount(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 b3Clear(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 b3DbvtTreeCollider : b3DynamicBvh::ICollide
 102 {
 103         b3DynamicBvhBroadphase* pbp;
 104         b3DbvtProxy* proxy;
 105         b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
 106         void Process(const b3DbvtNode* na, const b3DbvtNode* nb)
 107         {
 108                 if (na != nb)
 109                 {
 110                         b3DbvtProxy* pa = (b3DbvtProxy*)na->data;
 111                         b3DbvtProxy* pb = (b3DbvtProxy*)nb->data;
 112 #if B3_DBVT_BP_SORTPAIRS
 113                         if (pa->m_uniqueId > pb->m_uniqueId)
 114                                 b3Swap(pa, pb);
 115 #endif
 116                         pbp->m_paircache->addOverlappingPair(pa->getUid(), pb->getUid());
 117                         ++pbp->m_newpairs;
 118                 }
 119         }
 120         void Process(const b3DbvtNode* n)
 121         {
 122                 Process(n, proxy->leaf);
 123         }
 124 };
 125
 126 //
 127 // b3DynamicBvhBroadphase
 128 //
 129
 130 //
 131 b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* 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 (b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache), 16)) b3HashedOverlappingPairCache();
 147
 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 B3_DBVT_BP_PROFILE
 155         b3Clear(m_profiling);
 156 #endif
 157         m_proxies.resize(proxyCapacity);
 158 }
 159
 160 //
 161 b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
 162 {
 163         if (m_releasepaircache)
 164         {
 165                 m_paircache->~b3OverlappingPairCache();
 166                 b3AlignedFree(m_paircache);
 167         }
 168 }
 169
 170 //
 171 b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy(const b3Vector3& aabbMin,
 172                                                                                                            const b3Vector3& aabbMax,
 173                                                                                                            int objectId,
 174                                                                                                            void* userPtr,
 175                                                                                                            int collisionFilterGroup,
 176                                                                                                            int collisionFilterMask)
 177 {
 178         b3DbvtProxy* mem = &m_proxies[objectId];
 179         b3DbvtProxy* proxy = new (mem) b3DbvtProxy(aabbMin, aabbMax, userPtr,
 180                                                                                            collisionFilterGroup,
 181                                                                                            collisionFilterMask);
 182
 183         b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
 184
 185         //bproxy->aabb                  =       b3DbvtVolume::FromMM(aabbMin,aabbMax);
 186         proxy->stage = m_stageCurrent;
 187         proxy->m_uniqueId = objectId;
 188         proxy->leaf = m_sets[0].insert(aabb, proxy);
 189         b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
 190         if (!m_deferedcollide)
 191         {
 192                 b3DbvtTreeCollider collider(this);
 193                 collider.proxy = proxy;
 194                 m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
 195                 m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
 196         }
 197         return (proxy);
 198 }
 199
 200 //
 201 void b3DynamicBvhBroadphase::destroyProxy(b3BroadphaseProxy* absproxy,
 202                                                                                   b3Dispatcher* dispatcher)
 203 {
 204         b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
 205         if (proxy->stage == STAGECOUNT)
 206                 m_sets[1].remove(proxy->leaf);
 207         else
 208                 m_sets[0].remove(proxy->leaf);
 209         b3ListRemove(proxy, m_stageRoots[proxy->stage]);
 210         m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(), dispatcher);
 211
 212         m_needcleanup = true;
 213 }
 214
 215 void b3DynamicBvhBroadphase::getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const
 216 {
 217         const b3DbvtProxy* proxy = &m_proxies[objectId];
 218         aabbMin = proxy->m_aabbMin;
 219         aabbMax = proxy->m_aabbMax;
 220 }
 221 /*
 222 void    b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
 223 {
 224         b3DbvtProxy*                                            proxy=(b3DbvtProxy*)absproxy;
 225         aabbMin = proxy->m_aabbMin;
 226         aabbMax = proxy->m_aabbMax;
 227 }
 228 */
 229
 230 struct BroadphaseRayTester : b3DynamicBvh::ICollide
 231 {
 232         b3BroadphaseRayCallback& m_rayCallback;
 233         BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
 234                 : m_rayCallback(orgCallback)
 235         {
 236         }
 237         void Process(const b3DbvtNode* leaf)
 238         {
 239                 b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
 240                 m_rayCallback.process(proxy);
 241         }
 242 };
 243
 244 void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax)
 245 {
 246         BroadphaseRayTester callback(rayCallback);
 247
 248         m_sets[0].rayTestInternal(m_sets[0].m_root,
 249                                                           rayFrom,
 250                                                           rayTo,
 251                                                           rayCallback.m_rayDirectionInverse,
 252                                                           rayCallback.m_signs,
 253                                                           rayCallback.m_lambda_max,
 254                                                           aabbMin,
 255                                                           aabbMax,
 256                                                           callback);
 257
 258         m_sets[1].rayTestInternal(m_sets[1].m_root,
 259                                                           rayFrom,
 260                                                           rayTo,
 261                                                           rayCallback.m_rayDirectionInverse,
 262                                                           rayCallback.m_signs,
 263                                                           rayCallback.m_lambda_max,
 264                                                           aabbMin,
 265                                                           aabbMax,
 266                                                           callback);
 267 }
 268
 269 struct BroadphaseAabbTester : b3DynamicBvh::ICollide
 270 {
 271         b3BroadphaseAabbCallback& m_aabbCallback;
 272         BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
 273                 : m_aabbCallback(orgCallback)
 274         {
 275         }
 276         void Process(const b3DbvtNode* leaf)
 277         {
 278                 b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
 279                 m_aabbCallback.process(proxy);
 280         }
 281 };
 282
 283 void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& aabbCallback)
 284 {
 285         BroadphaseAabbTester callback(aabbCallback);
 286
 287         const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds = b3DbvtVolume::FromMM(aabbMin, aabbMax);
 288         //process all children, that overlap with  the given AABB bounds
 289         m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
 290         m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
 291 }
 292
 293 //
 294 void b3DynamicBvhBroadphase::setAabb(int objectId,
 295                                                                          const b3Vector3& aabbMin,
 296                                                                          const b3Vector3& aabbMax,
 297                                                                          b3Dispatcher* /*dispatcher*/)
 298 {
 299         b3DbvtProxy* proxy = &m_proxies[objectId];
 300         //      b3DbvtProxy*                                            proxy=(b3DbvtProxy*)absproxy;
 301         B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
 302         aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
 303 #if B3_DBVT_BP_PREVENTFALSEUPDATE
 304         if (b3NotEqual(aabb, proxy->leaf->volume))
 305 #endif
 306         {
 307                 bool docollide = false;
 308                 if (proxy->stage == STAGECOUNT)
 309                 { /* fixed -> dynamic set       */
 310                         m_sets[1].remove(proxy->leaf);
 311                         proxy->leaf = m_sets[0].insert(aabb, proxy);
 312                         docollide = true;
 313                 }
 314                 else
 315                 { /* dynamic set                                */
 316                         ++m_updates_call;
 317                         if (b3Intersect(proxy->leaf->volume, aabb))
 318                         { /* Moving                             */
 319
 320                                 const b3Vector3 delta = aabbMin - proxy->m_aabbMin;
 321                                 b3Vector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
 322                                 if (delta[0] < 0) velocity[0] = -velocity[0];
 323                                 if (delta[1] < 0) velocity[1] = -velocity[1];
 324                                 if (delta[2] < 0) velocity[2] = -velocity[2];
 325                                 if (
 326 #ifdef B3_DBVT_BP_MARGIN
 327                                         m_sets[0].update(proxy->leaf, aabb, velocity, B3_DBVT_BP_MARGIN)
 328 #else
 329                                         m_sets[0].update(proxy->leaf, aabb, velocity)
 330 #endif
 331                                 )
 332                                 {
 333                                         ++m_updates_done;
 334                                         docollide = true;
 335                                 }
 336                         }
 337                         else
 338                         { /* Teleporting                        */
 339                                 m_sets[0].update(proxy->leaf, aabb);
 340                                 ++m_updates_done;
 341                                 docollide = true;
 342                         }
 343                 }
 344                 b3ListRemove(proxy, m_stageRoots[proxy->stage]);
 345                 proxy->m_aabbMin = aabbMin;
 346                 proxy->m_aabbMax = aabbMax;
 347                 proxy->stage = m_stageCurrent;
 348                 b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
 349                 if (docollide)
 350                 {
 351                         m_needcleanup = true;
 352                         if (!m_deferedcollide)
 353                         {
 354                                 b3DbvtTreeCollider collider(this);
 355                                 m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
 356                                 m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 357                         }
 358                 }
 359         }
 360 }
 361
 362 //
 363 void b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
 364                                                                                                 const b3Vector3& aabbMin,
 365                                                                                                 const b3Vector3& aabbMax,
 366                                                                                                 b3Dispatcher* /*dispatcher*/)
 367 {
 368         b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
 369         B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
 370         aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
 371         bool docollide = false;
 372         if (proxy->stage == STAGECOUNT)
 373         { /* fixed -> dynamic set       */
 374                 m_sets[1].remove(proxy->leaf);
 375                 proxy->leaf = m_sets[0].insert(aabb, proxy);
 376                 docollide = true;
 377         }
 378         else
 379         { /* dynamic set                                */
 380                 ++m_updates_call;
 381                 /* Teleporting                  */
 382                 m_sets[0].update(proxy->leaf, aabb);
 383                 ++m_updates_done;
 384                 docollide = true;
 385         }
 386         b3ListRemove(proxy, m_stageRoots[proxy->stage]);
 387         proxy->m_aabbMin = aabbMin;
 388         proxy->m_aabbMax = aabbMax;
 389         proxy->stage = m_stageCurrent;
 390         b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
 391         if (docollide)
 392         {
 393                 m_needcleanup = true;
 394                 if (!m_deferedcollide)
 395                 {
 396                         b3DbvtTreeCollider collider(this);
 397                         m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
 398                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 399                 }
 400         }
 401 }
 402
 403 //
 404 void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
 405 {
 406         collide(dispatcher);
 407 #if B3_DBVT_BP_PROFILE
 408         if (0 == (m_pid % B3_DBVT_BP_PROFILING_RATE))
 409         {
 410                 printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
 411                 unsigned int total = m_profiling.m_total;
 412                 if (total <= 0) total = 1;
 413                 printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / B3_DBVT_BP_PROFILING_RATE);
 414                 printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / B3_DBVT_BP_PROFILING_RATE);
 415                 printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / B3_DBVT_BP_PROFILING_RATE);
 416                 printf("total:     %uus\r\n", total / B3_DBVT_BP_PROFILING_RATE);
 417                 const unsigned long sum = m_profiling.m_ddcollide +
 418                                                                   m_profiling.m_fdcollide +
 419                                                                   m_profiling.m_cleanup;
 420                 printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / B3_DBVT_BP_PROFILING_RATE);
 421                 printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * B3_DBVT_BP_PROFILING_RATE));
 422                 b3Clear(m_profiling);
 423                 m_clock.reset();
 424         }
 425 #endif
 426
 427         performDeferredRemoval(dispatcher);
 428 }
 429
 430 void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
 431 {
 432         if (m_paircache->hasDeferredRemoval())
 433         {
 434                 b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
 435
 436                 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 437                 overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
 438
 439                 int invalidPair = 0;
 440
 441                 int i;
 442
 443                 b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
 444
 445                 for (i = 0; i < overlappingPairArray.size(); i++)
 446                 {
 447                         b3BroadphasePair& pair = overlappingPairArray[i];
 448
 449                         bool isDuplicate = (pair == previousPair);
 450
 451                         previousPair = pair;
 452
 453                         bool needsRemoval = false;
 454
 455                         if (!isDuplicate)
 456                         {
 457                                 //important to perform AABB check that is consistent with the broadphase
 458                                 b3DbvtProxy* pa = &m_proxies[pair.x];
 459                                 b3DbvtProxy* pb = &m_proxies[pair.y];
 460                                 bool hasOverlap = b3Intersect(pa->leaf->volume, pb->leaf->volume);
 461
 462                                 if (hasOverlap)
 463                                 {
 464                                         needsRemoval = false;
 465                                 }
 466                                 else
 467                                 {
 468                                         needsRemoval = true;
 469                                 }
 470                         }
 471                         else
 472                         {
 473                                 //remove duplicate
 474                                 needsRemoval = true;
 475                                 //should have no algorithm
 476                         }
 477
 478                         if (needsRemoval)
 479                         {
 480                                 m_paircache->cleanOverlappingPair(pair, dispatcher);
 481
 482                                 pair.x = -1;
 483                                 pair.y = -1;
 484                                 invalidPair++;
 485                         }
 486                 }
 487
 488                 //perform a sort, to sort 'invalid' pairs to the end
 489                 overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
 490                 overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
 491         }
 492 }
 493
 494 //
 495 void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
 496 {
 497         /*printf("---------------------------------------------------------\n");
 498         printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
 499         printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
 500         printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
 501         {
 502                 int i;
 503                 for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
 504                 {
 505                         printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
 506                                 getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
 507                 }
 508                 printf("\n");
 509         }
 510 */
 511
 512         b3SPC(m_profiling.m_total);
 513         /* optimize                             */
 514         m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
 515         if (m_fixedleft)
 516         {
 517                 const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
 518                 m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
 519                 m_fixedleft = b3Max<int>(0, m_fixedleft - count);
 520         }
 521         /* dynamic -> fixed set */
 522         m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
 523         b3DbvtProxy* current = m_stageRoots[m_stageCurrent];
 524         if (current)
 525         {
 526                 b3DbvtTreeCollider collider(this);
 527                 do
 528                 {
 529                         b3DbvtProxy* next = current->links[1];
 530                         b3ListRemove(current, m_stageRoots[current->stage]);
 531                         b3ListAppend(current, m_stageRoots[STAGECOUNT]);
 532 #if B3_DBVT_BP_ACCURATESLEEPING
 533                         m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
 534                         collider.proxy = current;
 535                         b3DynamicBvh::collideTV(m_sets[0].m_root, current->aabb, collider);
 536                         b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
 537 #endif
 538                         m_sets[0].remove(current->leaf);
 539                         B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
 540                         curAabb = b3DbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
 541                         current->leaf = m_sets[1].insert(curAabb, current);
 542                         current->stage = STAGECOUNT;
 543                         current = next;
 544                 } while (current);
 545                 m_fixedleft = m_sets[1].m_leaves;
 546                 m_needcleanup = true;
 547         }
 548         /* collide dynamics             */
 549         {
 550                 b3DbvtTreeCollider collider(this);
 551                 if (m_deferedcollide)
 552                 {
 553                         b3SPC(m_profiling.m_fdcollide);
 554                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
 555                 }
 556                 if (m_deferedcollide)
 557                 {
 558                         b3SPC(m_profiling.m_ddcollide);
 559                         m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
 560                 }
 561         }
 562         /* clean up                             */
 563         if (m_needcleanup)
 564         {
 565                 b3SPC(m_profiling.m_cleanup);
 566                 b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
 567                 if (pairs.size() > 0)
 568                 {
 569                         int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
 570                         for (int i = 0; i < ni; ++i)
 571                         {
 572                                 b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
 573                                 b3DbvtProxy* pa = &m_proxies[p.x];
 574                                 b3DbvtProxy* pb = &m_proxies[p.y];
 575                                 if (!b3Intersect(pa->leaf->volume, pb->leaf->volume))
 576                                 {
 577 #if B3_DBVT_BP_SORTPAIRS
 578                                         if (pa->m_uniqueId > pb->m_uniqueId)
 579                                                 b3Swap(pa, pb);
 580 #endif
 581                                         m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), dispatcher);
 582                                         --ni;
 583                                         --i;
 584                                 }
 585                         }
 586                         if (pairs.size() > 0)
 587                                 m_cid = (m_cid + ni) % pairs.size();
 588                         else
 589                                 m_cid = 0;
 590                 }
 591         }
 592         ++m_pid;
 593         m_newpairs = 1;
 594         m_needcleanup = false;
 595         if (m_updates_call > 0)
 596         {
 597                 m_updates_ratio = m_updates_done / (b3Scalar)m_updates_call;
 598         }
 599         else
 600         {
 601                 m_updates_ratio = 0;
 602         }
 603         m_updates_done /= 2;
 604         m_updates_call /= 2;
 605 }
 606
 607 //
 608 void b3DynamicBvhBroadphase::optimize()
 609 {
 610         m_sets[0].optimizeTopDown();
 611         m_sets[1].optimizeTopDown();
 612 }
 613
 614 //
 615 b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
 616 {
 617         return (m_paircache);
 618 }
 619
 620 //
 621 const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
 622 {
 623         return (m_paircache);
 624 }
 625
 626 //
 627 void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
 628 {
 629         B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
 630         bounds;
 631
 632         if (!m_sets[0].empty())
 633                 if (!m_sets[1].empty())
 634                         b3Merge(m_sets[0].m_root->volume,
 635                                         m_sets[1].m_root->volume, bounds);
 636                 else
 637                         bounds = m_sets[0].m_root->volume;
 638         else if (!m_sets[1].empty())
 639                 bounds = m_sets[1].m_root->volume;
 640         else
 641                 bounds = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
 642         aabbMin = bounds.Mins();
 643         aabbMax = bounds.Maxs();
 644 }
 645
 646 void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
 647 {
 648         int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
 649         if (!totalObjects)
 650         {
 651                 //reset internal dynamic tree data structures
 652                 m_sets[0].clear();
 653                 m_sets[1].clear();
 654
 655                 m_deferedcollide = false;
 656                 m_needcleanup = true;
 657                 m_stageCurrent = 0;
 658                 m_fixedleft = 0;
 659                 m_fupdates = 1;
 660                 m_dupdates = 0;
 661                 m_cupdates = 10;
 662                 m_newpairs = 1;
 663                 m_updates_call = 0;
 664                 m_updates_done = 0;
 665                 m_updates_ratio = 0;
 666
 667                 m_pid = 0;
 668                 m_cid = 0;
 669                 for (int i = 0; i <= STAGECOUNT; ++i)
 670                 {
 671                         m_stageRoots[i] = 0;
 672                 }
 673         }
 674 }
 675
 676 //
 677 void b3DynamicBvhBroadphase::printStats()
 678 {
 679 }
 680
 681 //
 682 #if B3_DBVT_BP_ENABLE_BENCHMARK
 683
 684 struct b3BroadphaseBenchmark
 685 {
 686         struct Experiment
 687         {
 688                 const char* name;
 689                 int object_count;
 690                 int update_count;
 691                 int spawn_count;
 692                 int iterations;
 693                 b3Scalar speed;
 694                 b3Scalar amplitude;
 695         };
 696         struct Object
 697         {
 698                 b3Vector3 center;
 699                 b3Vector3 extents;
 700                 b3BroadphaseProxy* proxy;
 701                 b3Scalar time;
 702                 void update(b3Scalar speed, b3Scalar amplitude, b3BroadphaseInterface* pbi)
 703                 {
 704                         time += speed;
 705                         center[0] = b3Cos(time * (b3Scalar)2.17) * amplitude +
 706                                                 b3Sin(time) * amplitude / 2;
 707                         center[1] = b3Cos(time * (b3Scalar)1.38) * amplitude +
 708                                                 b3Sin(time) * amplitude;
 709                         center[2] = b3Sin(time * (b3Scalar)0.777) * amplitude;
 710                         pbi->setAabb(proxy, center - extents, center + extents, 0);
 711                 }
 712         };
 713         static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
 714         static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
 715         static void OutputTime(const char* name, b3Clock& c, unsigned count = 0)
 716         {
 717                 const unsigned long us = c.getTimeMicroseconds();
 718                 const unsigned long ms = (us + 500) / 1000;
 719                 const b3Scalar sec = us / (b3Scalar)(1000 * 1000);
 720                 if (count > 0)
 721                         printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
 722                 else
 723                         printf("%s : %u us (%u ms)\r\n", name, us, ms);
 724         }
 725 };
 726
 727 void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
 728 {
 729         static const b3BroadphaseBenchmark::Experiment experiments[] =
 730                 {
 731                         {"1024o.10%", 1024, 10, 0, 8192, (b3Scalar)0.005, (b3Scalar)100},
 732                         /*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
 733                 {"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
 734                 };
 735         static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
 736         b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
 737         b3Clock wallclock;
 738         /* Begin                        */
 739         for (int iexp = 0; iexp < nexperiments; ++iexp)
 740         {
 741                 const b3BroadphaseBenchmark::Experiment& experiment = experiments[iexp];
 742                 const int object_count = experiment.object_count;
 743                 const int update_count = (object_count * experiment.update_count) / 100;
 744                 const int spawn_count = (object_count * experiment.spawn_count) / 100;
 745                 const b3Scalar speed = experiment.speed;
 746                 const b3Scalar amplitude = experiment.amplitude;
 747                 printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
 748                 printf("\tObjects: %u\r\n", object_count);
 749                 printf("\tUpdate: %u\r\n", update_count);
 750                 printf("\tSpawn: %u\r\n", spawn_count);
 751                 printf("\tSpeed: %f\r\n", speed);
 752                 printf("\tAmplitude: %f\r\n", amplitude);
 753                 srand(180673);
 754                 /* Create objects       */
 755                 wallclock.reset();
 756                 objects.reserve(object_count);
 757                 for (int i = 0; i < object_count; ++i)
 758                 {
 759                         b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object();
 760                         po->center[0] = b3BroadphaseBenchmark::UnitRand() * 50;
 761                         po->center[1] = b3BroadphaseBenchmark::UnitRand() * 50;
 762                         po->center[2] = b3BroadphaseBenchmark::UnitRand() * 50;
 763                         po->extents[0] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
 764                         po->extents[1] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
 765                         po->extents[2] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
 766                         po->time = b3BroadphaseBenchmark::UnitRand() * 2000;
 767                         po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
 768                         objects.push_back(po);
 769                 }
 770                 b3BroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
 771                 /* First update         */
 772                 wallclock.reset();
 773                 for (int i = 0; i < objects.size(); ++i)
 774                 {
 775                         objects[i]->update(speed, amplitude, pbi);
 776                 }
 777                 b3BroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
 778                 /* Updates                      */
 779                 wallclock.reset();
 780                 for (int i = 0; i < experiment.iterations; ++i)
 781                 {
 782                         for (int j = 0; j < update_count; ++j)
 783                         {
 784                                 objects[j]->update(speed, amplitude, pbi);
 785                         }
 786                         pbi->calculateOverlappingPairs(0);
 787                 }
 788                 b3BroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
 789                 /* Clean up                     */
 790                 wallclock.reset();
 791                 for (int i = 0; i < objects.size(); ++i)
 792                 {
 793                         pbi->destroyProxy(objects[i]->proxy, 0);
 794                         delete objects[i];
 795                 }
 796                 objects.resize(0);
 797                 b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
 798         }
 799 }
 800 #else
 801 /*void                                                  b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
 802 {}
 803 */
 804 #endif
 805
 806 #if B3_DBVT_BP_PROFILE
 807 #undef b3SPC
 808 #endif