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
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)29 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
30 :btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
31 {
32
33 void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
34 m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
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
~btCompoundCompoundCollisionAlgorithm()52 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
53 {
54 removeChildAlgorithms();
55 m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
56 btAlignedFree(m_childCollisionAlgorithmCache);
57 }
58
getAllContactManifolds(btManifoldArray & manifoldArray)59 void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
60 {
61 int i;
62 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
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
removeChildAlgorithms()74 void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
75 {
76 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
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();
86 m_dispatcher->freeCollisionAlgorithm(algo);
87 }
88 }
89 m_childCollisionAlgorithmCache->removeAllPairs();
90 }
91
92 struct btCompoundCompoundLeafCallback : btDbvt::ICollide
93 {
94 int m_numOverlapPairs;
95
96
97 const btCollisionObjectWrapper* m_compound0ColObjWrap;
98 const btCollisionObjectWrapper* m_compound1ColObjWrap;
99 btDispatcher* m_dispatcher;
100 const btDispatcherInfo& m_dispatchInfo;
101 btManifoldResult* m_resultOut;
102
103
104 class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
105
106 btPersistentManifold* m_sharedManifold;
107
btCompoundCompoundLeafCallbackbtCompoundCompoundLeafCallback108 btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
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
ProcessbtCompoundCompoundLeafCallback125 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
162 if (gCompoundCompoundChildShapePairCallback)
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
MyIntersect(const btDbvtAabbMm & a,const btDbvtAabbMm & b,const btTransform & xform)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
MycollideTT(const btDbvtNode * root0,const btDbvtNode * root1,const btTransform & xform,btCompoundCompoundLeafCallback * callback)227 static inline void MycollideTT( const btDbvtNode* root0,
228 const btDbvtNode* root1,
229 const btTransform& xform,
230 btCompoundCompoundLeafCallback* callback)
231 {
232
233 if(root0&&root1)
234 {
235 int depth=1;
236 int treshold=btDbvt::DOUBLE_STACKSIZE-4;
237 btAlignedObjectArray<btDbvt::sStkNN> stkStack;
238 stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
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
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)281 void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
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 }
298 ///btCompoundShape might have changed:
299 ////make sure the internal child collision algorithm caches are still valid
300 if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
301 {
302 ///clear all
303 removeChildAlgorithms();
304 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
305 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
306
307 }
308
309
310 ///we need to refresh all contact manifolds
311 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
312 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
313 {
314 int i;
315 btManifoldArray manifoldArray;
316 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
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 {
351 btAssert(m_removePairs.size()==0);
352
353 //iterate over all children, perform an AABB check inside ProcessChildShape
354 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
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();
404 m_dispatcher->freeCollisionAlgorithm(algo);
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 {
411 m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
412 }
413 m_removePairs.clear();
414 }
415
416 }
417
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)418 btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
419 {
420 btAssert(0);
421 return 0.f;
422
423 }
424
425
426
427