1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 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
16 #include "btMultiSapBroadphase.h"
17
18 #include "btSimpleBroadphase.h"
19 #include "LinearMath/btAabbUtil2.h"
20 #include "btQuantizedBvh.h"
21
22 /// btSapBroadphaseArray m_sapBroadphases;
23
24 /// btOverlappingPairCache* m_overlappingPairs;
25 extern int gOverlappingPairs;
26
27 /*
28 class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
29 {
30 public:
31
32 virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
33 {
34 return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
35 }
36 };
37
38 */
39
btMultiSapBroadphase(int,btOverlappingPairCache * pairCache)40 btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
41 :m_overlappingPairs(pairCache),
42 m_optimizedAabbTree(0),
43 m_ownsPairCache(false),
44 m_invalidPair(0)
45 {
46 if (!m_overlappingPairs)
47 {
48 m_ownsPairCache = true;
49 void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
50 m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
51 }
52
53 struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
54 {
55 virtual ~btMultiSapOverlapFilterCallback()
56 {}
57 // return true when pairs need collision
58 virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
59 {
60 btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
61 btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
62
63 bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
64 collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
65
66 return collides;
67 }
68 };
69
70 void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
71 m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
72
73 m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
74 // mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
75 // m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
76 }
77
~btMultiSapBroadphase()78 btMultiSapBroadphase::~btMultiSapBroadphase()
79 {
80 if (m_ownsPairCache)
81 {
82 m_overlappingPairs->~btOverlappingPairCache();
83 btAlignedFree(m_overlappingPairs);
84 }
85 }
86
87
buildTree(const btVector3 & bvhAabbMin,const btVector3 & bvhAabbMax)88 void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
89 {
90 m_optimizedAabbTree = new btQuantizedBvh();
91 m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
92 QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray();
93 for (int i=0;i<m_sapBroadphases.size();i++)
94 {
95 btQuantizedBvhNode node;
96 btVector3 aabbMin,aabbMax;
97 m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
98 m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
99 m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
100 int partId = 0;
101 node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
102 nodes.push_back(node);
103 }
104 m_optimizedAabbTree->buildInternal();
105 }
106
createProxy(const btVector3 & aabbMin,const btVector3 & aabbMax,int shapeType,void * userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher * dispatcher,void *)107 btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
108 {
109 //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
110
111 void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
112 btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
113 m_multiSapProxies.push_back(proxy);
114
115 ///this should deal with inserting/removal into child broadphases
116 setAabb(proxy,aabbMin,aabbMax,dispatcher);
117 return proxy;
118 }
119
destroyProxy(btBroadphaseProxy *,btDispatcher *)120 void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
121 {
122 ///not yet
123 btAssert(0);
124
125 }
126
127
addToChildBroadphase(btMultiSapProxy * parentMultiSapProxy,btBroadphaseProxy * childProxy,btBroadphaseInterface * childBroadphase)128 void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase)
129 {
130 void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
131 btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
132 bridgeProxyRef->m_childProxy = childProxy;
133 bridgeProxyRef->m_childBroadphase = childBroadphase;
134 parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
135 }
136
137
138 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
boxIsContainedWithinBox(const btVector3 & amin,const btVector3 & amax,const btVector3 & bmin,const btVector3 & bmax)139 bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
140 {
141 return
142 amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
143 amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
144 amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
145 }
146
147
148
149
150
151
getAabb(btBroadphaseProxy * proxy,btVector3 & aabbMin,btVector3 & aabbMax) const152 void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
153 {
154 btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
155 aabbMin = multiProxy->m_aabbMin;
156 aabbMax = multiProxy->m_aabbMax;
157 }
158
rayTest(const btVector3 & rayFrom,const btVector3 & rayTo,btBroadphaseRayCallback & rayCallback,const btVector3 & aabbMin,const btVector3 & aabbMax)159 void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
160 {
161 for (int i=0;i<m_multiSapProxies.size();i++)
162 {
163 rayCallback.process(m_multiSapProxies[i]);
164 }
165 }
166
167
168 //#include <stdio.h>
169
setAabb(btBroadphaseProxy * proxy,const btVector3 & aabbMin,const btVector3 & aabbMax,btDispatcher * dispatcher)170 void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
171 {
172 btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
173 multiProxy->m_aabbMin = aabbMin;
174 multiProxy->m_aabbMax = aabbMax;
175
176
177 // bool fullyContained = false;
178 // bool alreadyInSimple = false;
179
180
181
182
183 struct MyNodeOverlapCallback : public btNodeOverlapCallback
184 {
185 btMultiSapBroadphase* m_multiSap;
186 btMultiSapProxy* m_multiProxy;
187 btDispatcher* m_dispatcher;
188
189 MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
190 :m_multiSap(multiSap),
191 m_multiProxy(multiProxy),
192 m_dispatcher(dispatcher)
193 {
194
195 }
196
197 virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
198 {
199 btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
200
201 int containingBroadphaseIndex = -1;
202 //already found?
203 for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
204 {
205
206 if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
207 {
208 containingBroadphaseIndex = i;
209 break;
210 }
211 }
212 if (containingBroadphaseIndex<0)
213 {
214 //add it
215 btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
216 m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
217
218 }
219 }
220 };
221
222 MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher);
223
224
225
226
227 if (m_optimizedAabbTree)
228 m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
229
230 int i;
231
232 for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
233 {
234 btVector3 worldAabbMin,worldAabbMax;
235 multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
236 bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
237 if (!overlapsBroadphase)
238 {
239 //remove it now
240 btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
241
242 btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
243 bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
244
245 multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
246 multiProxy->m_bridgeProxies.pop_back();
247
248 }
249 }
250
251
252 /*
253
254 if (1)
255 {
256
257 //find broadphase that contain this multiProxy
258 int numChildBroadphases = getBroadphaseArray().size();
259 for (int i=0;i<numChildBroadphases;i++)
260 {
261 btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
262 btVector3 worldAabbMin,worldAabbMax;
263 childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
264 bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
265
266 // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
267 int containingBroadphaseIndex = -1;
268
269 //if already contains this
270
271 for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
272 {
273 if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
274 {
275 containingBroadphaseIndex = i;
276 }
277 alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
278 }
279
280 if (overlapsBroadphase)
281 {
282 if (containingBroadphaseIndex<0)
283 {
284 btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
285 childProxy->m_multiSapParentProxy = multiProxy;
286 addToChildBroadphase(multiProxy,childProxy,childBroadphase);
287 }
288 } else
289 {
290 if (containingBroadphaseIndex>=0)
291 {
292 //remove
293 btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
294
295 btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
296 bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
297
298 multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
299 multiProxy->m_bridgeProxies.pop_back();
300 }
301 }
302 }
303
304
305 ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
306 ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
307 if (0)//!multiProxy->m_bridgeProxies.size())
308 {
309 ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
310 ///this is needed to be able to calculate the aabb overlap
311 btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
312 childProxy->m_multiSapParentProxy = multiProxy;
313 addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
314 }
315 }
316
317 if (!multiProxy->m_bridgeProxies.size())
318 {
319 ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
320 ///this is needed to be able to calculate the aabb overlap
321 btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
322 childProxy->m_multiSapParentProxy = multiProxy;
323 addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
324 }
325 */
326
327
328 //update
329 for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
330 {
331 btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
332 bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
333 }
334
335 }
336 bool stopUpdating=false;
337
338
339
340 class btMultiSapBroadphasePairSortPredicate
341 {
342 public:
343
operator ()(const btBroadphasePair & a1,const btBroadphasePair & b1) const344 bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const
345 {
346 btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
347 btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
348 btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
349 btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
350
351 return aProxy0 > bProxy0 ||
352 (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
353 (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm);
354 }
355 };
356
357
358 ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
calculateOverlappingPairs(btDispatcher * dispatcher)359 void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
360 {
361
362 // m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
363
364 if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
365 {
366
367 btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
368
369 // quicksort(overlappingPairArray,0,overlappingPairArray.size());
370
371 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
372
373 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
374 // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
375
376 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
377 m_invalidPair = 0;
378
379
380 int i;
381
382 btBroadphasePair previousPair;
383 previousPair.m_pProxy0 = 0;
384 previousPair.m_pProxy1 = 0;
385 previousPair.m_algorithm = 0;
386
387
388 for (i=0;i<overlappingPairArray.size();i++)
389 {
390
391 btBroadphasePair& pair = overlappingPairArray[i];
392
393 btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
394 btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
395 btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
396 btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
397
398 bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
399
400 previousPair = pair;
401
402 bool needsRemoval = false;
403
404 if (!isDuplicate)
405 {
406 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
407
408 if (hasOverlap)
409 {
410 needsRemoval = false;//callback->processOverlap(pair);
411 } else
412 {
413 needsRemoval = true;
414 }
415 } else
416 {
417 //remove duplicate
418 needsRemoval = true;
419 //should have no algorithm
420 btAssert(!pair.m_algorithm);
421 }
422
423 if (needsRemoval)
424 {
425 getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
426
427 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
428 // m_overlappingPairArray.pop_back();
429 pair.m_pProxy0 = 0;
430 pair.m_pProxy1 = 0;
431 m_invalidPair++;
432 gOverlappingPairs--;
433 }
434
435 }
436
437 ///if you don't like to skip the invalid pairs in the array, execute following code:
438 #define CLEAN_INVALID_PAIRS 1
439 #ifdef CLEAN_INVALID_PAIRS
440
441 //perform a sort, to sort 'invalid' pairs to the end
442 //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
443 overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
444
445 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
446 m_invalidPair = 0;
447 #endif//CLEAN_INVALID_PAIRS
448
449 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
450 }
451
452
453 }
454
455
testAabbOverlap(btBroadphaseProxy * childProxy0,btBroadphaseProxy * childProxy1)456 bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
457 {
458 btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
459 btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
460
461 return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
462 multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
463
464 }
465
466
printStats()467 void btMultiSapBroadphase::printStats()
468 {
469 /* printf("---------------------------------\n");
470
471 printf("btMultiSapBroadphase.h\n");
472 printf("numHandles = %d\n",m_multiSapProxies.size());
473 //find broadphase that contain this multiProxy
474 int numChildBroadphases = getBroadphaseArray().size();
475 for (int i=0;i<numChildBroadphases;i++)
476 {
477
478 btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
479 childBroadphase->printStats();
480
481 }
482 */
483
484 }
485
resetPool(btDispatcher * dispatcher)486 void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
487 {
488 // not yet
489 }
490