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