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