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  if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
167  return;
168  }
169 
170  if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
171  {
172  btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
173  btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
174 
175 
176  btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
177 
178  btCollisionAlgorithm* colAlgo = 0;
179 
180  if (pair)
181  {
182  colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
183 
184  } else
185  {
186  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
187  pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
188  btAssert(pair);
189  pair->m_userPointer = colAlgo;
190  }
191 
192  btAssert(colAlgo);
193 
194  const btCollisionObjectWrapper* tmpWrap0 = 0;
195  const btCollisionObjectWrapper* tmpWrap1 = 0;
196 
197  tmpWrap0 = m_resultOut->getBody0Wrap();
198  tmpWrap1 = m_resultOut->getBody1Wrap();
199 
200  m_resultOut->setBody0Wrap(&compoundWrap0);
201  m_resultOut->setBody1Wrap(&compoundWrap1);
202 
203  m_resultOut->setShapeIdentifiersA(-1,childIndex0);
204  m_resultOut->setShapeIdentifiersB(-1,childIndex1);
205 
206 
207  colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
208 
209  m_resultOut->setBody0Wrap(tmpWrap0);
210  m_resultOut->setBody1Wrap(tmpWrap1);
211 
212 
213 
214  }
215  }
216 };
217 
218 
219 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
220  const btDbvtAabbMm& b, const btTransform& xform)
221 {
222  btVector3 newmin,newmax;
223  btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
224  btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
225  return Intersect(a,newb);
226 }
227 
228 
229 static inline void MycollideTT( const btDbvtNode* root0,
230  const btDbvtNode* root1,
231  const btTransform& xform,
233 {
234 
235  if(root0&&root1)
236  {
237  int depth=1;
238  int treshold=btDbvt::DOUBLE_STACKSIZE-4;
241  stkStack[0]=btDbvt::sStkNN(root0,root1);
242  do {
243  btDbvt::sStkNN p=stkStack[--depth];
244  if(MyIntersect(p.a->volume,p.b->volume,xform))
245  {
246  if(depth>treshold)
247  {
248  stkStack.resize(stkStack.size()*2);
249  treshold=stkStack.size()-4;
250  }
251  if(p.a->isinternal())
252  {
253  if(p.b->isinternal())
254  {
255  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
256  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
257  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
258  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
259  }
260  else
261  {
262  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
263  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
264  }
265  }
266  else
267  {
268  if(p.b->isinternal())
269  {
270  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
271  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
272  }
273  else
274  {
275  callback->Process(p.a,p.b);
276  }
277  }
278  }
279  } while(depth);
280  }
281 }
282 
284 {
285 
286  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
287  const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
288 
289  btAssert (col0ObjWrap->getCollisionShape()->isCompound());
290  btAssert (col1ObjWrap->getCollisionShape()->isCompound());
291  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
292  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
293 
294  const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
295  const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
296  if (!tree0 || !tree1)
297  {
298  return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
299  }
302  if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
303  {
306  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
307  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
308 
309  }
310 
311 
315  {
316  int i;
319  for (i=0;i<pairs.size();i++)
320  {
321  if (pairs[i].m_userPointer)
322  {
323  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
324  algo->getAllContactManifolds(manifoldArray);
325  for (int m=0;m<manifoldArray.size();m++)
326  {
327  if (manifoldArray[m]->getNumContacts())
328  {
329  resultOut->setPersistentManifold(manifoldArray[m]);
330  resultOut->refreshContactPoints();
331  resultOut->setPersistentManifold(0);
332  }
333  }
334  manifoldArray.resize(0);
335  }
336  }
337  }
338 
339 
340 
341 
342  btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
343 
344 
345  const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
346  MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
347 
348  //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
349 
350  //remove non-overlapping child pairs
351 
352  {
354 
355  //iterate over all children, perform an AABB check inside ProcessChildShape
357 
358  int i;
360 
361 
362 
363 
364 
365  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
366 
367  for (i=0;i<pairs.size();i++)
368  {
369  if (pairs[i].m_userPointer)
370  {
371  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
372 
373  {
374  btTransform orgTrans0;
375  const btCollisionShape* childShape0 = 0;
376 
377  btTransform newChildWorldTrans0;
378  btTransform orgInterpolationTrans0;
379  childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
380  orgTrans0 = col0ObjWrap->getWorldTransform();
381  orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
382  const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
383  newChildWorldTrans0 = orgTrans0*childTrans0 ;
384  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
385  }
386 
387  {
388  btTransform orgInterpolationTrans1;
389  const btCollisionShape* childShape1 = 0;
390  btTransform orgTrans1;
391  btTransform newChildWorldTrans1;
392 
393  childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
394  orgTrans1 = col1ObjWrap->getWorldTransform();
395  orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
396  const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
397  newChildWorldTrans1 = orgTrans1*childTrans1 ;
398  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
399  }
400 
401 
402 
403  if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
404  {
405  algo->~btCollisionAlgorithm();
407  m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
408  }
409  }
410  }
411  for (int i=0;i<m_removePairs.size();i++)
412  {
414  }
416  }
417 
418 }
419 
421 {
422  btAssert(0);
423  return 0.f;
424 
425 }
426 
427 
428 
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 btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold=0)=0
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
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...
static void MycollideTT(const btDbvtNode *root0, const btDbvtNode *root1, const btTransform &xform, btCompoundCompoundLeafCallback *callback)
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)
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)
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:203
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
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:69
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