Bullet Collision Detection & Physics Library
btCompoundCollisionAlgorithm.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14
15*/
16
23#include "btManifoldResult.h"
25
27
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());
40
41 preallocateChildAlgorithms(body0Wrap, body1Wrap);
42}
43
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 {
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
76{
77 int numChildren = m_childCollisionAlgorithms.size();
78 int i;
79 for (i = 0; i < numChildren; i++)
80 {
82 {
83 m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
84 m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
85 }
86 }
87}
88
93
95{
96public:
104
105 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
110 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
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 {
163 }
164 algo = m_childCollisionAlgorithms[index];
165 }
166
167 const btCollisionObjectWrapper* tmpWrap = 0;
168
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
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 }
209 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
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
240 if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
241 {
244
245 preallocateChildAlgorithms(body0Wrap, body1Wrap);
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
259 {
260 int i;
261 manifoldArray.resize(0);
262 for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
263 {
265 {
266 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
267 for (int m = 0; m < manifoldArray.size(); m++)
268 {
269 if (manifoldArray[m]->getNumContacts())
270 {
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);
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 {
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]);
337 }
338 }
339 }
340 }
341}
342
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
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}
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition btAabbUtil2.h:43
SIMD_FORCE_INLINE void btTransformAabb(const btVector3 &halfExtents, btScalar margin, const btTransform &t, btVector3 &aabbMinOut, btVector3 &aabbMaxOut)
btShapePairCallback gCompoundChildShapePairCallback
bool(* btShapePairCallback)(const btCollisionShape *pShape0, const btCollisionShape *pShape1)
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition btDbvt.cpp:299
btDbvtAabbMm btDbvtVolume
Definition btDbvt.h:177
@ BT_CLOSEST_POINT_ALGORITHMS
@ BT_CONTACT_POINT_ALGORITHMS
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition btScalar.h:99
#define btAssert(x)
Definition btScalar.h:153
btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
btCollisionObject can be used to manage collision detection objects.
btTransform & getWorldTransform()
void setWorldTransform(const btTransform &worldTrans)
const btCollisionShape * getCollisionShape() const
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
bool isCompound() const
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
class btPersistentManifold * m_sharedManifold
btAlignedObjectArray< btCollisionAlgorithm * > m_childCollisionAlgorithms
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
void preallocateChildAlgorithms(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap)
btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
The btCompoundShape allows to store multiple other btCollisionShapes This allows for moving concave c...
btCollisionShape * getChildShape(int index)
btTransform & getChildTransform(int index)
int getUpdateRevision() const
const btDbvt * getDynamicAabbTree() const
int getNumChildShapes() const
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
btManifoldResult is a helper class to manage contact results.
void setPersistentManifold(btPersistentManifold *manifoldPtr)
btScalar m_closestPointDistanceThreshold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
const btCollisionObjectWrapper * m_otherObjWrap
void ProcessChildShape(const btCollisionShape *childShape, int index)
btCompoundLeafCallback(const btCollisionObjectWrapper *compoundObjWrap, const btCollisionObjectWrapper *otherObjWrap, btDispatcher *dispatcher, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut, btCollisionAlgorithm **childCollisionAlgorithms, btPersistentManifold *sharedManifold)
const btDispatcherInfo & m_dispatchInfo
btCollisionAlgorithm ** m_childCollisionAlgorithms
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_compoundColObjWrap
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition btDbvt.h:479
DBVT_INLINE const btVector3 & Mins() const
Definition btDbvt.h:137
DBVT_INLINE const btVector3 & Maxs() const
Definition btDbvt.h:138
btDbvtVolume volume
Definition btDbvt.h:182
int dataAsInt
Definition btDbvt.h:189
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition btDbvt.h:229
DBVT_PREFIX void collideTVNoStackAlloc(const btDbvtNode *root, const btDbvtVolume &volume, btNodeStack &stack, DBVT_IPOLICY) const
Definition btDbvt.h:1189
btDbvtNode * m_root
Definition btDbvt.h:302