Blender  V3.3
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);
36  m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache();
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 {
53  removeChildAlgorithms();
54  m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
55  btAlignedFree(m_childCollisionAlgorithmCache);
56 }
57 
59 {
60  int i;
61  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
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 
71 void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
72 {
73  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
74 
75  int numChildren = pairs.size();
76  int i;
77  for (i = 0; i < numChildren; i++)
78  {
79  if (pairs[i].m_userPointer)
80  {
82  algo->~btCollisionAlgorithm();
84  }
85  }
86  m_childCollisionAlgorithmCache->removeAllPairs();
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 
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,
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  {
306  removeChildAlgorithms();
307  m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
308  m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
309  }
310 
314  {
315  int i;
316  btManifoldArray manifoldArray;
317 #ifdef USE_LOCAL_STACK
318  btPersistentManifold localManifolds[4];
319  manifoldArray.initializeFromBuffer(&localManifolds, 0, 4);
320 #endif
321  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
322  for (i = 0; i < pairs.size(); i++)
323  {
324  if (pairs[i].m_userPointer)
325  {
327  algo->getAllContactManifolds(manifoldArray);
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  }
337  manifoldArray.resize(0);
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
355  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
356 
357  int i;
358  btManifoldArray manifoldArray;
359 
360  btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
361 
362  for (i = 0; i < pairs.size(); i++)
363  {
364  if (pairs[i].m_userPointer)
365  {
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  {
403  m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB);
404  }
405  m_removePairs.clear();
406  }
407 }
408 
410 {
411  btAssert(0);
412  return 0.f;
413 }
SIMD_FORCE_INLINE 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
SIMD_FORCE_INLINE void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
Definition: btAabbUtil2.h:172
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
btCollisionObject
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void * m_userPointer
bool(* btShapePairCallback)(const btCollisionShape *pShape0, const btCollisionShape *pShape1)
btShapePairCallback gCompoundCompoundChildShapePairCallback
static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b, const btTransform &xform, btScalar distanceThreshold)
static void MycollideTT(const btDbvtNode *root0, const btDbvtNode *root1, const btTransform &xform, btCompoundCompoundLeafCallback *callback, btScalar distanceThreshold)
btCompoundShape(bool enableDynamicAabbTree=true, const int initialChildCapacity=0)
int getNumChildShapes() const
DBVT_INLINE bool Intersect(const btDbvtAabbMm &a, const btDbvtAabbMm &b)
Definition: btDbvt.h:621
#define DBVT_INLINE
Definition: btDbvt.h:53
@ BT_CLOSEST_POINT_ALGORITHMS
Definition: btDispatcher.h:71
@ BT_CONTACT_POINT_ALGORITHMS
Definition: btDispatcher.h:70
btPersistentManifold()
SIMD_FORCE_INLINE int getNumContacts() const
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:285
#define btAssert(x)
Definition: btScalar.h:295
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void initializeFromBuffer(void *buffer, int size, int capacity)
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)=0
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision s...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
class btPersistentManifold * m_sharedManifold
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
btSimplePairArray & getOverlappingPairArray()
btSimplePair * findPair(int indexA, int indexB)
virtual btSimplePair * addOverlappingPair(int indexA, int indexB)
virtual void * removeOverlappingPair(int indexA, int indexB)
btManifoldResult is a helper class to manage contact results.
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
void setBody0Wrap(const btCollisionObjectWrapper *obj0Wrap)
const btCollisionObjectWrapper * getBody1Wrap() const
const btCollisionObjectWrapper * getBody0Wrap() const
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
virtual void setShapeIdentifiersB(int partId1, int index1)
btScalar m_closestPointDistanceThreshold
SIMD_FORCE_INLINE void refreshContactPoints()
DEGForeachIDComponentCallback callback
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const
void Process(const btDbvtNode *leaf0, const btDbvtNode *leaf1)
const btCollisionObjectWrapper * m_compound0ColObjWrap
btCompoundCompoundLeafCallback(const btCollisionObjectWrapper *compound1ObjWrap, const btCollisionObjectWrapper *compound0ObjWrap, btDispatcher *dispatcher, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut, btHashedSimplePairCache *childAlgorithmsCache, btPersistentManifold *sharedManifold)
const btCollisionObjectWrapper * m_compound1ColObjWrap
class btHashedSimplePairCache * m_childCollisionAlgorithmCache
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:185
btDbvtNode * childs[2]
Definition: btDbvt.h:187
btDbvtVolume volume
Definition: btDbvt.h:182
int dataAsInt
Definition: btDbvt.h:189
const btDbvtNode * b
Definition: btDbvt.h:234
const btDbvtNode * a
Definition: btDbvt.h:233
Definition: btDbvt.h:229
@ DOUBLE_STACKSIZE
Definition: btDbvt.h:298
btDbvtNode * m_root
Definition: btDbvt.h:302
PointerRNA * ptr
Definition: wm_files.c:3480