| 1 | /* |
|---|
| 2 | Bullet Continuous Collision Detection and Physics Library |
|---|
| 3 | Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ |
|---|
| 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 | ///btDbvtBroadphase implementation by Nathanael Presson |
|---|
| 16 | |
|---|
| 17 | #include "btDbvtBroadphase.h" |
|---|
| 18 | |
|---|
| 19 | // |
|---|
| 20 | // Profiling |
|---|
| 21 | // |
|---|
| 22 | |
|---|
| 23 | #if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK |
|---|
| 24 | #include <stdio.h> |
|---|
| 25 | #endif |
|---|
| 26 | |
|---|
| 27 | #if DBVT_BP_PROFILE |
|---|
| 28 | struct ProfileScope |
|---|
| 29 | { |
|---|
| 30 | __forceinline ProfileScope(btClock& clock,unsigned long& value) : |
|---|
| 31 | m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) |
|---|
| 32 | { |
|---|
| 33 | } |
|---|
| 34 | __forceinline ~ProfileScope() |
|---|
| 35 | { |
|---|
| 36 | (*m_value)+=m_clock->getTimeMicroseconds()-m_base; |
|---|
| 37 | } |
|---|
| 38 | btClock* m_clock; |
|---|
| 39 | unsigned long* m_value; |
|---|
| 40 | unsigned long m_base; |
|---|
| 41 | }; |
|---|
| 42 | #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) |
|---|
| 43 | #else |
|---|
| 44 | #define SPC(_value_) |
|---|
| 45 | #endif |
|---|
| 46 | |
|---|
| 47 | // |
|---|
| 48 | // Helpers |
|---|
| 49 | // |
|---|
| 50 | |
|---|
| 51 | // |
|---|
| 52 | template <typename T> |
|---|
| 53 | static inline void listappend(T* item,T*& list) |
|---|
| 54 | { |
|---|
| 55 | item->links[0]=0; |
|---|
| 56 | item->links[1]=list; |
|---|
| 57 | if(list) list->links[0]=item; |
|---|
| 58 | list=item; |
|---|
| 59 | } |
|---|
| 60 | |
|---|
| 61 | // |
|---|
| 62 | template <typename T> |
|---|
| 63 | static inline void listremove(T* item,T*& list) |
|---|
| 64 | { |
|---|
| 65 | if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; |
|---|
| 66 | if(item->links[1]) item->links[1]->links[0]=item->links[0]; |
|---|
| 67 | } |
|---|
| 68 | |
|---|
| 69 | // |
|---|
| 70 | template <typename T> |
|---|
| 71 | static inline int listcount(T* root) |
|---|
| 72 | { |
|---|
| 73 | int n=0; |
|---|
| 74 | while(root) { ++n;root=root->links[1]; } |
|---|
| 75 | return(n); |
|---|
| 76 | } |
|---|
| 77 | |
|---|
| 78 | // |
|---|
| 79 | template <typename T> |
|---|
| 80 | static inline void clear(T& value) |
|---|
| 81 | { |
|---|
| 82 | static const struct ZeroDummy : T {} zerodummy; |
|---|
| 83 | value=zerodummy; |
|---|
| 84 | } |
|---|
| 85 | |
|---|
| 86 | // |
|---|
| 87 | // Colliders |
|---|
| 88 | // |
|---|
| 89 | |
|---|
| 90 | /* Tree collider */ |
|---|
| 91 | struct btDbvtTreeCollider : btDbvt::ICollide |
|---|
| 92 | { |
|---|
| 93 | btDbvtBroadphase* pbp; |
|---|
| 94 | btDbvtProxy* proxy; |
|---|
| 95 | btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} |
|---|
| 96 | void Process(const btDbvtNode* na,const btDbvtNode* nb) |
|---|
| 97 | { |
|---|
| 98 | if(na!=nb) |
|---|
| 99 | { |
|---|
| 100 | btDbvtProxy* pa=(btDbvtProxy*)na->data; |
|---|
| 101 | btDbvtProxy* pb=(btDbvtProxy*)nb->data; |
|---|
| 102 | #if DBVT_BP_SORTPAIRS |
|---|
| 103 | if(pa>pb) btSwap(pa,pb); |
|---|
| 104 | #endif |
|---|
| 105 | pbp->m_paircache->addOverlappingPair(pa,pb); |
|---|
| 106 | ++pbp->m_newpairs; |
|---|
| 107 | } |
|---|
| 108 | } |
|---|
| 109 | void Process(const btDbvtNode* n) |
|---|
| 110 | { |
|---|
| 111 | Process(n,proxy->leaf); |
|---|
| 112 | } |
|---|
| 113 | }; |
|---|
| 114 | |
|---|
| 115 | // |
|---|
| 116 | // btDbvtBroadphase |
|---|
| 117 | // |
|---|
| 118 | |
|---|
| 119 | // |
|---|
| 120 | btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) |
|---|
| 121 | { |
|---|
| 122 | m_deferedcollide = false; |
|---|
| 123 | m_needcleanup = true; |
|---|
| 124 | m_releasepaircache = (paircache!=0)?false:true; |
|---|
| 125 | m_prediction = 1/(btScalar)2; |
|---|
| 126 | m_stageCurrent = 0; |
|---|
| 127 | m_fixedleft = 0; |
|---|
| 128 | m_fupdates = 1; |
|---|
| 129 | m_dupdates = 0; |
|---|
| 130 | m_cupdates = 10; |
|---|
| 131 | m_newpairs = 1; |
|---|
| 132 | m_updates_call = 0; |
|---|
| 133 | m_updates_done = 0; |
|---|
| 134 | m_updates_ratio = 0; |
|---|
| 135 | m_paircache = paircache? |
|---|
| 136 | paircache : |
|---|
| 137 | 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 | // |
|---|
| 151 | btDbvtBroadphase::~btDbvtBroadphase() |
|---|
| 152 | { |
|---|
| 153 | if(m_releasepaircache) |
|---|
| 154 | { |
|---|
| 155 | m_paircache->~btOverlappingPairCache(); |
|---|
| 156 | btAlignedFree(m_paircache); |
|---|
| 157 | } |
|---|
| 158 | } |
|---|
| 159 | |
|---|
| 160 | // |
|---|
| 161 | btBroadphaseProxy* 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 | // |
|---|
| 192 | void 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 | |
|---|
| 206 | void 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 | |
|---|
| 213 | void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) |
|---|
| 214 | { |
|---|
| 215 | |
|---|
| 216 | struct BroadphaseRayTester : btDbvt::ICollide |
|---|
| 217 | { |
|---|
| 218 | btBroadphaseRayCallback& m_rayCallback; |
|---|
| 219 | BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) |
|---|
| 220 | :m_rayCallback(orgCallback) |
|---|
| 221 | { |
|---|
| 222 | } |
|---|
| 223 | void Process(const btDbvtNode* leaf) |
|---|
| 224 | { |
|---|
| 225 | btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; |
|---|
| 226 | m_rayCallback.process(proxy); |
|---|
| 227 | } |
|---|
| 228 | }; |
|---|
| 229 | |
|---|
| 230 | BroadphaseRayTester callback(rayCallback); |
|---|
| 231 | |
|---|
| 232 | m_sets[0].rayTestInternal( m_sets[0].m_root, |
|---|
| 233 | rayFrom, |
|---|
| 234 | rayTo, |
|---|
| 235 | rayCallback.m_rayDirectionInverse, |
|---|
| 236 | rayCallback.m_signs, |
|---|
| 237 | rayCallback.m_lambda_max, |
|---|
| 238 | aabbMin, |
|---|
| 239 | aabbMax, |
|---|
| 240 | callback); |
|---|
| 241 | |
|---|
| 242 | m_sets[1].rayTestInternal( m_sets[1].m_root, |
|---|
| 243 | rayFrom, |
|---|
| 244 | rayTo, |
|---|
| 245 | rayCallback.m_rayDirectionInverse, |
|---|
| 246 | rayCallback.m_signs, |
|---|
| 247 | rayCallback.m_lambda_max, |
|---|
| 248 | aabbMin, |
|---|
| 249 | aabbMax, |
|---|
| 250 | callback); |
|---|
| 251 | |
|---|
| 252 | } |
|---|
| 253 | |
|---|
| 254 | // |
|---|
| 255 | void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, |
|---|
| 256 | const btVector3& aabbMin, |
|---|
| 257 | const btVector3& aabbMax, |
|---|
| 258 | btDispatcher* /*dispatcher*/) |
|---|
| 259 | { |
|---|
| 260 | btDbvtProxy* proxy=(btDbvtProxy*)absproxy; |
|---|
| 261 | ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); |
|---|
| 262 | #if DBVT_BP_PREVENTFALSEUPDATE |
|---|
| 263 | if(NotEqual(aabb,proxy->leaf->volume)) |
|---|
| 264 | #endif |
|---|
| 265 | { |
|---|
| 266 | bool docollide=false; |
|---|
| 267 | if(proxy->stage==STAGECOUNT) |
|---|
| 268 | {/* fixed -> dynamic set */ |
|---|
| 269 | m_sets[1].remove(proxy->leaf); |
|---|
| 270 | proxy->leaf=m_sets[0].insert(aabb,proxy); |
|---|
| 271 | docollide=true; |
|---|
| 272 | } |
|---|
| 273 | else |
|---|
| 274 | {/* dynamic set */ |
|---|
| 275 | ++m_updates_call; |
|---|
| 276 | if(Intersect(proxy->leaf->volume,aabb)) |
|---|
| 277 | {/* Moving */ |
|---|
| 278 | |
|---|
| 279 | const btVector3 delta=aabbMin-proxy->m_aabbMin; |
|---|
| 280 | btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); |
|---|
| 281 | if(delta[0]<0) velocity[0]=-velocity[0]; |
|---|
| 282 | if(delta[1]<0) velocity[1]=-velocity[1]; |
|---|
| 283 | if(delta[2]<0) velocity[2]=-velocity[2]; |
|---|
| 284 | if ( |
|---|
| 285 | #ifdef DBVT_BP_MARGIN |
|---|
| 286 | m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) |
|---|
| 287 | #else |
|---|
| 288 | m_sets[0].update(proxy->leaf,aabb,velocity) |
|---|
| 289 | #endif |
|---|
| 290 | ) |
|---|
| 291 | { |
|---|
| 292 | ++m_updates_done; |
|---|
| 293 | docollide=true; |
|---|
| 294 | } |
|---|
| 295 | } |
|---|
| 296 | else |
|---|
| 297 | {/* Teleporting */ |
|---|
| 298 | m_sets[0].update(proxy->leaf,aabb); |
|---|
| 299 | ++m_updates_done; |
|---|
| 300 | docollide=true; |
|---|
| 301 | } |
|---|
| 302 | } |
|---|
| 303 | listremove(proxy,m_stageRoots[proxy->stage]); |
|---|
| 304 | proxy->m_aabbMin = aabbMin; |
|---|
| 305 | proxy->m_aabbMax = aabbMax; |
|---|
| 306 | proxy->stage = m_stageCurrent; |
|---|
| 307 | listappend(proxy,m_stageRoots[m_stageCurrent]); |
|---|
| 308 | if(docollide) |
|---|
| 309 | { |
|---|
| 310 | m_needcleanup=true; |
|---|
| 311 | if(!m_deferedcollide) |
|---|
| 312 | { |
|---|
| 313 | btDbvtTreeCollider collider(this); |
|---|
| 314 | m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); |
|---|
| 315 | m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); |
|---|
| 316 | } |
|---|
| 317 | } |
|---|
| 318 | } |
|---|
| 319 | } |
|---|
| 320 | |
|---|
| 321 | // |
|---|
| 322 | void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) |
|---|
| 323 | { |
|---|
| 324 | collide(dispatcher); |
|---|
| 325 | #if DBVT_BP_PROFILE |
|---|
| 326 | if(0==(m_pid%DBVT_BP_PROFILING_RATE)) |
|---|
| 327 | { |
|---|
| 328 | printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); |
|---|
| 329 | unsigned int total=m_profiling.m_total; |
|---|
| 330 | if(total<=0) total=1; |
|---|
| 331 | printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); |
|---|
| 332 | printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); |
|---|
| 333 | printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); |
|---|
| 334 | printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); |
|---|
| 335 | const unsigned long sum=m_profiling.m_ddcollide+ |
|---|
| 336 | m_profiling.m_fdcollide+ |
|---|
| 337 | m_profiling.m_cleanup; |
|---|
| 338 | printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); |
|---|
| 339 | 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)); |
|---|
| 340 | clear(m_profiling); |
|---|
| 341 | m_clock.reset(); |
|---|
| 342 | } |
|---|
| 343 | #endif |
|---|
| 344 | } |
|---|
| 345 | |
|---|
| 346 | // |
|---|
| 347 | void btDbvtBroadphase::collide(btDispatcher* dispatcher) |
|---|
| 348 | { |
|---|
| 349 | SPC(m_profiling.m_total); |
|---|
| 350 | /* optimize */ |
|---|
| 351 | m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); |
|---|
| 352 | if(m_fixedleft) |
|---|
| 353 | { |
|---|
| 354 | const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; |
|---|
| 355 | m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); |
|---|
| 356 | m_fixedleft=btMax<int>(0,m_fixedleft-count); |
|---|
| 357 | } |
|---|
| 358 | /* dynamic -> fixed set */ |
|---|
| 359 | m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; |
|---|
| 360 | btDbvtProxy* current=m_stageRoots[m_stageCurrent]; |
|---|
| 361 | if(current) |
|---|
| 362 | { |
|---|
| 363 | btDbvtTreeCollider collider(this); |
|---|
| 364 | do { |
|---|
| 365 | btDbvtProxy* next=current->links[1]; |
|---|
| 366 | listremove(current,m_stageRoots[current->stage]); |
|---|
| 367 | listappend(current,m_stageRoots[STAGECOUNT]); |
|---|
| 368 | #if DBVT_BP_ACCURATESLEEPING |
|---|
| 369 | m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); |
|---|
| 370 | collider.proxy=current; |
|---|
| 371 | btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); |
|---|
| 372 | btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); |
|---|
| 373 | #endif |
|---|
| 374 | m_sets[0].remove(current->leaf); |
|---|
| 375 | ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); |
|---|
| 376 | current->leaf = m_sets[1].insert(curAabb,current); |
|---|
| 377 | current->stage = STAGECOUNT; |
|---|
| 378 | current = next; |
|---|
| 379 | } while(current); |
|---|
| 380 | m_fixedleft=m_sets[1].m_leaves; |
|---|
| 381 | m_needcleanup=true; |
|---|
| 382 | } |
|---|
| 383 | /* collide dynamics */ |
|---|
| 384 | { |
|---|
| 385 | btDbvtTreeCollider collider(this); |
|---|
| 386 | if(m_deferedcollide) |
|---|
| 387 | { |
|---|
| 388 | SPC(m_profiling.m_fdcollide); |
|---|
| 389 | m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); |
|---|
| 390 | } |
|---|
| 391 | if(m_deferedcollide) |
|---|
| 392 | { |
|---|
| 393 | SPC(m_profiling.m_ddcollide); |
|---|
| 394 | m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); |
|---|
| 395 | } |
|---|
| 396 | } |
|---|
| 397 | /* clean up */ |
|---|
| 398 | if(m_needcleanup) |
|---|
| 399 | { |
|---|
| 400 | SPC(m_profiling.m_cleanup); |
|---|
| 401 | btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); |
|---|
| 402 | if(pairs.size()>0) |
|---|
| 403 | { |
|---|
| 404 | const int ci=pairs.size(); |
|---|
| 405 | int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100)); |
|---|
| 406 | for(int i=0;i<ni;++i) |
|---|
| 407 | { |
|---|
| 408 | btBroadphasePair& p=pairs[(m_cid+i)%ci]; |
|---|
| 409 | btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; |
|---|
| 410 | btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; |
|---|
| 411 | if(!Intersect(pa->leaf->volume,pb->leaf->volume)) |
|---|
| 412 | { |
|---|
| 413 | #if DBVT_BP_SORTPAIRS |
|---|
| 414 | if(pa>pb) btSwap(pa,pb); |
|---|
| 415 | #endif |
|---|
| 416 | m_paircache->removeOverlappingPair(pa,pb,dispatcher); |
|---|
| 417 | --ni;--i; |
|---|
| 418 | } |
|---|
| 419 | } |
|---|
| 420 | if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; |
|---|
| 421 | } |
|---|
| 422 | } |
|---|
| 423 | ++m_pid; |
|---|
| 424 | m_newpairs=1; |
|---|
| 425 | m_needcleanup=false; |
|---|
| 426 | if(m_updates_call>0) |
|---|
| 427 | { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } |
|---|
| 428 | else |
|---|
| 429 | { m_updates_ratio=0; } |
|---|
| 430 | m_updates_done/=2; |
|---|
| 431 | m_updates_call/=2; |
|---|
| 432 | } |
|---|
| 433 | |
|---|
| 434 | // |
|---|
| 435 | void btDbvtBroadphase::optimize() |
|---|
| 436 | { |
|---|
| 437 | m_sets[0].optimizeTopDown(); |
|---|
| 438 | m_sets[1].optimizeTopDown(); |
|---|
| 439 | } |
|---|
| 440 | |
|---|
| 441 | // |
|---|
| 442 | btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() |
|---|
| 443 | { |
|---|
| 444 | return(m_paircache); |
|---|
| 445 | } |
|---|
| 446 | |
|---|
| 447 | // |
|---|
| 448 | const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const |
|---|
| 449 | { |
|---|
| 450 | return(m_paircache); |
|---|
| 451 | } |
|---|
| 452 | |
|---|
| 453 | // |
|---|
| 454 | void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const |
|---|
| 455 | { |
|---|
| 456 | |
|---|
| 457 | ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; |
|---|
| 458 | |
|---|
| 459 | if(!m_sets[0].empty()) |
|---|
| 460 | if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, |
|---|
| 461 | m_sets[1].m_root->volume,bounds); |
|---|
| 462 | else |
|---|
| 463 | bounds=m_sets[0].m_root->volume; |
|---|
| 464 | else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; |
|---|
| 465 | else |
|---|
| 466 | bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); |
|---|
| 467 | aabbMin=bounds.Mins(); |
|---|
| 468 | aabbMax=bounds.Maxs(); |
|---|
| 469 | } |
|---|
| 470 | |
|---|
| 471 | // |
|---|
| 472 | void btDbvtBroadphase::printStats() |
|---|
| 473 | {} |
|---|
| 474 | |
|---|
| 475 | // |
|---|
| 476 | #if DBVT_BP_ENABLE_BENCHMARK |
|---|
| 477 | |
|---|
| 478 | struct btBroadphaseBenchmark |
|---|
| 479 | { |
|---|
| 480 | struct Experiment |
|---|
| 481 | { |
|---|
| 482 | const char* name; |
|---|
| 483 | int object_count; |
|---|
| 484 | int update_count; |
|---|
| 485 | int spawn_count; |
|---|
| 486 | int iterations; |
|---|
| 487 | btScalar speed; |
|---|
| 488 | btScalar amplitude; |
|---|
| 489 | }; |
|---|
| 490 | struct Object |
|---|
| 491 | { |
|---|
| 492 | btVector3 center; |
|---|
| 493 | btVector3 extents; |
|---|
| 494 | btBroadphaseProxy* proxy; |
|---|
| 495 | btScalar time; |
|---|
| 496 | void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) |
|---|
| 497 | { |
|---|
| 498 | time += speed; |
|---|
| 499 | center[0] = btCos(time*(btScalar)2.17)*amplitude+ |
|---|
| 500 | btSin(time)*amplitude/2; |
|---|
| 501 | center[1] = btCos(time*(btScalar)1.38)*amplitude+ |
|---|
| 502 | btSin(time)*amplitude; |
|---|
| 503 | center[2] = btSin(time*(btScalar)0.777)*amplitude; |
|---|
| 504 | pbi->setAabb(proxy,center-extents,center+extents,0); |
|---|
| 505 | } |
|---|
| 506 | }; |
|---|
| 507 | static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } |
|---|
| 508 | static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } |
|---|
| 509 | static void OutputTime(const char* name,btClock& c,unsigned count=0) |
|---|
| 510 | { |
|---|
| 511 | const unsigned long us=c.getTimeMicroseconds(); |
|---|
| 512 | const unsigned long ms=(us+500)/1000; |
|---|
| 513 | const btScalar sec=us/(btScalar)(1000*1000); |
|---|
| 514 | if(count>0) |
|---|
| 515 | printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); |
|---|
| 516 | else |
|---|
| 517 | printf("%s : %u us (%u ms)\r\n",name,us,ms); |
|---|
| 518 | } |
|---|
| 519 | }; |
|---|
| 520 | |
|---|
| 521 | void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) |
|---|
| 522 | { |
|---|
| 523 | static const btBroadphaseBenchmark::Experiment experiments[]= |
|---|
| 524 | { |
|---|
| 525 | {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, |
|---|
| 526 | /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, |
|---|
| 527 | {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ |
|---|
| 528 | }; |
|---|
| 529 | static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); |
|---|
| 530 | btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects; |
|---|
| 531 | btClock wallclock; |
|---|
| 532 | /* Begin */ |
|---|
| 533 | for(int iexp=0;iexp<nexperiments;++iexp) |
|---|
| 534 | { |
|---|
| 535 | const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp]; |
|---|
| 536 | const int object_count=experiment.object_count; |
|---|
| 537 | const int update_count=(object_count*experiment.update_count)/100; |
|---|
| 538 | const int spawn_count=(object_count*experiment.spawn_count)/100; |
|---|
| 539 | const btScalar speed=experiment.speed; |
|---|
| 540 | const btScalar amplitude=experiment.amplitude; |
|---|
| 541 | printf("Experiment #%u '%s':\r\n",iexp,experiment.name); |
|---|
| 542 | printf("\tObjects: %u\r\n",object_count); |
|---|
| 543 | printf("\tUpdate: %u\r\n",update_count); |
|---|
| 544 | printf("\tSpawn: %u\r\n",spawn_count); |
|---|
| 545 | printf("\tSpeed: %f\r\n",speed); |
|---|
| 546 | printf("\tAmplitude: %f\r\n",amplitude); |
|---|
| 547 | srand(180673); |
|---|
| 548 | /* Create objects */ |
|---|
| 549 | wallclock.reset(); |
|---|
| 550 | objects.reserve(object_count); |
|---|
| 551 | for(int i=0;i<object_count;++i) |
|---|
| 552 | { |
|---|
| 553 | btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); |
|---|
| 554 | po->center[0]=btBroadphaseBenchmark::UnitRand()*50; |
|---|
| 555 | po->center[1]=btBroadphaseBenchmark::UnitRand()*50; |
|---|
| 556 | po->center[2]=btBroadphaseBenchmark::UnitRand()*50; |
|---|
| 557 | po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; |
|---|
| 558 | po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; |
|---|
| 559 | po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; |
|---|
| 560 | po->time=btBroadphaseBenchmark::UnitRand()*2000; |
|---|
| 561 | po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); |
|---|
| 562 | objects.push_back(po); |
|---|
| 563 | } |
|---|
| 564 | btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); |
|---|
| 565 | /* First update */ |
|---|
| 566 | wallclock.reset(); |
|---|
| 567 | for(int i=0;i<objects.size();++i) |
|---|
| 568 | { |
|---|
| 569 | objects[i]->update(speed,amplitude,pbi); |
|---|
| 570 | } |
|---|
| 571 | btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); |
|---|
| 572 | /* Updates */ |
|---|
| 573 | wallclock.reset(); |
|---|
| 574 | for(int i=0;i<experiment.iterations;++i) |
|---|
| 575 | { |
|---|
| 576 | for(int j=0;j<update_count;++j) |
|---|
| 577 | { |
|---|
| 578 | objects[j]->update(speed,amplitude,pbi); |
|---|
| 579 | } |
|---|
| 580 | pbi->calculateOverlappingPairs(0); |
|---|
| 581 | } |
|---|
| 582 | btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); |
|---|
| 583 | /* Clean up */ |
|---|
| 584 | wallclock.reset(); |
|---|
| 585 | for(int i=0;i<objects.size();++i) |
|---|
| 586 | { |
|---|
| 587 | pbi->destroyProxy(objects[i]->proxy,0); |
|---|
| 588 | delete objects[i]; |
|---|
| 589 | } |
|---|
| 590 | objects.resize(0); |
|---|
| 591 | btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); |
|---|
| 592 | } |
|---|
| 593 | |
|---|
| 594 | } |
|---|
| 595 | #else |
|---|
| 596 | void btDbvtBroadphase::benchmark(btBroadphaseInterface*) |
|---|
| 597 | {} |
|---|
| 598 | #endif |
|---|
| 599 | |
|---|
| 600 | #if DBVT_BP_PROFILE |
|---|
| 601 | #undef SPC |
|---|
| 602 | #endif |
|---|