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