1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, -1, index);
143
144 btCollisionAlgorithm* algo = 0;
145 bool allocatedAlgorithm = false;
146
147 if (m_resultOut->m_closestPointDistanceThreshold > 0)
148 {
149 algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
150 allocatedAlgorithm = true;
151 }
152 else
153 {
154 //the contactpoint is still projected back using the original inverted worldtrans
155 if (!m_childCollisionAlgorithms[index])
156 {
157 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
158 }
159 algo = m_childCollisionAlgorithms[index];
160 }
161
162 const btCollisionObjectWrapper* tmpWrap = 0;
163
164 ///detect swapping case
165 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
166 {
167 tmpWrap = m_resultOut->getBody0Wrap();
168 m_resultOut->setBody0Wrap(&compoundWrap);
169 m_resultOut->setShapeIdentifiersA(-1, index);
170 }
171 else
172 {
173 tmpWrap = m_resultOut->getBody1Wrap();
174 m_resultOut->setBody1Wrap(&compoundWrap);
175 m_resultOut->setShapeIdentifiersB(-1, index);
176 }
177
178 algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
179
180 #if 0
181 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
182 {
183 btVector3 worldAabbMin,worldAabbMax;
184 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
185 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
186 }
187 #endif
188
189 if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
190 {
191 m_resultOut->setBody0Wrap(tmpWrap);
192 }
193 else
194 {
195 m_resultOut->setBody1Wrap(tmpWrap);
196 }
197 if (allocatedAlgorithm)
198 {
199 algo->~btCollisionAlgorithm();
200 m_dispatcher->freeCollisionAlgorithm(algo);
201 }
202 }
203 }
ProcessbtCompoundLeafCallback204 void Process(const btDbvtNode* leaf)
205 {
206 int index = leaf->dataAsInt;
207
208 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
209 const btCollisionShape* childShape = compoundShape->getChildShape(index);
210
211 #if 0
212 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
213 {
214 btVector3 worldAabbMin,worldAabbMax;
215 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
216 btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
217 m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
218 }
219 #endif
220
221 ProcessChildShape(childShape, index);
222 }
223 };
224
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)225 void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
226 {
227 const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
228 const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
229
230 btAssert(colObjWrap->getCollisionShape()->isCompound());
231 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
232
233 ///btCompoundShape might have changed:
234 ////make sure the internal child collision algorithm caches are still valid
235 if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
236 {
237 ///clear and update all
238 removeChildAlgorithms();
239
240 preallocateChildAlgorithms(body0Wrap, body1Wrap);
241 m_compoundShapeRevision = compoundShape->getUpdateRevision();
242 }
243
244 if (m_childCollisionAlgorithms.size() == 0)
245 return;
246
247 const btDbvt* tree = compoundShape->getDynamicAabbTree();
248 //use a dynamic aabb tree to cull potential child-overlaps
249 btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
250
251 ///we need to refresh all contact manifolds
252 ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
253 ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
254 {
255 int i;
256 manifoldArray.resize(0);
257 for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
258 {
259 if (m_childCollisionAlgorithms[i])
260 {
261 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
262 for (int m = 0; m < manifoldArray.size(); m++)
263 {
264 if (manifoldArray[m]->getNumContacts())
265 {
266 resultOut->setPersistentManifold(manifoldArray[m]);
267 resultOut->refreshContactPoints();
268 resultOut->setPersistentManifold(0); //??necessary?
269 }
270 }
271 manifoldArray.resize(0);
272 }
273 }
274 }
275
276 if (tree)
277 {
278 btVector3 localAabbMin, localAabbMax;
279 btTransform otherInCompoundSpace;
280 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
281 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
282 btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
283 localAabbMin -= extraExtends;
284 localAabbMax += extraExtends;
285
286 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
287 //process all children, that overlap with the given AABB bounds
288 tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
289 }
290 else
291 {
292 //iterate over all children, perform an AABB check inside ProcessChildShape
293 int numChildren = m_childCollisionAlgorithms.size();
294 int i;
295 for (i = 0; i < numChildren; i++)
296 {
297 callback.ProcessChildShape(compoundShape->getChildShape(i), i);
298 }
299 }
300
301 {
302 //iterate over all children, perform an AABB check inside ProcessChildShape
303 int numChildren = m_childCollisionAlgorithms.size();
304 int i;
305 manifoldArray.resize(0);
306 const btCollisionShape* childShape = 0;
307 btTransform orgTrans;
308
309 btTransform newChildWorldTrans;
310 btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
311
312 for (i = 0; i < numChildren; i++)
313 {
314 if (m_childCollisionAlgorithms[i])
315 {
316 childShape = compoundShape->getChildShape(i);
317 //if not longer overlapping, remove the algorithm
318 orgTrans = colObjWrap->getWorldTransform();
319
320 const btTransform& childTrans = compoundShape->getChildTransform(i);
321 newChildWorldTrans = orgTrans * childTrans;
322
323 //perform an AABB check first
324 childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
325 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
326
327 if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
328 {
329 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
330 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
331 m_childCollisionAlgorithms[i] = 0;
332 }
333 }
334 }
335 }
336 }
337
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)338 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
339 {
340 btAssert(0);
341 //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
342 btCollisionObject* colObj = m_isSwapped ? body1 : body0;
343 btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
344
345 btAssert(colObj->getCollisionShape()->isCompound());
346
347 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
348
349 //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
350 //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
351 //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
352 //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
353 //then use each overlapping node AABB against Tree0
354 //and vise versa.
355
356 btScalar hitFraction = btScalar(1.);
357
358 int numChildren = m_childCollisionAlgorithms.size();
359 int i;
360 btTransform orgTrans;
361 btScalar frac;
362 for (i = 0; i < numChildren; i++)
363 {
364 //btCollisionShape* childShape = compoundShape->getChildShape(i);
365
366 //backup
367 orgTrans = colObj->getWorldTransform();
368
369 const btTransform& childTrans = compoundShape->getChildTransform(i);
370 //btTransform newChildWorldTrans = orgTrans*childTrans ;
371 colObj->setWorldTransform(orgTrans * childTrans);
372
373 //btCollisionShape* tmpShape = colObj->getCollisionShape();
374 //colObj->internalSetTemporaryCollisionShape( childShape );
375 frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
376 if (frac < hitFraction)
377 {
378 hitFraction = frac;
379 }
380 //revert back
381 //colObj->internalSetTemporaryCollisionShape( tmpShape);
382 colObj->setWorldTransform(orgTrans);
383 }
384 return hitFraction;
385 }
386