Bullet Collision Detection & Physics Library
btCompoundCollisionAlgorithm.cpp
Go to the documentation of this file.
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 
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 
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 
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 
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 
108  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 
117  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  btTransform orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
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 
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  }
189  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 
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 
226  if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
227  {
229  removeChildAlgorithms();
230 
231  preallocateChildAlgorithms(body0Wrap,body1Wrap);
232  }
233 
234 
235  const btDbvt* tree = compoundShape->getDynamicAabbTree();
236  //use a dynamic aabb tree to cull potential child-overlaps
237  btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
238 
242  {
243  int i;
244  btManifoldArray manifoldArray;
245  for (i=0;i<m_childCollisionAlgorithms.size();i++)
246  {
247  if (m_childCollisionAlgorithms[i])
248  {
249  m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
250  for (int m=0;m<manifoldArray.size();m++)
251  {
252  if (manifoldArray[m]->getNumContacts())
253  {
254  resultOut->setPersistentManifold(manifoldArray[m]);
255  resultOut->refreshContactPoints();
256  resultOut->setPersistentManifold(0);//??necessary?
257  }
258  }
259  manifoldArray.resize(0);
260  }
261  }
262  }
263 
264  if (tree)
265  {
266 
267  btVector3 localAabbMin,localAabbMax;
268  btTransform otherInCompoundSpace;
269  otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
270  otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
271 
272  const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
273  //process all children, that overlap with the given AABB bounds
274  tree->collideTV(tree->m_root,bounds,callback);
275 
276  } else
277  {
278  //iterate over all children, perform an AABB check inside ProcessChildShape
279  int numChildren = m_childCollisionAlgorithms.size();
280  int i;
281  for (i=0;i<numChildren;i++)
282  {
283  callback.ProcessChildShape(compoundShape->getChildShape(i),i);
284  }
285  }
286 
287  {
288  //iterate over all children, perform an AABB check inside ProcessChildShape
289  int numChildren = m_childCollisionAlgorithms.size();
290  int i;
291  btManifoldArray manifoldArray;
292  const btCollisionShape* childShape = 0;
293  btTransform orgTrans;
294  btTransform orgInterpolationTrans;
295  btTransform newChildWorldTrans;
296  btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
297 
298  for (i=0;i<numChildren;i++)
299  {
300  if (m_childCollisionAlgorithms[i])
301  {
302  childShape = compoundShape->getChildShape(i);
303  //if not longer overlapping, remove the algorithm
304  orgTrans = colObjWrap->getWorldTransform();
305  orgInterpolationTrans = colObjWrap->getWorldTransform();
306  const btTransform& childTrans = compoundShape->getChildTransform(i);
307  newChildWorldTrans = orgTrans*childTrans ;
308 
309  //perform an AABB check first
310  childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
311  otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
312 
313  if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
314  {
315  m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
316  m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
317  m_childCollisionAlgorithms[i] = 0;
318  }
319  }
320  }
321  }
322 }
323 
324 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
325 {
326  btAssert(0);
327  //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
328  btCollisionObject* colObj = m_isSwapped? body1 : body0;
329  btCollisionObject* otherObj = m_isSwapped? body0 : body1;
330 
331  btAssert (colObj->getCollisionShape()->isCompound());
332 
333  btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
334 
335  //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
336  //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
337  //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
338  //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
339  //then use each overlapping node AABB against Tree0
340  //and vise versa.
341 
342  btScalar hitFraction = btScalar(1.);
343 
344  int numChildren = m_childCollisionAlgorithms.size();
345  int i;
346  btTransform orgTrans;
347  btScalar frac;
348  for (i=0;i<numChildren;i++)
349  {
350  //btCollisionShape* childShape = compoundShape->getChildShape(i);
351 
352  //backup
353  orgTrans = colObj->getWorldTransform();
354 
355  const btTransform& childTrans = compoundShape->getChildTransform(i);
356  //btTransform newChildWorldTrans = orgTrans*childTrans ;
357  colObj->setWorldTransform( orgTrans*childTrans );
358 
359  //btCollisionShape* tmpShape = colObj->getCollisionShape();
360  //colObj->internalSetTemporaryCollisionShape( childShape );
361  frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
362  if (frac<hitFraction)
363  {
364  hitFraction = frac;
365  }
366  //revert back
367  //colObj->internalSetTemporaryCollisionShape( tmpShape);
368  colObj->setWorldTransform( orgTrans);
369  }
370  return hitFraction;
371 
372 }
373 
374 
375 
Generated on Wed Apr 9 2014 02:38:22 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2