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 
17 #include "btCompoundCompoundCollisionAlgorithm.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
20 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
21 #include "LinearMath/btIDebugDraw.h"
22 #include "LinearMath/btAabbUtil2.h"
23 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
25 
26 
27 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
28 
29 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
30 :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
31 m_sharedManifold(ci.m_manifold)
32 {
33  m_ownsManifold = false;
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 
51 }
52 
53 
54 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
55 {
56  removeChildAlgorithms();
57  m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
58  btAlignedFree(m_childCollisionAlgorithmCache);
59 }
60 
61 void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
62 {
63  int i;
64  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
65  for (i=0;i<pairs.size();i++)
66  {
67  if (pairs[i].m_userPointer)
68  {
69 
70  ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
71  }
72  }
73 }
74 
75 
76 void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
77 {
78  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
79 
80  int numChildren = pairs.size();
81  int i;
82  for (i=0;i<numChildren;i++)
83  {
84  if (pairs[i].m_userPointer)
85  {
86  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
87  algo->~btCollisionAlgorithm();
88  m_dispatcher->freeCollisionAlgorithm(algo);
89  }
90  }
91  m_childCollisionAlgorithmCache->removeAllPairs();
92 }
93 
94 struct btCompoundCompoundLeafCallback : btDbvt::ICollide
95 {
96  int m_numOverlapPairs;
97 
98 
99  const btCollisionObjectWrapper* m_compound0ColObjWrap;
100  const btCollisionObjectWrapper* m_compound1ColObjWrap;
101  btDispatcher* m_dispatcher;
102  const btDispatcherInfo& m_dispatchInfo;
103  btManifoldResult* m_resultOut;
104 
105 
106  class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
107 
108  btPersistentManifold* m_sharedManifold;
109 
110  btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
111  const btCollisionObjectWrapper* compound0ObjWrap,
112  btDispatcher* dispatcher,
113  const btDispatcherInfo& dispatchInfo,
114  btManifoldResult* resultOut,
115  btHashedSimplePairCache* childAlgorithmsCache,
116  btPersistentManifold* sharedManifold)
117  :m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
118  m_childCollisionAlgorithmCache(childAlgorithmsCache),
119  m_sharedManifold(sharedManifold),
120  m_numOverlapPairs(0)
121  {
122 
123  }
124 
125 
126 
127 
128  void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
129  {
130  m_numOverlapPairs++;
131 
132 
133  int childIndex0 = leaf0->dataAsInt;
134  int childIndex1 = leaf1->dataAsInt;
135 
136 
137  btAssert(childIndex0>=0);
138  btAssert(childIndex1>=0);
139 
140 
141  const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
142  btAssert(childIndex0<compoundShape0->getNumChildShapes());
143 
144  const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
145  btAssert(childIndex1<compoundShape1->getNumChildShapes());
146 
147  const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
148  const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
149 
150  //backup
151  btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
152  const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
153  btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ;
154 
155  btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
156  const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
157  btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ;
158 
159 
160  //perform an AABB check first
161  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
162  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
163  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
164 
165  if (gCompoundCompoundChildShapePairCallback)
166  {
167  if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
168  return;
169  }
170 
171  if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
172  {
173  btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
174  btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
175 
176 
177  btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
178 
179  btCollisionAlgorithm* colAlgo = 0;
180 
181  if (pair)
182  {
183  colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
184 
185  } else
186  {
187  colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
188  pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
189  btAssert(pair);
190  pair->m_userPointer = colAlgo;
191  }
192 
193  btAssert(colAlgo);
194 
195  const btCollisionObjectWrapper* tmpWrap0 = 0;
196  const btCollisionObjectWrapper* tmpWrap1 = 0;
197 
198  tmpWrap0 = m_resultOut->getBody0Wrap();
199  tmpWrap1 = m_resultOut->getBody1Wrap();
200 
201  m_resultOut->setBody0Wrap(&compoundWrap0);
202  m_resultOut->setBody1Wrap(&compoundWrap1);
203 
204  m_resultOut->setShapeIdentifiersA(-1,childIndex0);
205  m_resultOut->setShapeIdentifiersB(-1,childIndex1);
206 
207 
208  colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
209 
210  m_resultOut->setBody0Wrap(tmpWrap0);
211  m_resultOut->setBody1Wrap(tmpWrap1);
212 
213 
214 
215  }
216  }
217 };
218 
219 
220 static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
221  const btDbvtAabbMm& b, const btTransform& xform)
222 {
223  btVector3 newmin,newmax;
224  btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
225  btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
226  return Intersect(a,newb);
227 }
228 
229 
230 static inline void MycollideTT( const btDbvtNode* root0,
231  const btDbvtNode* root1,
232  const btTransform& xform,
233  btCompoundCompoundLeafCallback* callback)
234 {
235 
236  if(root0&&root1)
237  {
238  int depth=1;
239  int treshold=btDbvt::DOUBLE_STACKSIZE-4;
240  btAlignedObjectArray<btDbvt::sStkNN> stkStack;
241  stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
242  stkStack[0]=btDbvt::sStkNN(root0,root1);
243  do {
244  btDbvt::sStkNN p=stkStack[--depth];
245  if(MyIntersect(p.a->volume,p.b->volume,xform))
246  {
247  if(depth>treshold)
248  {
249  stkStack.resize(stkStack.size()*2);
250  treshold=stkStack.size()-4;
251  }
252  if(p.a->isinternal())
253  {
254  if(p.b->isinternal())
255  {
256  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
257  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
258  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
259  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
260  }
261  else
262  {
263  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
264  stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
265  }
266  }
267  else
268  {
269  if(p.b->isinternal())
270  {
271  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
272  stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
273  }
274  else
275  {
276  callback->Process(p.a,p.b);
277  }
278  }
279  }
280  } while(depth);
281  }
282 }
283 
284 void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
285 {
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 
297  if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
298  {
300  removeChildAlgorithms();
301  }
302 
303 
307  {
308  int i;
309  btManifoldArray manifoldArray;
310  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
311  for (i=0;i<pairs.size();i++)
312  {
313  if (pairs[i].m_userPointer)
314  {
315  btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
316  algo->getAllContactManifolds(manifoldArray);
317  for (int m=0;m<manifoldArray.size();m++)
318  {
319  if (manifoldArray[m]->getNumContacts())
320  {
321  resultOut->setPersistentManifold(manifoldArray[m]);
322  resultOut->refreshContactPoints();
323  resultOut->setPersistentManifold(0);
324  }
325  }
326  manifoldArray.resize(0);
327  }
328  }
329  }
330 
331 
332  const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
333  const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
334 
335  btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
336 
337 
338  const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
339  MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
340 
341  //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
342 
343  //remove non-overlapping child pairs
344 
345  {
346  btAssert(m_removePairs.size()==0);
347 
348  //iterate over all children, perform an AABB check inside ProcessChildShape
349  btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
350 
351  int i;
352  btManifoldArray manifoldArray;
353 
354 
355 
356 
357 
358  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
359 
360  for (i=0;i<pairs.size();i++)
361  {
362  if (pairs[i].m_userPointer)
363  {
364  btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
365 
366  {
367  btTransform orgTrans0;
368  const btCollisionShape* childShape0 = 0;
369 
370  btTransform newChildWorldTrans0;
371  btTransform orgInterpolationTrans0;
372  childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
373  orgTrans0 = col0ObjWrap->getWorldTransform();
374  orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
375  const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
376  newChildWorldTrans0 = orgTrans0*childTrans0 ;
377  childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
378  }
379 
380  {
381  btTransform orgInterpolationTrans1;
382  const btCollisionShape* childShape1 = 0;
383  btTransform orgTrans1;
384  btTransform newChildWorldTrans1;
385 
386  childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
387  orgTrans1 = col1ObjWrap->getWorldTransform();
388  orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
389  const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
390  newChildWorldTrans1 = orgTrans1*childTrans1 ;
391  childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
392  }
393 
394 
395 
396  if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
397  {
398  algo->~btCollisionAlgorithm();
399  m_dispatcher->freeCollisionAlgorithm(algo);
400  m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
401  }
402  }
403  }
404  for (int i=0;i<m_removePairs.size();i++)
405  {
406  m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
407  }
408  m_removePairs.clear();
409  }
410 
411 }
412 
413 btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
414 {
415  btAssert(0);
416  return 0.f;
417 
418 }
419 
420 
421 
Generated on Wed Apr 9 2014 02:38:22 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2