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 "LinearMath/btQuickprof.h"
19 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
20 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
21 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
22 #include "LinearMath/btIDebugDraw.h"
23 #include "LinearMath/btAabbUtil2.h"
24 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
25 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.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
30 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
31
btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)32 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
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
~btCompoundCompoundCollisionAlgorithm()51 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
52 {
53 removeChildAlgorithms();
54 m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
55 btAlignedFree(m_childCollisionAlgorithmCache);
56 }
57
getAllContactManifolds(btManifoldArray & manifoldArray)58 void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
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
removeChildAlgorithms()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 {
81 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
82 algo->~btCollisionAlgorithm();
83 m_dispatcher->freeCollisionAlgorithm(algo);
84 }
85 }
86 m_childCollisionAlgorithmCache->removeAllPairs();
87 }
88
89 struct btCompoundCompoundLeafCallback : btDbvt::ICollide
90 {
91 int m_numOverlapPairs;
92
93 const btCollisionObjectWrapper* m_compound0ColObjWrap;
94 const btCollisionObjectWrapper* m_compound1ColObjWrap;
95 btDispatcher* m_dispatcher;
96 const btDispatcherInfo& m_dispatchInfo;
97 btManifoldResult* m_resultOut;
98
99 class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
100
101 btPersistentManifold* m_sharedManifold;
102
btCompoundCompoundLeafCallbackbtCompoundCompoundLeafCallback103 btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap,
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
ProcessbtCompoundCompoundLeafCallback114 void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1)
115 {
116 BT_PROFILE("btCompoundCompoundLeafCallback::Process");
117 m_numOverlapPairs++;
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
135 btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
136 const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
137 btTransform newChildWorldTrans0 = orgTrans0 * childTrans0;
138
139 btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
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
148 btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
149
150 aabbMin0 -= thresholdVec;
151 aabbMax0 += thresholdVec;
152
153 if (gCompoundCompoundChildShapePairCallback)
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;
167 if (m_resultOut->m_closestPointDistanceThreshold > 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();
209 m_dispatcher->freeCollisionAlgorithm(colAlgo);
210 }
211 }
212 }
213 };
214
MyIntersect(const btDbvtAabbMm & a,const btDbvtAabbMm & b,const btTransform & xform,btScalar distanceThreshold)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
MycollideTT(const btDbvtNode * root0,const btDbvtNode * root1,const btTransform & xform,btCompoundCompoundLeafCallback * callback,btScalar distanceThreshold)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;
235 btAlignedObjectArray<btDbvt::sStkNN> stkStack;
236 #ifdef USE_LOCAL_STACK
237 ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
238 stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE);
239 #else
240 stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
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
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)285 void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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 }
301 ///btCompoundShape might have changed:
302 ////make sure the internal child collision algorithm caches are still valid
303 if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
304 {
305 ///clear all
306 removeChildAlgorithms();
307 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
308 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
309 }
310
311 ///we need to refresh all contact manifolds
312 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
313 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
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 {
326 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
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 {
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 }
377 btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
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();
396 m_dispatcher->freeCollisionAlgorithm(algo);
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
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)409 btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
410 {
411 btAssert(0);
412 return 0.f;
413 }
414