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 
22 #include "LinearMath/btAabbUtil2.h"
25 
26 
28 
30 :btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
31 {
32 
33  void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
35 
36  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
37  btAssert (col0ObjWrap->getCollisionShape()->isCompound());
38 
39  const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
40  btAssert (col1ObjWrap->getCollisionShape()->isCompound());
41 
42  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
43  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
44 
45  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
46  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
47 
48 
49 }
50 
51 
53 {
57 }
58 
60 {
61  int i;
63  for (i=0;i<pairs.size();i++)
64  {
65  if (pairs[i].m_userPointer)
66  {
67 
68  ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
69  }
70  }
71 }
72 
73 
75 {
77 
78  int numChildren = pairs.size();
79  int i;
80  for (i=0;i<numChildren;i++)
81  {
82  if (pairs[i].m_userPointer)
83  {
84  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
85  algo->~btCollisionAlgorithm();
87  }
88  }
90 }
91 
93 {
95 
96 
102 
103 
105 
107 
109  const btCollisionObjectWrapper* compound0ObjWrap,
110  btDispatcher* dispatcher,
111  const btDispatcherInfo& dispatchInfo,
112  btManifoldResult* resultOut,
113  btHashedSimplePairCache* childAlgorithmsCache,
114  btPersistentManifold* sharedManifold)
115  :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
116  m_childCollisionAlgorithmCache(childAlgorithmsCache),
117  m_sharedManifold(sharedManifold)
118  {
119 
120  }
121 
122 
123 
124 
125  void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
126  {
127  m_numOverlapPairs++;
128 
129 
130  int childIndex0 = leaf0->dataAsInt;
131  int childIndex1 = leaf1->dataAsInt;
132 
133 
134  btAssert(childIndex0>=0);
135  btAssert(childIndex1>=0);
136 
137 
138  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
139  btAssert(childIndex0<compoundShape0->getNumChildShapes());
140 
141  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
142  btAssert(childIndex1<compoundShape1->getNumChildShapes());
143 
144  const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
145  const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
146 
147  //backup
148  btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
149  const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
150  btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
151 
152  btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
153  const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
154  btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ;
155 
156 
157  //perform an AABB check first
158  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
159  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
160  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
161 
163  {
164  if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
165  return;
166  }
167 
168  if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
169  {
170  btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
171  btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
172 
173 
174  btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
175 
176  btCollisionAlgorithm* colAlgo = 0;
177 
178  if (pair)
179  {
180  colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
181 
182  } else
183  {
184  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
185  pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
186  btAssert(pair);
187  pair->m_userPointer = colAlgo;
188  }
189 
190  btAssert(colAlgo);
191 
192  const btCollisionObjectWrapper* tmpWrap0 = 0;
193  const btCollisionObjectWrapper* tmpWrap1 = 0;
194 
195  tmpWrap0 = m_resultOut->getBody0Wrap();
196  tmpWrap1 = m_resultOut->getBody1Wrap();
197 
198  m_resultOut->setBody0Wrap(&compoundWrap0);
199  m_resultOut->setBody1Wrap(&compoundWrap1);
200 
201  m_resultOut->setShapeIdentifiersA(-1,childIndex0);
202  m_resultOut->setShapeIdentifiersB(-1,childIndex1);
203 
204 
205  colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
206 
207  m_resultOut->setBody0Wrap(tmpWrap0);
208  m_resultOut->setBody1Wrap(tmpWrap1);
209 
210 
211 
212  }
213  }
214 };
215 
216 
217 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
218  const btDbvtAabbMm& b, const btTransform& xform)
219 {
220  btVector3 newmin,newmax;
221  btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
222  btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
223  return Intersect(a,newb);
224 }
225 
226 
227 static inline void MycollideTT( const btDbvtNode* root0,
228  const btDbvtNode* root1,
229  const btTransform& xform,
231 {
232 
233  if(root0&&root1)
234  {
235  int depth=1;
236  int treshold=btDbvt::DOUBLE_STACKSIZE-4;
239  stkStack[0]=btDbvt::sStkNN(root0,root1);
240  do {
241  btDbvt::sStkNN p=stkStack[--depth];
242  if(MyIntersect(p.a->volume,p.b->volume,xform))
243  {
244  if(depth>treshold)
245  {
246  stkStack.resize(stkStack.size()*2);
247  treshold=stkStack.size()-4;
248  }
249  if(p.a->isinternal())
250  {
251  if(p.b->isinternal())
252  {
253  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
254  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
255  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
256  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
257  }
258  else
259  {
260  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
261  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
262  }
263  }
264  else
265  {
266  if(p.b->isinternal())
267  {
268  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
269  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
270  }
271  else
272  {
273  callback->Process(p.a,p.b);
274  }
275  }
276  }
277  } while(depth);
278  }
279 }
280 
282 {
283 
284  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
285  const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
286 
287  btAssert (col0ObjWrap->getCollisionShape()->isCompound());
288  btAssert (col1ObjWrap->getCollisionShape()->isCompound());
289  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
290  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
291 
292  const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
293  const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
294  if (!tree0 || !tree1)
295  {
296  return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
297  }
300  if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
301  {
304  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
305  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
306 
307  }
308 
309 
313  {
314  int i;
315  btManifoldArray manifoldArray;
317  for (i=0;i<pairs.size();i++)
318  {
319  if (pairs[i].m_userPointer)
320  {
321  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
322  algo->getAllContactManifolds(manifoldArray);
323  for (int m=0;m<manifoldArray.size();m++)
324  {
325  if (manifoldArray[m]->getNumContacts())
326  {
327  resultOut->setPersistentManifold(manifoldArray[m]);
328  resultOut->refreshContactPoints();
329  resultOut->setPersistentManifold(0);
330  }
331  }
332  manifoldArray.resize(0);
333  }
334  }
335  }
336 
337 
338 
339 
340  btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
341 
342 
343  const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
344  MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
345 
346  //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
347 
348  //remove non-overlapping child pairs
349 
350  {
352 
353  //iterate over all children, perform an AABB check inside ProcessChildShape
355 
356  int i;
357  btManifoldArray manifoldArray;
358 
359 
360 
361 
362 
363  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
364 
365  for (i=0;i<pairs.size();i++)
366  {
367  if (pairs[i].m_userPointer)
368  {
369  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
370 
371  {
372  btTransform orgTrans0;
373  const btCollisionShape* childShape0 = 0;
374 
375  btTransform newChildWorldTrans0;
376  btTransform orgInterpolationTrans0;
377  childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
378  orgTrans0 = col0ObjWrap->getWorldTransform();
379  orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
380  const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
381  newChildWorldTrans0 = orgTrans0*childTrans0 ;
382  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
383  }
384 
385  {
386  btTransform orgInterpolationTrans1;
387  const btCollisionShape* childShape1 = 0;
388  btTransform orgTrans1;
389  btTransform newChildWorldTrans1;
390 
391  childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
392  orgTrans1 = col1ObjWrap->getWorldTransform();
393  orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
394  const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
395  newChildWorldTrans1 = orgTrans1*childTrans1 ;
396  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
397  }
398 
399 
400 
401  if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
402  {
403  algo->~btCollisionAlgorithm();
405  m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
406  }
407  }
408  }
409  for (int i=0;i<m_removePairs.size();i++)
410  {
412  }
414  }
415 
416 }
417 
419 {
420  btAssert(0);
421  return 0.f;
422 
423 }
424 
425 
426 
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)
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold=0)=0
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
virtual btSimplePair * addOverlappingPair(int indexA, int indexB)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
class btPersistentManifold * m_sharedManifold
int getUpdateRevision() const
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
const btCollisionObjectWrapper * m_compound0ColObjWrap
#define btAssert(x)
Definition: btScalar.h:113
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:194
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
static void MycollideTT(const btDbvtNode *root0, const btDbvtNode *root1, const btTransform &xform, btCompoundCompoundLeafCallback *callback)
const btDbvtNode * b
Definition: btDbvt.h:200
btDbvtNode * m_root
Definition: btDbvt.h:258
btManifoldResult is a helper class to manage contact results.
const btCollisionObjectWrapper * getBody1Wrap() const
const btDbvt * getDynamicAabbTree() 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()
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:182
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...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
int size() const
return the number of elements in the array
btDbvtNode * childs[2]
Definition: btDbvt.h:185
const btCollisionObjectWrapper * getBody0Wrap() const
btTransform & getChildTransform(int index)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:411
const btDbvtNode * a
Definition: btDbvt.h:199
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
const btTransform & getWorldTransform() const
#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)
virtual void freeCollisionAlgorithm(void *ptr)=0
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
virtual void setShapeIdentifiersB(int partId1, int index1)
const btCollisionShape * getCollisionShape() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
DBVT_INLINE const btVector3 & Maxs() const
Definition: btDbvt.h:136
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
bool isCompound() const
void resize(int newsize, const T &fillData=T())
btDbvtVolume volume
Definition: btDbvt.h:179
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision s...
btSimplePair * findPair(int indexA, int indexB)
const btCollisionObjectWrapper * m_compound1ColObjWrap
int dataAsInt
Definition: btDbvt.h:187
#define DBVT_INLINE
Definition: btDbvt.h:55
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:520
#define btAlignedAlloc(size, alignment)
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)
DBVT_INLINE const btVector3 & Mins() const
Definition: btDbvt.h:135
btCollisionShape * getChildShape(int index)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
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:278
btShapePairCallback gCompoundCompoundChildShapePairCallback
const btCollisionObject * getCollisionObject() const