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 
29 
31 :btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
32 {
33 
34  void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
36 
37  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
38  btAssert (col0ObjWrap->getCollisionShape()->isCompound());
39 
40  const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
41  btAssert (col1ObjWrap->getCollisionShape()->isCompound());
42 
43  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
44  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
45 
46  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
47  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
48 
49 
50 }
51 
52 
54 {
58 }
59 
61 {
62  int i;
64  for (i=0;i<pairs.size();i++)
65  {
66  if (pairs[i].m_userPointer)
67  {
68 
69  ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
70  }
71  }
72 }
73 
74 
76 {
78 
79  int numChildren = pairs.size();
80  int i;
81  for (i=0;i<numChildren;i++)
82  {
83  if (pairs[i].m_userPointer)
84  {
85  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
86  algo->~btCollisionAlgorithm();
88  }
89  }
91 }
92 
94 {
96 
97 
103 
104 
106 
108 
110  const btCollisionObjectWrapper* compound0ObjWrap,
111  btDispatcher* dispatcher,
112  const btDispatcherInfo& dispatchInfo,
113  btManifoldResult* resultOut,
114  btHashedSimplePairCache* childAlgorithmsCache,
115  btPersistentManifold* sharedManifold)
116  :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
117  m_childCollisionAlgorithmCache(childAlgorithmsCache),
118  m_sharedManifold(sharedManifold)
119  {
120 
121  }
122 
123 
124 
125 
126  void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
127  {
128  BT_PROFILE("btCompoundCompoundLeafCallback::Process");
129  m_numOverlapPairs++;
130 
131 
132  int childIndex0 = leaf0->dataAsInt;
133  int childIndex1 = leaf1->dataAsInt;
134 
135 
136  btAssert(childIndex0>=0);
137  btAssert(childIndex1>=0);
138 
139 
140  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
141  btAssert(childIndex0<compoundShape0->getNumChildShapes());
142 
143  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
144  btAssert(childIndex1<compoundShape1->getNumChildShapes());
145 
146  const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
147  const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
148 
149  //backup
150  btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
151  const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
152  btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
153 
154  btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
155  const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
156  btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ;
157 
158 
159  //perform an AABB check first
160  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
161  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
162  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
163 
165 
166  aabbMin0 -= thresholdVec;
167  aabbMax0 += thresholdVec;
168 
170  {
171  if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
172  return;
173  }
174 
175  if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
176  {
177  btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
178  btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
179 
180 
181  btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
182 
183  btCollisionAlgorithm* colAlgo = 0;
184  if (m_resultOut->m_closestPointDistanceThreshold > 0)
185  {
186  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
187  }
188  else
189  {
190  if (pair)
191  {
192  colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
193 
194  }
195  else
196  {
197  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
198  pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
199  btAssert(pair);
200  pair->m_userPointer = colAlgo;
201  }
202  }
203 
204  btAssert(colAlgo);
205 
206  const btCollisionObjectWrapper* tmpWrap0 = 0;
207  const btCollisionObjectWrapper* tmpWrap1 = 0;
208 
209  tmpWrap0 = m_resultOut->getBody0Wrap();
210  tmpWrap1 = m_resultOut->getBody1Wrap();
211 
212  m_resultOut->setBody0Wrap(&compoundWrap0);
213  m_resultOut->setBody1Wrap(&compoundWrap1);
214 
215  m_resultOut->setShapeIdentifiersA(-1,childIndex0);
216  m_resultOut->setShapeIdentifiersB(-1,childIndex1);
217 
218 
219  colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
220 
221  m_resultOut->setBody0Wrap(tmpWrap0);
222  m_resultOut->setBody1Wrap(tmpWrap1);
223 
224 
225 
226  }
227  }
228 };
229 
230 
231 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
232  const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
233 {
234  btVector3 newmin,newmax;
235  btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
236  newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
237  newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
238  btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
239  return Intersect(a,newb);
240 }
241 
242 
243 static inline void MycollideTT( const btDbvtNode* root0,
244  const btDbvtNode* root1,
245  const btTransform& xform,
246  btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
247 {
248 
249  if(root0&&root1)
250  {
251  int depth=1;
252  int treshold=btDbvt::DOUBLE_STACKSIZE-4;
255  stkStack[0]=btDbvt::sStkNN(root0,root1);
256  do {
257  btDbvt::sStkNN p=stkStack[--depth];
258  if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
259  {
260  if(depth>treshold)
261  {
262  stkStack.resize(stkStack.size()*2);
263  treshold=stkStack.size()-4;
264  }
265  if(p.a->isinternal())
266  {
267  if(p.b->isinternal())
268  {
269  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
270  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
271  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
272  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
273  }
274  else
275  {
276  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
277  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
278  }
279  }
280  else
281  {
282  if(p.b->isinternal())
283  {
284  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
285  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
286  }
287  else
288  {
289  callback->Process(p.a,p.b);
290  }
291  }
292  }
293  } while(depth);
294  }
295 }
296 
298 {
299 
300  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
301  const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
302 
303  btAssert (col0ObjWrap->getCollisionShape()->isCompound());
304  btAssert (col1ObjWrap->getCollisionShape()->isCompound());
305  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
306  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
307 
308  const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
309  const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
310  if (!tree0 || !tree1)
311  {
312  return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
313  }
316  if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
317  {
320  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
321  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
322 
323  }
324 
325 
329  {
330  int i;
333  for (i=0;i<pairs.size();i++)
334  {
335  if (pairs[i].m_userPointer)
336  {
337  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
338  algo->getAllContactManifolds(manifoldArray);
339  for (int m=0;m<manifoldArray.size();m++)
340  {
341  if (manifoldArray[m]->getNumContacts())
342  {
343  resultOut->setPersistentManifold(manifoldArray[m]);
344  resultOut->refreshContactPoints();
345  resultOut->setPersistentManifold(0);
346  }
347  }
348  manifoldArray.resize(0);
349  }
350  }
351  }
352 
353 
354 
355 
356  btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
357 
358 
359  const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
360  MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
361 
362  //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
363 
364  //remove non-overlapping child pairs
365 
366  {
368 
369  //iterate over all children, perform an AABB check inside ProcessChildShape
371 
372  int i;
374 
375 
376 
377 
378 
379  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
380 
381  for (i=0;i<pairs.size();i++)
382  {
383  if (pairs[i].m_userPointer)
384  {
385  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
386 
387  {
388  btTransform orgTrans0;
389  const btCollisionShape* childShape0 = 0;
390 
391  btTransform newChildWorldTrans0;
392  btTransform orgInterpolationTrans0;
393  childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
394  orgTrans0 = col0ObjWrap->getWorldTransform();
395  orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
396  const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
397  newChildWorldTrans0 = orgTrans0*childTrans0 ;
398  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
399  }
401  aabbMin0 -= thresholdVec;
402  aabbMax0 += thresholdVec;
403  {
404  btTransform orgInterpolationTrans1;
405  const btCollisionShape* childShape1 = 0;
406  btTransform orgTrans1;
407  btTransform newChildWorldTrans1;
408 
409  childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
410  orgTrans1 = col1ObjWrap->getWorldTransform();
411  orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
412  const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
413  newChildWorldTrans1 = orgTrans1*childTrans1 ;
414  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
415  }
416 
417  aabbMin1 -= thresholdVec;
418  aabbMax1 += thresholdVec;
419 
420  if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
421  {
422  algo->~btCollisionAlgorithm();
424  m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
425  }
426  }
427  }
428  for (int i=0;i<m_removePairs.size();i++)
429  {
431  }
433  }
434 
435 }
436 
438 {
439  btAssert(0);
440  return 0.f;
441 
442 }
443 
444 
445 
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:114
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.
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
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
int size() const
return the number of elements in the array
#define BT_PROFILE(name)
Definition: btQuickprof.h:213
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:279
btShapePairCallback gCompoundCompoundChildShapePairCallback