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  void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16);
37 
38  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
39  btAssert(col0ObjWrap->getCollisionShape()->isCompound());
40 
41  const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
42  btAssert(col1ObjWrap->getCollisionShape()->isCompound());
43 
44  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
45  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
46 
47  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
48  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
49 }
50 
52 {
56 }
57 
59 {
60  int i;
62  for (i = 0; i < pairs.size(); i++)
63  {
64  if (pairs[i].m_userPointer)
65  {
66  ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
67  }
68  }
69 }
70 
72 {
74 
75  int numChildren = pairs.size();
76  int i;
77  for (i = 0; i < numChildren; i++)
78  {
79  if (pairs[i].m_userPointer)
80  {
81  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
82  algo->~btCollisionAlgorithm();
84  }
85  }
87 }
88 
90 {
92 
98 
100 
102 
104  const btCollisionObjectWrapper* compound0ObjWrap,
105  btDispatcher* dispatcher,
106  const btDispatcherInfo& dispatchInfo,
107  btManifoldResult* resultOut,
108  btHashedSimplePairCache* childAlgorithmsCache,
109  btPersistentManifold* sharedManifold)
110  : m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold)
111  {
112  }
113 
114  void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1)
115  {
116  BT_PROFILE("btCompoundCompoundLeafCallback::Process");
118 
119  int childIndex0 = leaf0->dataAsInt;
120  int childIndex1 = leaf1->dataAsInt;
121 
122  btAssert(childIndex0 >= 0);
123  btAssert(childIndex1 >= 0);
124 
125  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
126  btAssert(childIndex0 < compoundShape0->getNumChildShapes());
127 
128  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
129  btAssert(childIndex1 < compoundShape1->getNumChildShapes());
130 
131  const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
132  const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
133 
134  //backup
136  const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
137  btTransform newChildWorldTrans0 = orgTrans0 * childTrans0;
138 
140  const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
141  btTransform newChildWorldTrans1 = orgTrans1 * childTrans1;
142 
143  //perform an AABB check first
144  btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
145  childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
146  childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
147 
149 
150  aabbMin0 -= thresholdVec;
151  aabbMax0 += thresholdVec;
152 
154  {
155  if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1))
156  return;
157  }
158 
159  if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
160  {
161  btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0);
162  btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1);
163 
164  btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1);
165  bool removePair = false;
166  btCollisionAlgorithm* colAlgo = 0;
168  {
169  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
170  removePair = true;
171  }
172  else
173  {
174  if (pair)
175  {
176  colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
177  }
178  else
179  {
180  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
181  pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
182  btAssert(pair);
183  pair->m_userPointer = colAlgo;
184  }
185  }
186 
187  btAssert(colAlgo);
188 
189  const btCollisionObjectWrapper* tmpWrap0 = 0;
190  const btCollisionObjectWrapper* tmpWrap1 = 0;
191 
192  tmpWrap0 = m_resultOut->getBody0Wrap();
193  tmpWrap1 = m_resultOut->getBody1Wrap();
194 
195  m_resultOut->setBody0Wrap(&compoundWrap0);
196  m_resultOut->setBody1Wrap(&compoundWrap1);
197 
198  m_resultOut->setShapeIdentifiersA(-1, childIndex0);
199  m_resultOut->setShapeIdentifiersB(-1, childIndex1);
200 
201  colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut);
202 
203  m_resultOut->setBody0Wrap(tmpWrap0);
204  m_resultOut->setBody1Wrap(tmpWrap1);
205 
206  if (removePair)
207  {
208  colAlgo->~btCollisionAlgorithm();
210  }
211  }
212  }
213 };
214 
215 static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a,
216  const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
217 {
218  btVector3 newmin, newmax;
219  btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax);
220  newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
221  newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
222  btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax);
223  return Intersect(a, newb);
224 }
225 
226 static inline void MycollideTT(const btDbvtNode* root0,
227  const btDbvtNode* root1,
228  const btTransform& xform,
229  btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
230 {
231  if (root0 && root1)
232  {
233  int depth = 1;
234  int treshold = btDbvt::DOUBLE_STACKSIZE - 4;
236 #ifdef USE_LOCAL_STACK
239 #else
241 #endif
242  stkStack[0] = btDbvt::sStkNN(root0, root1);
243  do
244  {
245  btDbvt::sStkNN p = stkStack[--depth];
246  if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold))
247  {
248  if (depth > treshold)
249  {
250  stkStack.resize(stkStack.size() * 2);
251  treshold = stkStack.size() - 4;
252  }
253  if (p.a->isinternal())
254  {
255  if (p.b->isinternal())
256  {
257  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]);
258  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]);
259  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]);
260  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]);
261  }
262  else
263  {
264  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b);
265  stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b);
266  }
267  }
268  else
269  {
270  if (p.b->isinternal())
271  {
272  stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]);
273  stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]);
274  }
275  else
276  {
277  callback->Process(p.a, p.b);
278  }
279  }
280  }
281  } while (depth);
282  }
283 }
284 
286 {
287  const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
288  const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
289 
290  btAssert(col0ObjWrap->getCollisionShape()->isCompound());
291  btAssert(col1ObjWrap->getCollisionShape()->isCompound());
292  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
293  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
294 
295  const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
296  const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
297  if (!tree0 || !tree1)
298  {
299  return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut);
300  }
303  if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
304  {
307  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
308  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
309  }
310 
314  {
315  int i;
317 #ifdef USE_LOCAL_STACK
318  btPersistentManifold localManifolds[4];
319  manifoldArray.initializeFromBuffer(&localManifolds, 0, 4);
320 #endif
322  for (i = 0; i < pairs.size(); i++)
323  {
324  if (pairs[i].m_userPointer)
325  {
326  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
328  for (int m = 0; m < manifoldArray.size(); m++)
329  {
330  if (manifoldArray[m]->getNumContacts())
331  {
332  resultOut->setPersistentManifold(manifoldArray[m]);
333  resultOut->refreshContactPoints();
334  resultOut->setPersistentManifold(0);
335  }
336  }
338  }
339  }
340  }
341 
342  btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold);
343 
344  const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform();
345  MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold);
346 
347  //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
348 
349  //remove non-overlapping child pairs
350 
351  {
352  btAssert(m_removePairs.size() == 0);
353 
354  //iterate over all children, perform an AABB check inside ProcessChildShape
356 
357  int i;
359 
360  btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
361 
362  for (i = 0; i < pairs.size(); i++)
363  {
364  if (pairs[i].m_userPointer)
365  {
366  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
367 
368  {
369  const btCollisionShape* childShape0 = 0;
370 
371  btTransform newChildWorldTrans0;
372  childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
373  const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
374  newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0;
375  childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
376  }
378  aabbMin0 -= thresholdVec;
379  aabbMax0 += thresholdVec;
380  {
381  const btCollisionShape* childShape1 = 0;
382  btTransform newChildWorldTrans1;
383 
384  childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
385  const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
386  newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1;
387  childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
388  }
389 
390  aabbMin1 -= thresholdVec;
391  aabbMax1 += thresholdVec;
392 
393  if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
394  {
395  algo->~btCollisionAlgorithm();
397  m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB));
398  }
399  }
400  }
401  for (int i = 0; i < m_removePairs.size(); i++)
402  {
404  }
406  }
407 }
408 
410 {
411  btAssert(0);
412  return 0.f;
413 }
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:137
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:133
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:263
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:43
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:138
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:187
btTransform & getChildTransform(int index)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:434
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:80
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:84
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
int size() const
return the number of elements in the array
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
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:182
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:185
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:189
#define DBVT_INLINE
Definition: btDbvt.h:54
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:576
#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:172
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:76
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:182
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:294
btShapePairCallback gCompoundCompoundChildShapePairCallback