1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.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 "btManifoldResult.h"
24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
25
26 btShapePairCallback gCompoundChildShapePairCallback = 0;
27
btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)28 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
29 : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
30 m_isSwapped(isSwapped),
31 m_sharedManifold(ci.m_manifold)
32 {
33 m_ownsManifold = false;
34
35 const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
36 btAssert(colObjWrap->getCollisionShape()->isCompound());
37
38 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
39 m_compoundShapeRevision = compoundShape->getUpdateRevision();
40
41 preallocateChildAlgorithms(body0Wrap, body1Wrap);
42 }
43
preallocateChildAlgorithms(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap)44 void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
45 {
46 const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
47 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
48 btAssert(colObjWrap->getCollisionShape()->isCompound());
49
50 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
51
52 int numChildren = compoundShape->getNumChildShapes();
53 int i;
54
55 m_childCollisionAlgorithms.resize(numChildren);
56 for (i = 0; i < numChildren; i++)
57 {
58 if (compoundShape->getDynamicAabbTree())
59 {
60 m_childCollisionAlgorithms[i] = 0;
61 }
62 else
63 {
64 const btCollisionShape* childShape = compoundShape->getChildShape(i);
65
66 btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i); //wrong child trans, but unused (hopefully)
67 m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
68
69 btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
70 btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
71 }
72 }
73 }
74
removeChildAlgorithms()75 void btCompoundCollisionAlgorithm::removeChildAlgorithms()
76 {
77 int numChildren = m_childCollisionAlgorithms.size();
78 int i;
79 for (i = 0; i < numChildren; i++)
80 {
81 if (m_childCollisionAlgorithms[i])
82 {
83 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
84 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
85 }
86 }
87 }
88
~btCompoundCollisionAlgorithm()89 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
90 {
91 removeChildAlgorithms();
92 }
93
94 struct btCompoundLeafCallback : btDbvt::ICollide
95 {
96 public:
97 const btCollisionObjectWrapper* m_compoundColObjWrap;
98 const btCollisionObjectWrapper* m_otherObjWrap;
99 btDispatcher* m_dispatcher;
100 const btDispatcherInfo& m_dispatchInfo;
101 btManifoldResult* m_resultOut;
102 btCollisionAlgorithm** m_childCollisionAlgorithms;
103 btPersistentManifold* m_sharedManifold;
104
btCompoundLeafCallbackbtCompoundLeafCallback105 btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold)
106 : m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold)
107 {
108 }
109
ProcessChildShapebtCompoundLeafCallback110 void ProcessChildShape(const btCollisionShape* childShape, int index)
111 {
112 btAssert(index >= 0);
113 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
114 btAssert(index < compoundShape->getNumChildShapes());
115
116 if (gCompoundChildShapePairCallback)
117 {
118 if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
119 return;
120 }
121
122 //backup
123 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
124
125 const btTransform& childTrans = compoundShape->getChildTransform(index);
126 btTransform newChildWorldTrans = orgTrans * childTrans;
127
128 //perform an AABB check first
129 btVector3 aabbMin0, aabbMax0;
130 childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
131
132 btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
133 aabbMin0 -= extendAabb;
134 aabbMax0 += extendAabb;
135
136 btVector3 aabbMin1, aabbMax1;
137 m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
138
139
140 if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
141 {
142 btTransform preTransform = childTrans;
143 if (this->m_compoundColObjWrap->m_preTransform)
144 {
145 preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
146 }
147 btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
148
149 btCollisionAlgorithm* algo = 0;
150 bool allocatedAlgorithm = false;
151
152 if (m_resultOut->m_closestPointDistanceThreshold > 0)
153 {
154 algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
155 allocatedAlgorithm = true;
156 }
157 else
158 {
159 //the contactpoint is still projected back using the original inverted worldtrans
160 if (!m_childCollisionAlgorithms[index])
161 {
162 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
163 }
164 algo = m_childCollisionAlgorithms[index];
165 }
166
167 const btCollisionObjectWrapper* tmpWrap = 0;
168
169 ///detect swapping case
170 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
171 {
172 tmpWrap = m_resultOut->getBody0Wrap();
173 m_resultOut->setBody0Wrap(&compoundWrap);
174 m_resultOut->setShapeIdentifiersA(-1, index);
175 }
176 else
177 {
178 tmpWrap = m_resultOut->getBody1Wrap();
179 m_resultOut->setBody1Wrap(&compoundWrap);
180 m_resultOut->setShapeIdentifiersB(-1, index);
181 }
182
183 algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
184
185 #if 0
186 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
187 {
188 btVector3 worldAabbMin,worldAabbMax;
189 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
190 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
191 }
192 #endif
193
194 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
195 {
196 m_resultOut->setBody0Wrap(tmpWrap);
197 }
198 else
199 {
200 m_resultOut->setBody1Wrap(tmpWrap);
201 }
202 if (allocatedAlgorithm)
203 {
204 algo->~btCollisionAlgorithm();
205 m_dispatcher->freeCollisionAlgorithm(algo);
206 }
207 }
208 }
ProcessbtCompoundLeafCallback209 void Process(const btDbvtNode* leaf)
210 {
211 int index = leaf->dataAsInt;
212
213 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
214 const btCollisionShape* childShape = compoundShape->getChildShape(index);
215
216 #if 0
217 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
218 {
219 btVector3 worldAabbMin,worldAabbMax;
220 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
221 btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
222 m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
223 }
224 #endif
225
226 ProcessChildShape(childShape, index);
227 }
228 };
229
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)230 void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
231 {
232 const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
233 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
234
235 btAssert(colObjWrap->getCollisionShape()->isCompound());
236 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
237
238 ///btCompoundShape might have changed:
239 ////make sure the internal child collision algorithm caches are still valid
240 if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
241 {
242 ///clear and update all
243 removeChildAlgorithms();
244
245 preallocateChildAlgorithms(body0Wrap, body1Wrap);
246 m_compoundShapeRevision = compoundShape->getUpdateRevision();
247 }
248
249 if (m_childCollisionAlgorithms.size() == 0)
250 return;
251
252 const btDbvt* tree = compoundShape->getDynamicAabbTree();
253 //use a dynamic aabb tree to cull potential child-overlaps
254 btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
255
256 ///we need to refresh all contact manifolds
257 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
258 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
259 {
260 int i;
261 manifoldArray.resize(0);
262 for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
263 {
264 if (m_childCollisionAlgorithms[i])
265 {
266 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
267 for (int m = 0; m < manifoldArray.size(); m++)
268 {
269 if (manifoldArray[m]->getNumContacts())
270 {
271 resultOut->setPersistentManifold(manifoldArray[m]);
272 resultOut->refreshContactPoints();
273 resultOut->setPersistentManifold(0); //??necessary?
274 }
275 }
276 manifoldArray.resize(0);
277 }
278 }
279 }
280
281 if (tree)
282 {
283 btVector3 localAabbMin, localAabbMax;
284 btTransform otherInCompoundSpace;
285 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
286 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
287 btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
288 localAabbMin -= extraExtends;
289 localAabbMax += extraExtends;
290
291 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
292 //process all children, that overlap with the given AABB bounds
293 tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
294 }
295 else
296 {
297 //iterate over all children, perform an AABB check inside ProcessChildShape
298 int numChildren = m_childCollisionAlgorithms.size();
299 int i;
300 for (i = 0; i < numChildren; i++)
301 {
302 callback.ProcessChildShape(compoundShape->getChildShape(i), i);
303 }
304 }
305
306 {
307 //iterate over all children, perform an AABB check inside ProcessChildShape
308 int numChildren = m_childCollisionAlgorithms.size();
309 int i;
310 manifoldArray.resize(0);
311 const btCollisionShape* childShape = 0;
312 btTransform orgTrans;
313
314 btTransform newChildWorldTrans;
315 btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
316
317 for (i = 0; i < numChildren; i++)
318 {
319 if (m_childCollisionAlgorithms[i])
320 {
321 childShape = compoundShape->getChildShape(i);
322 //if not longer overlapping, remove the algorithm
323 orgTrans = colObjWrap->getWorldTransform();
324
325 const btTransform& childTrans = compoundShape->getChildTransform(i);
326 newChildWorldTrans = orgTrans * childTrans;
327
328 //perform an AABB check first
329 childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
330 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
331
332 if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
333 {
334 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
335 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
336 m_childCollisionAlgorithms[i] = 0;
337 }
338 }
339 }
340 }
341 }
342
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)343 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
344 {
345 btAssert(0);
346 //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
347 btCollisionObject* colObj = m_isSwapped ? body1 : body0;
348 btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
349
350 btAssert(colObj->getCollisionShape()->isCompound());
351
352 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
353
354 //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
355 //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
356 //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
357 //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
358 //then use each overlapping node AABB against Tree0
359 //and vise versa.
360
361 btScalar hitFraction = btScalar(1.);
362
363 int numChildren = m_childCollisionAlgorithms.size();
364 int i;
365 btTransform orgTrans;
366 btScalar frac;
367 for (i = 0; i < numChildren; i++)
368 {
369 //btCollisionShape* childShape = compoundShape->getChildShape(i);
370
371 //backup
372 orgTrans = colObj->getWorldTransform();
373
374 const btTransform& childTrans = compoundShape->getChildTransform(i);
375 //btTransform newChildWorldTrans = orgTrans*childTrans ;
376 colObj->setWorldTransform(orgTrans * childTrans);
377
378 //btCollisionShape* tmpShape = colObj->getCollisionShape();
379 //colObj->internalSetTemporaryCollisionShape( childShape );
380 frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
381 if (frac < hitFraction)
382 {
383 hitFraction = frac;
384 }
385 //revert back
386 //colObj->internalSetTemporaryCollisionShape( tmpShape);
387 colObj->setWorldTransform(orgTrans);
388 }
389 return hitFraction;
390 }
391