Bullet Collision Detection & Physics Library
btCompoundCompoundCollisionAlgorithm.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2013 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
26
27//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
28#define USE_LOCAL_STACK 1
29
31
33:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
34{
35
36 void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
38
39 const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
40 btAssert (col0ObjWrap->getCollisionShape()->isCompound());
41
42 const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
43 btAssert (col1ObjWrap->getCollisionShape()->isCompound());
44
45 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
47
48 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
50
51
52}
53
54
56{
60}
61
63{
64 int i;
66 for (i=0;i<pairs.size();i++)
67 {
68 if (pairs[i].m_userPointer)
69 {
70
71 ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
72 }
73 }
74}
75
76
78{
80
81 int numChildren = pairs.size();
82 int i;
83 for (i=0;i<numChildren;i++)
84 {
85 if (pairs[i].m_userPointer)
86 {
87 btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
90 }
91 }
93}
94
96{
98
99
105
106
108
110
112 const btCollisionObjectWrapper* compound0ObjWrap,
113 btDispatcher* dispatcher,
114 const btDispatcherInfo& dispatchInfo,
115 btManifoldResult* resultOut,
116 btHashedSimplePairCache* childAlgorithmsCache,
117 btPersistentManifold* sharedManifold)
118 :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
119 m_childCollisionAlgorithmCache(childAlgorithmsCache),
120 m_sharedManifold(sharedManifold)
121 {
122
123 }
124
125
126
127
128 void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
129 {
130 BT_PROFILE("btCompoundCompoundLeafCallback::Process");
132
133
134 int childIndex0 = leaf0->dataAsInt;
135 int childIndex1 = leaf1->dataAsInt;
136
137
138 btAssert(childIndex0>=0);
139 btAssert(childIndex1>=0);
140
141
142 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
143 btAssert(childIndex0<compoundShape0->getNumChildShapes());
144
145 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
146 btAssert(childIndex1<compoundShape1->getNumChildShapes());
147
148 const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
149 const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
150
151 //backup
153 const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
154 btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
155
157 const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
158 btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ;
159
160
161 //perform an AABB check first
162 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
163 childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
164 childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
165
167
168 aabbMin0 -= thresholdVec;
169 aabbMax0 += thresholdVec;
170
172 {
173 if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
174 return;
175 }
176
177 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
178 {
179 btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
180 btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
181
182
183 btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
184
185 btCollisionAlgorithm* colAlgo = 0;
187 {
188 colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
189 }
190 else
191 {
192 if (pair)
193 {
194 colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
195
196 }
197 else
198 {
199 colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
200 pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
201 btAssert(pair);
202 pair->m_userPointer = colAlgo;
203 }
204 }
205
206 btAssert(colAlgo);
207
208 const btCollisionObjectWrapper* tmpWrap0 = 0;
209 const btCollisionObjectWrapper* tmpWrap1 = 0;
210
211 tmpWrap0 = m_resultOut->getBody0Wrap();
212 tmpWrap1 = m_resultOut->getBody1Wrap();
213
214 m_resultOut->setBody0Wrap(&compoundWrap0);
215 m_resultOut->setBody1Wrap(&compoundWrap1);
216
217 m_resultOut->setShapeIdentifiersA(-1,childIndex0);
218 m_resultOut->setShapeIdentifiersB(-1,childIndex1);
219
220
221 colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
222
223 m_resultOut->setBody0Wrap(tmpWrap0);
224 m_resultOut->setBody1Wrap(tmpWrap1);
225
226
227
228 }
229 }
230};
231
232
234 const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
235{
236 btVector3 newmin,newmax;
237 btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
238 newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
239 newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
240 btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
241 return Intersect(a,newb);
242}
243
244
245static inline void MycollideTT( const btDbvtNode* root0,
246 const btDbvtNode* root1,
247 const btTransform& xform,
248 btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
249{
250
251 if(root0&&root1)
252 {
253 int depth=1;
254 int treshold=btDbvt::DOUBLE_STACKSIZE-4;
256#ifdef USE_LOCAL_STACK
259#else
261#endif
262 stkStack[0]=btDbvt::sStkNN(root0,root1);
263 do {
264 btDbvt::sStkNN p=stkStack[--depth];
265 if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
266 {
267 if(depth>treshold)
268 {
269 stkStack.resize(stkStack.size()*2);
270 treshold=stkStack.size()-4;
271 }
272 if(p.a->isinternal())
273 {
274 if(p.b->isinternal())
275 {
276 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
277 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
278 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
279 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
280 }
281 else
282 {
283 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
284 stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
285 }
286 }
287 else
288 {
289 if(p.b->isinternal())
290 {
291 stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
292 stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
293 }
294 else
295 {
296 callback->Process(p.a,p.b);
297 }
298 }
299 }
300 } while(depth);
301 }
302}
303
305{
306
307 const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
308 const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
309
310 btAssert (col0ObjWrap->getCollisionShape()->isCompound());
311 btAssert (col1ObjWrap->getCollisionShape()->isCompound());
312 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
313 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
314
315 const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
316 const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
317 if (!tree0 || !tree1)
318 {
319 return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
320 }
323 if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
324 {
329
330 }
331
332
336 {
337 int i;
339#ifdef USE_LOCAL_STACK
340 btPersistentManifold localManifolds[4];
341 manifoldArray.initializeFromBuffer(&localManifolds,0,4);
342#endif
344 for (i=0;i<pairs.size();i++)
345 {
346 if (pairs[i].m_userPointer)
347 {
348 btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
350 for (int m=0;m<manifoldArray.size();m++)
351 {
352 if (manifoldArray[m]->getNumContacts())
353 {
355 resultOut->refreshContactPoints();
356 resultOut->setPersistentManifold(0);
357 }
358 }
360 }
361 }
362 }
363
364
365
366
367 btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
368
369
370 const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
371 MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
372
373 //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
374
375 //remove non-overlapping child pairs
376
377 {
379
380 //iterate over all children, perform an AABB check inside ProcessChildShape
382
383 int i;
385
386
387
388
389
390 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
391
392 for (i=0;i<pairs.size();i++)
393 {
394 if (pairs[i].m_userPointer)
395 {
396 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
397
398 {
399 btTransform orgTrans0;
400 const btCollisionShape* childShape0 = 0;
401
402 btTransform newChildWorldTrans0;
403 btTransform orgInterpolationTrans0;
404 childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
405 orgTrans0 = col0ObjWrap->getWorldTransform();
406 orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
407 const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
408 newChildWorldTrans0 = orgTrans0*childTrans0 ;
409 childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
410 }
412 aabbMin0 -= thresholdVec;
413 aabbMax0 += thresholdVec;
414 {
415 btTransform orgInterpolationTrans1;
416 const btCollisionShape* childShape1 = 0;
417 btTransform orgTrans1;
418 btTransform newChildWorldTrans1;
419
420 childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
421 orgTrans1 = col1ObjWrap->getWorldTransform();
422 orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
423 const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
424 newChildWorldTrans1 = orgTrans1*childTrans1 ;
425 childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
426 }
427
428 aabbMin1 -= thresholdVec;
429 aabbMax1 += thresholdVec;
430
431 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
432 {
433 algo->~btCollisionAlgorithm();
435 m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
436 }
437 }
438 }
439 for (int i=0;i<m_removePairs.size();i++)
440 {
442 }
444 }
445
446}
447
449{
450 btAssert(0);
451 return 0.f;
452
453}
454
455
456
bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition: btAabbUtil2.h:48
void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
Definition: btAabbUtil2.h:182
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
bool(* btShapePairCallback)(const btCollisionShape *pShape0, const btCollisionShape *pShape1)
btShapePairCallback gCompoundCompoundChildShapePairCallback
static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b, const btTransform &xform, btScalar distanceThreshold)
static void MycollideTT(const btDbvtNode *root0, const btDbvtNode *root1, const btTransform &xform, btCompoundCompoundLeafCallback *callback, btScalar distanceThreshold)
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:534
#define DBVT_INLINE
Definition: btDbvt.h:55
@ BT_CLOSEST_POINT_ALGORITHMS
Definition: btDispatcher.h:70
@ BT_CONTACT_POINT_ALGORITHMS
Definition: btDispatcher.h:69
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
#define btAssert(x)
Definition: btScalar.h:131
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void push_back(const T &_Val)
void initializeFromBuffer(void *buffer, int size, int capacity)
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
btCollisionObject can be used to manage collision detection objects.
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
bool isCompound() const
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision s...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
class btPersistentManifold * m_sharedManifold
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
The btCompoundShape allows to store multiple other btCollisionShapes This allows for moving concave c...
btCollisionShape * getChildShape(int index)
btTransform & getChildTransform(int index)
int getUpdateRevision() const
const btDbvt * getDynamicAabbTree() const
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
btSimplePairArray & getOverlappingPairArray()
btSimplePair * findPair(int indexA, int indexB)
virtual btSimplePair * addOverlappingPair(int indexA, int indexB)
virtual void * removeOverlappingPair(int indexA, int indexB)
btManifoldResult is a helper class to manage contact results.
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
void setBody0Wrap(const btCollisionObjectWrapper *obj0Wrap)
const btCollisionObjectWrapper * getBody1Wrap() const
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
virtual void setShapeIdentifiersB(int partId1, int index1)
btScalar m_closestPointDistanceThreshold
const btCollisionObjectWrapper * getBody0Wrap() const
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
void Process(const btDbvtNode *leaf0, const btDbvtNode *leaf1)
const btCollisionObjectWrapper * m_compound0ColObjWrap
btCompoundCompoundLeafCallback(const btCollisionObjectWrapper *compound1ObjWrap, const btCollisionObjectWrapper *compound0ObjWrap, btDispatcher *dispatcher, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut, btHashedSimplePairCache *childAlgorithmsCache, btPersistentManifold *sharedManifold)
const btCollisionObjectWrapper * m_compound1ColObjWrap
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:425
DBVT_INLINE const btVector3 & Mins() const
Definition: btDbvt.h:136
DBVT_INLINE const btVector3 & Maxs() const
Definition: btDbvt.h:137
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:183
btDbvtNode * childs[2]
Definition: btDbvt.h:186
btDbvtVolume volume
Definition: btDbvt.h:180
int dataAsInt
Definition: btDbvt.h:188
const btDbvtNode * b
Definition: btDbvt.h:204
const btDbvtNode * a
Definition: btDbvt.h:203
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:199
@ DOUBLE_STACKSIZE
Definition: btDbvt.h:258
btDbvtNode * m_root
Definition: btDbvt.h:262