Bullet Collision Detection & Physics Library
btCompoundCompoundCollisionAlgorithm.cpp
Go to the documentation of this file.
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 
18 #include "LinearMath/btQuickprof.h"
23 #include "LinearMath/btAabbUtil2.h"
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());
46  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
47 
48  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
49  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
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;
88  algo->~btCollisionAlgorithm();
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");
131  m_numOverlapPairs++;
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
152  btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
153  const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
154  btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
155 
156  btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
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;
186  if (m_resultOut->m_closestPointDistanceThreshold > 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 
233 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
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 
245 static 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
258  stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE);
259 #else
260  stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
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  {
327  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
328  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
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;
349  algo->getAllContactManifolds(manifoldArray);
350  for (int m=0;m<manifoldArray.size();m++)
351  {
352  if (manifoldArray[m]->getNumContacts())
353  {
354  resultOut->setPersistentManifold(manifoldArray[m]);
355  resultOut->refreshContactPoints();
356  resultOut->setPersistentManifold(0);
357  }
358  }
359  manifoldArray.resize(0);
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 
void Process(const btDbvtNode *leaf0, const btDbvtNode *leaf1)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void push_back(const T &_Val)
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
DBVT_INLINE const btVector3 & Mins() const
Definition: btDbvt.h:136
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
virtual btSimplePair * addOverlappingPair(int indexA, int indexB)
const btCollisionObjectWrapper * getBody0Wrap() const
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
class btPersistentManifold * m_sharedManifold
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
const btCollisionObjectWrapper * m_compound0ColObjWrap
const btDbvt * getDynamicAabbTree() const
#define btAssert(x)
Definition: btScalar.h:131
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:198
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
const btDbvtNode * b
Definition: btDbvt.h:204
btDbvtNode * m_root
Definition: btDbvt.h:262
btManifoldResult is a helper class to manage contact results.
int getUpdateRevision() const
const btCollisionShape * getCollisionShape() const
btCompoundCompoundLeafCallback(const btCollisionObjectWrapper *compound1ObjWrap, const btCollisionObjectWrapper *compound0ObjWrap, btDispatcher *dispatcher, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut, btHashedSimplePairCache *childAlgorithmsCache, btPersistentManifold *sharedManifold)
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
btSimplePairArray & getOverlappingPairArray()
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...
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
DBVT_INLINE const btVector3 & Maxs() const
Definition: btDbvt.h:137
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
bool isCompound() const
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar m_closestPointDistanceThreshold
btDbvtNode * childs[2]
Definition: btDbvt.h:186
btTransform & getChildTransform(int index)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:425
const btDbvtNode * a
Definition: btDbvt.h:203
const btCollisionObject * getCollisionObject() const
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
#define btAlignedFree(ptr)
btCollisionObject can be used to manage collision detection objects.
void initializeFromBuffer(void *buffer, int size, int capacity)
bool(* btShapePairCallback)(const btCollisionShape *pShape0, const btCollisionShape *pShape1)
static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b, const btTransform &xform, btScalar distanceThreshold)
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual void setShapeIdentifiersB(int partId1, int index1)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
int size() const
return the number of elements in the array
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
static void MycollideTT(const btDbvtNode *root0, const btDbvtNode *root1, const btTransform &xform, btCompoundCompoundLeafCallback *callback, btScalar distanceThreshold)
void resize(int newsize, const T &fillData=T())
btDbvtVolume volume
Definition: btDbvt.h:180
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:183
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
const btTransform & getWorldTransform() const
btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision s...
btSimplePair * findPair(int indexA, int indexB)
const btCollisionObjectWrapper * m_compound1ColObjWrap
int dataAsInt
Definition: btDbvt.h:188
#define DBVT_INLINE
Definition: btDbvt.h:55
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:534
#define btAlignedAlloc(size, alignment)
const btCollisionObjectWrapper * getBody1Wrap() const
void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
Definition: btAabbUtil2.h:182
The btCompoundShape allows to store multiple other btCollisionShapes This allows for moving concave c...
void setBody0Wrap(const btCollisionObjectWrapper *obj0Wrap)
btCollisionShape * getChildShape(int index)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
virtual void * removeOverlappingPair(int indexA, int indexB)
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btShapePairCallback gCompoundCompoundChildShapePairCallback