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 {
b3ProfileScopeb3ProfileScope32 __forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
33 {
34 }
~b3ProfileScopeb3ProfileScope35 __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>
b3ListAppend(T * item,T * & list)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>
b3ListRemove(T * item,T * & list)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>
b3ListCount(T * root)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>
b3Clear(T & value)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;
b3DbvtTreeColliderb3DbvtTreeCollider105 b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
Processb3DbvtTreeCollider106 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 }
Processb3DbvtTreeCollider120 void Process(const b3DbvtNode* n)
121 {
122 Process(n, proxy->leaf);
123 }
124 };
125
126 //
127 // b3DynamicBvhBroadphase
128 //
129
130 //
b3DynamicBvhBroadphase(int proxyCapacity,b3OverlappingPairCache * paircache)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 //
~b3DynamicBvhBroadphase()161 b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
162 {
163 if (m_releasepaircache)
164 {
165 m_paircache->~b3OverlappingPairCache();
166 b3AlignedFree(m_paircache);
167 }
168 }
169
170 //
createProxy(const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,int objectId,void * userPtr,int collisionFilterGroup,int collisionFilterMask)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 //
destroyProxy(b3BroadphaseProxy * absproxy,b3Dispatcher * dispatcher)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
getAabb(int objectId,b3Vector3 & aabbMin,b3Vector3 & aabbMax) const215 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;
BroadphaseRayTesterBroadphaseRayTester233 BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
234 : m_rayCallback(orgCallback)
235 {
236 }
ProcessBroadphaseRayTester237 void Process(const b3DbvtNode* leaf)
238 {
239 b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
240 m_rayCallback.process(proxy);
241 }
242 };
243
rayTest(const b3Vector3 & rayFrom,const b3Vector3 & rayTo,b3BroadphaseRayCallback & rayCallback,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax)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;
BroadphaseAabbTesterBroadphaseAabbTester272 BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
273 : m_aabbCallback(orgCallback)
274 {
275 }
ProcessBroadphaseAabbTester276 void Process(const b3DbvtNode* leaf)
277 {
278 b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
279 m_aabbCallback.process(proxy);
280 }
281 };
282
aabbTest(const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,b3BroadphaseAabbCallback & aabbCallback)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 //
setAabb(int objectId,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,b3Dispatcher *)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 //
setAabbForceUpdate(b3BroadphaseProxy * absproxy,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,b3Dispatcher *)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 //
calculateOverlappingPairs(b3Dispatcher * dispatcher)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
performDeferredRemoval(b3Dispatcher * dispatcher)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 //
collide(b3Dispatcher * dispatcher)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 //
optimize()608 void b3DynamicBvhBroadphase::optimize()
609 {
610 m_sets[0].optimizeTopDown();
611 m_sets[1].optimizeTopDown();
612 }
613
614 //
getOverlappingPairCache()615 b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
616 {
617 return (m_paircache);
618 }
619
620 //
getOverlappingPairCache() const621 const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
622 {
623 return (m_paircache);
624 }
625
626 //
getBroadphaseAabb(b3Vector3 & aabbMin,b3Vector3 & aabbMax) const627 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
resetPool(b3Dispatcher * dispatcher)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 //
printStats()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;
updateb3BroadphaseBenchmark::Object702 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 };
UnsignedRandb3BroadphaseBenchmark713 static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
UnitRandb3BroadphaseBenchmark714 static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
OutputTimeb3BroadphaseBenchmark715 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
benchmark(b3BroadphaseInterface * pbi)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
809