Bullet Collision Detection & Physics Library
btCollisionWorld.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 #include "btCollisionWorld.h"
17 #include "btCollisionDispatcher.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
21 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
22 #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
23 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
24 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
25 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
26 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
27 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
28 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
29 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
32 #include "LinearMath/btAabbUtil2.h"
33 #include "LinearMath/btQuickprof.h"
34 #include "LinearMath/btSerializer.h"
35 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
36 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
37 #include "BulletCollision/Gimpact/btGImpactShape.h"
38 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
39 
40 
41 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
42 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest
43 //#define RECALCULATE_AABB_RAYCAST 1
44 
45 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
46 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
47 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
48 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
49 
50 
52 
53 //for debug rendering
54 #include "BulletCollision/CollisionShapes/btBoxShape.h"
55 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
56 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
57 #include "BulletCollision/CollisionShapes/btConeShape.h"
58 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
59 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
60 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
61 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
62 #include "BulletCollision/CollisionShapes/btSphereShape.h"
63 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
64 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
65 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
66 
67 
68 
69 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
70 :m_dispatcher1(dispatcher),
71 m_broadphasePairCache(pairCache),
72 m_debugDrawer(0),
73 m_forceUpdateAllAabbs(true)
74 {
75 }
76 
77 
78 btCollisionWorld::~btCollisionWorld()
79 {
80 
81  //clean up remaining objects
82  int i;
83  for (i=0;i<m_collisionObjects.size();i++)
84  {
85  btCollisionObject* collisionObject= m_collisionObjects[i];
86 
87  btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
88  if (bp)
89  {
90  //
91  // only clear the cached algorithms
92  //
93  getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
94  getBroadphase()->destroyProxy(bp,m_dispatcher1);
95  collisionObject->setBroadphaseHandle(0);
96  }
97  }
98 
99 
100 }
101 
102 
103 
104 
105 
106 
107 
108 
109 
110 
111 void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
112 {
113 
114  btAssert(collisionObject);
115 
116  //check that the object isn't already added
117  btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
118 
119  m_collisionObjects.push_back(collisionObject);
120 
121  //calculate new AABB
122  btTransform trans = collisionObject->getWorldTransform();
123 
124  btVector3 minAabb;
125  btVector3 maxAabb;
126  collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
127 
128  int type = collisionObject->getCollisionShape()->getShapeType();
129  collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
130  minAabb,
131  maxAabb,
132  type,
133  collisionObject,
134  collisionFilterGroup,
135  collisionFilterMask,
136  m_dispatcher1,0
137  )) ;
138 
139 
140 
141 
142 
143 }
144 
145 
146 
147 void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
148 {
149  btVector3 minAabb,maxAabb;
150  colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
151  //need to increase the aabb for contact thresholds
152  btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
153  minAabb -= contactThreshold;
154  maxAabb += contactThreshold;
155 
156  if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
157  {
158  btVector3 minAabb2,maxAabb2;
159  colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
160  minAabb2 -= contactThreshold;
161  maxAabb2 += contactThreshold;
162  minAabb.setMin(minAabb2);
163  maxAabb.setMax(maxAabb2);
164  }
165 
166  btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
167 
168  //moving objects should be moderately sized, probably something wrong if not
169  if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
170  {
171  bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
172  } else
173  {
174  //something went wrong, investigate
175  //this assert is unwanted in 3D modelers (danger of loosing work)
176  colObj->setActivationState(DISABLE_SIMULATION);
177 
178  static bool reportMe = true;
179  if (reportMe && m_debugDrawer)
180  {
181  reportMe = false;
182  m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
183  m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
184  m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
185  m_debugDrawer->reportErrorWarning("Thanks.\n");
186  }
187  }
188 }
189 
190 void btCollisionWorld::updateAabbs()
191 {
192  BT_PROFILE("updateAabbs");
193 
194  btTransform predictedTrans;
195  for ( int i=0;i<m_collisionObjects.size();i++)
196  {
197  btCollisionObject* colObj = m_collisionObjects[i];
198 
199  //only update aabb of active objects
200  if (m_forceUpdateAllAabbs || colObj->isActive())
201  {
202  updateSingleAabb(colObj);
203  }
204  }
205 }
206 
207 
208 void btCollisionWorld::computeOverlappingPairs()
209 {
210  BT_PROFILE("calculateOverlappingPairs");
211  m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
212 }
213 
214 void btCollisionWorld::performDiscreteCollisionDetection()
215 {
216  BT_PROFILE("performDiscreteCollisionDetection");
217 
218  btDispatcherInfo& dispatchInfo = getDispatchInfo();
219 
220  updateAabbs();
221 
222  computeOverlappingPairs();
223 
224  btDispatcher* dispatcher = getDispatcher();
225  {
226  BT_PROFILE("dispatchAllCollisionPairs");
227  if (dispatcher)
228  dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
229  }
230 
231 }
232 
233 
234 
235 void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
236 {
237 
238 
239  //bool removeFromBroadphase = false;
240 
241  {
242 
243  btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
244  if (bp)
245  {
246  //
247  // only clear the cached algorithms
248  //
249  getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
250  getBroadphase()->destroyProxy(bp,m_dispatcher1);
251  collisionObject->setBroadphaseHandle(0);
252  }
253  }
254 
255 
256  //swapremove
257  m_collisionObjects.remove(collisionObject);
258 
259 }
260 
261 
262 void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
263  btCollisionObject* collisionObject,
264  const btCollisionShape* collisionShape,
265  const btTransform& colObjWorldTransform,
266  RayResultCallback& resultCallback)
267 {
268  btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
269  btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback);
270 }
271 
272 void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
273  const btCollisionObjectWrapper* collisionObjectWrap,
274  RayResultCallback& resultCallback)
275 {
276  btSphereShape pointShape(btScalar(0.0));
277  pointShape.setMargin(0.f);
278  const btConvexShape* castShape = &pointShape;
279  const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape();
280  const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform();
281 
282  if (collisionShape->isConvex())
283  {
284  // BT_PROFILE("rayTestConvex");
285  btConvexCast::CastResult castResult;
286  castResult.m_fraction = resultCallback.m_closestHitFraction;
287 
288  btConvexShape* convexShape = (btConvexShape*) collisionShape;
289  btVoronoiSimplexSolver simplexSolver;
290  btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver);
291 
292  btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver);
293 
294  //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
295  bool condition = true;
296  btConvexCast* convexCasterPtr = 0;
297  if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
298  convexCasterPtr = &subSimplexConvexCaster;
299  else
300  convexCasterPtr = &gjkConvexCaster;
301 
302  btConvexCast& convexCaster = *convexCasterPtr;
303 
304  if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
305  {
306  //add hit
307  if (castResult.m_normal.length2() > btScalar(0.0001))
308  {
309  if (castResult.m_fraction < resultCallback.m_closestHitFraction)
310  {
311 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
312  //rotate normal into worldspace
313  castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
314 #endif //USE_SUBSIMPLEX_CONVEX_CAST
315 
316  castResult.m_normal.normalize();
317  btCollisionWorld::LocalRayResult localRayResult
318  (
319  collisionObjectWrap->getCollisionObject(),
320  0,
321  castResult.m_normal,
322  castResult.m_fraction
323  );
324 
325  bool normalInWorldSpace = true;
326  resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
327 
328  }
329  }
330  }
331  } else {
332  if (collisionShape->isConcave())
333  {
334 
335  //ConvexCast::CastResult
336  struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
337  {
338  btCollisionWorld::RayResultCallback* m_resultCallback;
339  const btCollisionObject* m_collisionObject;
340  const btConcaveShape* m_triangleMesh;
341 
342  btTransform m_colObjWorldTransform;
343 
344  BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
345  btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform):
346  //@BP Mod
347  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
348  m_resultCallback(resultCallback),
349  m_collisionObject(collisionObject),
350  m_triangleMesh(triangleMesh),
351  m_colObjWorldTransform(colObjWorldTransform)
352  {
353  }
354 
355 
356  virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
357  {
358  btCollisionWorld::LocalShapeInfo shapeInfo;
359  shapeInfo.m_shapePart = partId;
360  shapeInfo.m_triangleIndex = triangleIndex;
361 
362  btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
363 
364  btCollisionWorld::LocalRayResult rayResult
365  (m_collisionObject,
366  &shapeInfo,
367  hitNormalWorld,
368  hitFraction);
369 
370  bool normalInWorldSpace = true;
371  return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
372  }
373 
374  };
375 
376  btTransform worldTocollisionObject = colObjWorldTransform.inverse();
377  btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
378  btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
379 
380  // BT_PROFILE("rayTestConcave");
381  if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
382  {
384  btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
385 
386  BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
387  rcb.m_hitFraction = resultCallback.m_closestHitFraction;
388  triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
389  }
390  else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
391  {
392  btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape;
393 
394  BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
395  rcb.m_hitFraction = resultCallback.m_closestHitFraction;
396  concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal);
397  }else
398  {
399  //generic (slower) case
400  btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
401 
402  btTransform worldTocollisionObject = colObjWorldTransform.inverse();
403 
404  btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
405  btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
406 
407  //ConvexCast::CastResult
408 
409  struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
410  {
411  btCollisionWorld::RayResultCallback* m_resultCallback;
412  const btCollisionObject* m_collisionObject;
413  btConcaveShape* m_triangleMesh;
414 
415  btTransform m_colObjWorldTransform;
416 
417  BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
418  btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
419  //@BP Mod
420  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
421  m_resultCallback(resultCallback),
422  m_collisionObject(collisionObject),
423  m_triangleMesh(triangleMesh),
424  m_colObjWorldTransform(colObjWorldTransform)
425  {
426  }
427 
428 
429  virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
430  {
431  btCollisionWorld::LocalShapeInfo shapeInfo;
432  shapeInfo.m_shapePart = partId;
433  shapeInfo.m_triangleIndex = triangleIndex;
434 
435  btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
436 
437  btCollisionWorld::LocalRayResult rayResult
438  (m_collisionObject,
439  &shapeInfo,
440  hitNormalWorld,
441  hitFraction);
442 
443  bool normalInWorldSpace = true;
444  return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
445  }
446 
447  };
448 
449 
450  BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
451  rcb.m_hitFraction = resultCallback.m_closestHitFraction;
452 
453  btVector3 rayAabbMinLocal = rayFromLocal;
454  rayAabbMinLocal.setMin(rayToLocal);
455  btVector3 rayAabbMaxLocal = rayFromLocal;
456  rayAabbMaxLocal.setMax(rayToLocal);
457 
458  concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
459  }
460  } else {
461  // BT_PROFILE("rayTestCompound");
462  if (collisionShape->isCompound())
463  {
464  struct LocalInfoAdder2 : public RayResultCallback
465  {
466  RayResultCallback* m_userCallback;
467  int m_i;
468 
469  LocalInfoAdder2 (int i, RayResultCallback *user)
470  : m_userCallback(user), m_i(i)
471  {
472  m_closestHitFraction = m_userCallback->m_closestHitFraction;
473  m_flags = m_userCallback->m_flags;
474  }
475  virtual bool needsCollision(btBroadphaseProxy* p) const
476  {
477  return m_userCallback->needsCollision(p);
478  }
479 
480  virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
481  {
482  btCollisionWorld::LocalShapeInfo shapeInfo;
483  shapeInfo.m_shapePart = -1;
484  shapeInfo.m_triangleIndex = m_i;
485  if (r.m_localShapeInfo == NULL)
486  r.m_localShapeInfo = &shapeInfo;
487 
488  const btScalar result = m_userCallback->addSingleResult(r, b);
489  m_closestHitFraction = m_userCallback->m_closestHitFraction;
490  return result;
491  }
492  };
493 
494  struct RayTester : btDbvt::ICollide
495  {
496  const btCollisionObject* m_collisionObject;
497  const btCompoundShape* m_compoundShape;
498  const btTransform& m_colObjWorldTransform;
499  const btTransform& m_rayFromTrans;
500  const btTransform& m_rayToTrans;
501  RayResultCallback& m_resultCallback;
502 
503  RayTester(const btCollisionObject* collisionObject,
504  const btCompoundShape* compoundShape,
505  const btTransform& colObjWorldTransform,
506  const btTransform& rayFromTrans,
507  const btTransform& rayToTrans,
508  RayResultCallback& resultCallback):
509  m_collisionObject(collisionObject),
510  m_compoundShape(compoundShape),
511  m_colObjWorldTransform(colObjWorldTransform),
512  m_rayFromTrans(rayFromTrans),
513  m_rayToTrans(rayToTrans),
514  m_resultCallback(resultCallback)
515  {
516 
517  }
518 
519  void ProcessLeaf(int i)
520  {
521  const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
522  const btTransform& childTrans = m_compoundShape->getChildTransform(i);
523  btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
524 
525  btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i);
526  // replace collision shape so that callback can determine the triangle
527 
528 
529 
530  LocalInfoAdder2 my_cb(i, &m_resultCallback);
531 
532  rayTestSingleInternal(
533  m_rayFromTrans,
534  m_rayToTrans,
535  &tmpOb,
536  my_cb);
537 
538  }
539 
540  void Process(const btDbvtNode* leaf)
541  {
542  ProcessLeaf(leaf->dataAsInt);
543  }
544  };
545 
546  const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
547  const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
548 
549 
550  RayTester rayCB(
551  collisionObjectWrap->getCollisionObject(),
552  compoundShape,
553  colObjWorldTransform,
554  rayFromTrans,
555  rayToTrans,
556  resultCallback);
557 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
558  if (dbvt)
559  {
560  btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
561  btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
562  btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
563  }
564  else
565 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
566  {
567  for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
568  {
569  rayCB.ProcessLeaf(i);
570  }
571  }
572  }
573  }
574  }
575 }
576 
577 void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
578  btCollisionObject* collisionObject,
579  const btCollisionShape* collisionShape,
580  const btTransform& colObjWorldTransform,
581  ConvexResultCallback& resultCallback, btScalar allowedPenetration)
582 {
583  btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
584  btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration);
585 }
586 
587 void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
588  const btCollisionObjectWrapper* colObjWrap,
589  ConvexResultCallback& resultCallback, btScalar allowedPenetration)
590 {
591  const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
592  const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
593 
594  if (collisionShape->isConvex())
595  {
596  //BT_PROFILE("convexSweepConvex");
597  btConvexCast::CastResult castResult;
598  castResult.m_allowedPenetration = allowedPenetration;
599  castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
600 
601  btConvexShape* convexShape = (btConvexShape*) collisionShape;
602  btVoronoiSimplexSolver simplexSolver;
603  btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
604 
605  btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
606  //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
607  //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
608 
609  btConvexCast* castPtr = &convexCaster1;
610 
611 
612 
613  if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
614  {
615  //add hit
616  if (castResult.m_normal.length2() > btScalar(0.0001))
617  {
618  if (castResult.m_fraction < resultCallback.m_closestHitFraction)
619  {
620  castResult.m_normal.normalize();
621  btCollisionWorld::LocalConvexResult localConvexResult
622  (
623  colObjWrap->getCollisionObject(),
624  0,
625  castResult.m_normal,
626  castResult.m_hitPoint,
627  castResult.m_fraction
628  );
629 
630  bool normalInWorldSpace = true;
631  resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
632 
633  }
634  }
635  }
636  } else {
637  if (collisionShape->isConcave())
638  {
639  if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
640  {
641  //BT_PROFILE("convexSweepbtBvhTriangleMesh");
642  btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
643  btTransform worldTocollisionObject = colObjWorldTransform.inverse();
644  btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
645  btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
646  // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
647  btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
648 
649  //ConvexCast::CastResult
650  struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
651  {
652  btCollisionWorld::ConvexResultCallback* m_resultCallback;
653  const btCollisionObject* m_collisionObject;
654  btTriangleMeshShape* m_triangleMesh;
655 
656  BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
657  btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
658  btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
659  m_resultCallback(resultCallback),
660  m_collisionObject(collisionObject),
661  m_triangleMesh(triangleMesh)
662  {
663  }
664 
665 
666  virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
667  {
668  btCollisionWorld::LocalShapeInfo shapeInfo;
669  shapeInfo.m_shapePart = partId;
670  shapeInfo.m_triangleIndex = triangleIndex;
671  if (hitFraction <= m_resultCallback->m_closestHitFraction)
672  {
673 
674  btCollisionWorld::LocalConvexResult convexResult
675  (m_collisionObject,
676  &shapeInfo,
677  hitNormalLocal,
678  hitPointLocal,
679  hitFraction);
680 
681  bool normalInWorldSpace = true;
682 
683 
684  return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
685  }
686  return hitFraction;
687  }
688 
689  };
690 
691  BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform);
692  tccb.m_hitFraction = resultCallback.m_closestHitFraction;
693  tccb.m_allowedPenetration = allowedPenetration;
694  btVector3 boxMinLocal, boxMaxLocal;
695  castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
696  triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
697  } else
698  {
699  if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
700  {
701  btConvexCast::CastResult castResult;
702  castResult.m_allowedPenetration = allowedPenetration;
703  castResult.m_fraction = resultCallback.m_closestHitFraction;
704  btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
705  btContinuousConvexCollision convexCaster1(castShape,planeShape);
706  btConvexCast* castPtr = &convexCaster1;
707 
708  if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
709  {
710  //add hit
711  if (castResult.m_normal.length2() > btScalar(0.0001))
712  {
713  if (castResult.m_fraction < resultCallback.m_closestHitFraction)
714  {
715  castResult.m_normal.normalize();
716  btCollisionWorld::LocalConvexResult localConvexResult
717  (
718  colObjWrap->getCollisionObject(),
719  0,
720  castResult.m_normal,
721  castResult.m_hitPoint,
722  castResult.m_fraction
723  );
724 
725  bool normalInWorldSpace = true;
726  resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
727  }
728  }
729  }
730 
731  } else
732  {
733  //BT_PROFILE("convexSweepConcave");
734  btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
735  btTransform worldTocollisionObject = colObjWorldTransform.inverse();
736  btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
737  btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
738  // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
739  btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
740 
741  //ConvexCast::CastResult
742  struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
743  {
744  btCollisionWorld::ConvexResultCallback* m_resultCallback;
745  const btCollisionObject* m_collisionObject;
746  btConcaveShape* m_triangleMesh;
747 
748  BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
749  btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
750  btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
751  m_resultCallback(resultCallback),
752  m_collisionObject(collisionObject),
753  m_triangleMesh(triangleMesh)
754  {
755  }
756 
757 
758  virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
759  {
760  btCollisionWorld::LocalShapeInfo shapeInfo;
761  shapeInfo.m_shapePart = partId;
762  shapeInfo.m_triangleIndex = triangleIndex;
763  if (hitFraction <= m_resultCallback->m_closestHitFraction)
764  {
765 
766  btCollisionWorld::LocalConvexResult convexResult
767  (m_collisionObject,
768  &shapeInfo,
769  hitNormalLocal,
770  hitPointLocal,
771  hitFraction);
772 
773  bool normalInWorldSpace = false;
774 
775  return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
776  }
777  return hitFraction;
778  }
779 
780  };
781 
782  BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
783  tccb.m_hitFraction = resultCallback.m_closestHitFraction;
784  tccb.m_allowedPenetration = allowedPenetration;
785  btVector3 boxMinLocal, boxMaxLocal;
786  castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
787 
788  btVector3 rayAabbMinLocal = convexFromLocal;
789  rayAabbMinLocal.setMin(convexToLocal);
790  btVector3 rayAabbMaxLocal = convexFromLocal;
791  rayAabbMaxLocal.setMax(convexToLocal);
792  rayAabbMinLocal += boxMinLocal;
793  rayAabbMaxLocal += boxMaxLocal;
794  concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
795  }
796  }
797  } else {
799  if (collisionShape->isCompound())
800  {
801  BT_PROFILE("convexSweepCompound");
802  const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
803  int i=0;
804  for (i=0;i<compoundShape->getNumChildShapes();i++)
805  {
806  btTransform childTrans = compoundShape->getChildTransform(i);
807  const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
808  btTransform childWorldTrans = colObjWorldTransform * childTrans;
809 
810  struct LocalInfoAdder : public ConvexResultCallback {
811  ConvexResultCallback* m_userCallback;
812  int m_i;
813 
814  LocalInfoAdder (int i, ConvexResultCallback *user)
815  : m_userCallback(user), m_i(i)
816  {
817  m_closestHitFraction = m_userCallback->m_closestHitFraction;
818  }
819  virtual bool needsCollision(btBroadphaseProxy* p) const
820  {
821  return m_userCallback->needsCollision(p);
822  }
823  virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
824  {
825  btCollisionWorld::LocalShapeInfo shapeInfo;
826  shapeInfo.m_shapePart = -1;
827  shapeInfo.m_triangleIndex = m_i;
828  if (r.m_localShapeInfo == NULL)
829  r.m_localShapeInfo = &shapeInfo;
830  const btScalar result = m_userCallback->addSingleResult(r, b);
831  m_closestHitFraction = m_userCallback->m_closestHitFraction;
832  return result;
833 
834  }
835  };
836 
837  LocalInfoAdder my_cb(i, &resultCallback);
838 
839  btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i);
840 
841  objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
842  &tmpObj,my_cb, allowedPenetration);
843 
844  }
845  }
846  }
847  }
848 }
849 
850 
851 struct btSingleRayCallback : public btBroadphaseRayCallback
852 {
853 
854  btVector3 m_rayFromWorld;
855  btVector3 m_rayToWorld;
856  btTransform m_rayFromTrans;
857  btTransform m_rayToTrans;
858  btVector3 m_hitNormal;
859 
860  const btCollisionWorld* m_world;
861  btCollisionWorld::RayResultCallback& m_resultCallback;
862 
863  btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
864  :m_rayFromWorld(rayFromWorld),
865  m_rayToWorld(rayToWorld),
866  m_world(world),
867  m_resultCallback(resultCallback)
868  {
869  m_rayFromTrans.setIdentity();
870  m_rayFromTrans.setOrigin(m_rayFromWorld);
871  m_rayToTrans.setIdentity();
872  m_rayToTrans.setOrigin(m_rayToWorld);
873 
874  btVector3 rayDir = (rayToWorld-rayFromWorld);
875 
876  rayDir.normalize ();
878  m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
879  m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
880  m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
881  m_signs[0] = m_rayDirectionInverse[0] < 0.0;
882  m_signs[1] = m_rayDirectionInverse[1] < 0.0;
883  m_signs[2] = m_rayDirectionInverse[2] < 0.0;
884 
885  m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
886 
887  }
888 
889 
890 
891  virtual bool process(const btBroadphaseProxy* proxy)
892  {
894  if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
895  return false;
896 
897  btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
898 
899  //only perform raycast if filterMask matches
900  if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
901  {
902  //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
903  //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
904 #if 0
905 #ifdef RECALCULATE_AABB
906  btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
907  collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
908 #else
909  //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
910  const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
911  const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
912 #endif
913 #endif
914  //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
915  //culling already done by broadphase
916  //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
917  {
918  m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
919  collisionObject,
920  collisionObject->getCollisionShape(),
921  collisionObject->getWorldTransform(),
922  m_resultCallback);
923  }
924  }
925  return true;
926  }
927 };
928 
929 void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
930 {
931  //BT_PROFILE("rayTest");
934  btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
935 
936 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
937  m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
938 #else
939  for (int i=0;i<this->getNumCollisionObjects();i++)
940  {
941  rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
942  }
943 #endif //USE_BRUTEFORCE_RAYBROADPHASE
944 
945 }
946 
947 
948 struct btSingleSweepCallback : public btBroadphaseRayCallback
949 {
950 
951  btTransform m_convexFromTrans;
952  btTransform m_convexToTrans;
953  btVector3 m_hitNormal;
954  const btCollisionWorld* m_world;
955  btCollisionWorld::ConvexResultCallback& m_resultCallback;
956  btScalar m_allowedCcdPenetration;
957  const btConvexShape* m_castShape;
958 
959 
960  btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
961  :m_convexFromTrans(convexFromTrans),
962  m_convexToTrans(convexToTrans),
963  m_world(world),
964  m_resultCallback(resultCallback),
965  m_allowedCcdPenetration(allowedPenetration),
966  m_castShape(castShape)
967  {
968  btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
969  btVector3 rayDir = unnormalizedRayDir.normalized();
971  m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
972  m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
973  m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
974  m_signs[0] = m_rayDirectionInverse[0] < 0.0;
975  m_signs[1] = m_rayDirectionInverse[1] < 0.0;
976  m_signs[2] = m_rayDirectionInverse[2] < 0.0;
977 
978  m_lambda_max = rayDir.dot(unnormalizedRayDir);
979 
980  }
981 
982  virtual bool process(const btBroadphaseProxy* proxy)
983  {
985  if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
986  return false;
987 
988  btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
989 
990  //only perform raycast if filterMask matches
991  if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
992  //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
993  m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
994  collisionObject,
995  collisionObject->getCollisionShape(),
996  collisionObject->getWorldTransform(),
997  m_resultCallback,
998  m_allowedCcdPenetration);
999  }
1000 
1001  return true;
1002  }
1003 };
1004 
1005 
1006 
1007 void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
1008 {
1009 
1010  BT_PROFILE("convexSweepTest");
1014 
1015 
1016 
1017  btTransform convexFromTrans,convexToTrans;
1018  convexFromTrans = convexFromWorld;
1019  convexToTrans = convexToWorld;
1020  btVector3 castShapeAabbMin, castShapeAabbMax;
1021  /* Compute AABB that encompasses angular movement */
1022  {
1023  btVector3 linVel, angVel;
1024  btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
1025  btVector3 zeroLinVel;
1026  zeroLinVel.setValue(0,0,0);
1027  btTransform R;
1028  R.setIdentity ();
1029  R.setRotation (convexFromTrans.getRotation());
1030  castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
1031  }
1032 
1033 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
1034 
1035  btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
1036 
1037  m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
1038 
1039 #else
1040 
1041  // do a ray-shape query using convexCaster (CCD)
1042  int i;
1043  for (i=0;i<m_collisionObjects.size();i++)
1044  {
1045  btCollisionObject* collisionObject= m_collisionObjects[i];
1046  //only perform raycast if filterMask matches
1047  if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
1048  //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1049  btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
1050  collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
1051  AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1052  btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1053  btVector3 hitNormal;
1054  if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
1055  {
1056  objectQuerySingle(castShape, convexFromTrans,convexToTrans,
1057  collisionObject,
1058  collisionObject->getCollisionShape(),
1059  collisionObject->getWorldTransform(),
1060  resultCallback,
1061  allowedCcdPenetration);
1062  }
1063  }
1064  }
1065 #endif //USE_BRUTEFORCE_RAYBROADPHASE
1066 }
1067 
1068 
1069 
1070 struct btBridgedManifoldResult : public btManifoldResult
1071 {
1072 
1073  btCollisionWorld::ContactResultCallback& m_resultCallback;
1074 
1075  btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback )
1076  :btManifoldResult(obj0Wrap,obj1Wrap),
1077  m_resultCallback(resultCallback)
1078  {
1079  }
1080 
1081  virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
1082  {
1083  bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
1084  btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1085  btVector3 localA;
1086  btVector3 localB;
1087  if (isSwapped)
1088  {
1089  localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1090  localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1091  } else
1092  {
1093  localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1094  localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1095  }
1096 
1097  btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
1098  newPt.m_positionWorldOnA = pointA;
1099  newPt.m_positionWorldOnB = pointInWorld;
1100 
1101  //BP mod, store contact triangles.
1102  if (isSwapped)
1103  {
1104  newPt.m_partId0 = m_partId1;
1105  newPt.m_partId1 = m_partId0;
1106  newPt.m_index0 = m_index1;
1107  newPt.m_index1 = m_index0;
1108  } else
1109  {
1110  newPt.m_partId0 = m_partId0;
1111  newPt.m_partId1 = m_partId1;
1112  newPt.m_index0 = m_index0;
1113  newPt.m_index1 = m_index1;
1114  }
1115 
1116  //experimental feature info, for per-triangle material etc.
1117  const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
1118  const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
1119  m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
1120 
1121  }
1122 
1123 };
1124 
1125 
1126 
1127 struct btSingleContactCallback : public btBroadphaseAabbCallback
1128 {
1129 
1130  btCollisionObject* m_collisionObject;
1131  btCollisionWorld* m_world;
1132  btCollisionWorld::ContactResultCallback& m_resultCallback;
1133 
1134 
1135  btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
1136  :m_collisionObject(collisionObject),
1137  m_world(world),
1138  m_resultCallback(resultCallback)
1139  {
1140  }
1141 
1142  virtual bool process(const btBroadphaseProxy* proxy)
1143  {
1144  btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1145  if (collisionObject == m_collisionObject)
1146  return true;
1147 
1148  //only perform raycast if filterMask matches
1149  if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1150  {
1151  btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
1152  btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
1153 
1154  btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
1155  if (algorithm)
1156  {
1157  btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
1158  //discrete collision detection query
1159 
1160  algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult);
1161 
1162  algorithm->~btCollisionAlgorithm();
1163  m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1164  }
1165  }
1166  return true;
1167  }
1168 };
1169 
1170 
1173 void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1174 {
1175  btVector3 aabbMin,aabbMax;
1176  colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1177  btSingleContactCallback contactCB(colObj,this,resultCallback);
1178 
1179  m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1180 }
1181 
1182 
1185 void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1186 {
1187  btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
1188  btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
1189 
1190  btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
1191  if (algorithm)
1192  {
1193  btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
1194  //discrete collision detection query
1195  algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
1196 
1197  algorithm->~btCollisionAlgorithm();
1198  getDispatcher()->freeCollisionAlgorithm(algorithm);
1199  }
1200 
1201 }
1202 
1203 
1204 
1205 
1206 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1207 {
1208  btIDebugDraw* m_debugDrawer;
1209  btVector3 m_color;
1210  btTransform m_worldTrans;
1211 
1212 public:
1213 
1214  DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1215  m_debugDrawer(debugDrawer),
1216  m_color(color),
1217  m_worldTrans(worldTrans)
1218  {
1219  }
1220 
1221  virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
1222  {
1223  processTriangle(triangle,partId,triangleIndex);
1224  }
1225 
1226  virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1227  {
1228  (void)partId;
1229  (void)triangleIndex;
1230 
1231  btVector3 wv0,wv1,wv2;
1232  wv0 = m_worldTrans*triangle[0];
1233  wv1 = m_worldTrans*triangle[1];
1234  wv2 = m_worldTrans*triangle[2];
1235  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1236 
1237  if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals )
1238  {
1239  btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1240  normal.normalize();
1241  btVector3 normalColor(1,1,0);
1242  m_debugDrawer->drawLine(center,center+normal,normalColor);
1243  }
1244  m_debugDrawer->drawLine(wv0,wv1,m_color);
1245  m_debugDrawer->drawLine(wv1,wv2,m_color);
1246  m_debugDrawer->drawLine(wv2,wv0,m_color);
1247  }
1248 };
1249 
1250 
1251 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1252 {
1253  // Draw a small simplex at the center of the object
1254  getDebugDrawer()->drawTransform(worldTransform,1);
1255 
1256  if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1257  {
1258  const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1259  for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1260  {
1261  btTransform childTrans = compoundShape->getChildTransform(i);
1262  const btCollisionShape* colShape = compoundShape->getChildShape(i);
1263  debugDrawObject(worldTransform*childTrans,colShape,color);
1264  }
1265 
1266  } else
1267  {
1268 
1269  switch (shape->getShapeType())
1270  {
1271 
1272  case BOX_SHAPE_PROXYTYPE:
1273  {
1274  const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1275  btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1276  getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1277  break;
1278  }
1279 
1280  case SPHERE_SHAPE_PROXYTYPE:
1281  {
1282  const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1283  btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1284 
1285  getDebugDrawer()->drawSphere(radius, worldTransform, color);
1286  break;
1287  }
1288  case MULTI_SPHERE_SHAPE_PROXYTYPE:
1289  {
1290  const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1291 
1292  btTransform childTransform;
1293  childTransform.setIdentity();
1294 
1295  for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1296  {
1297  childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1298  getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1299  }
1300 
1301  break;
1302  }
1303  case CAPSULE_SHAPE_PROXYTYPE:
1304  {
1305  const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1306 
1307  btScalar radius = capsuleShape->getRadius();
1308  btScalar halfHeight = capsuleShape->getHalfHeight();
1309 
1310  int upAxis = capsuleShape->getUpAxis();
1311  getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1312  break;
1313  }
1314  case CONE_SHAPE_PROXYTYPE:
1315  {
1316  const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1317  btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1318  btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1319 
1320  int upAxis= coneShape->getConeUpIndex();
1321  getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1322  break;
1323 
1324  }
1325  case CYLINDER_SHAPE_PROXYTYPE:
1326  {
1327  const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1328  int upAxis = cylinder->getUpAxis();
1329  btScalar radius = cylinder->getRadius();
1330  btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1331  getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1332  break;
1333  }
1334 
1335  case STATIC_PLANE_PROXYTYPE:
1336  {
1337  const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1338  btScalar planeConst = staticPlaneShape->getPlaneConstant();
1339  const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1340  getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
1341  break;
1342 
1343  }
1344  default:
1345  {
1346 
1348  if (shape->isPolyhedral())
1349  {
1350  btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1351 
1352  int i;
1353  if (polyshape->getConvexPolyhedron())
1354  {
1355  const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1356  for (i=0;i<poly->m_faces.size();i++)
1357  {
1358  btVector3 centroid(0,0,0);
1359  int numVerts = poly->m_faces[i].m_indices.size();
1360  if (numVerts)
1361  {
1362  int lastV = poly->m_faces[i].m_indices[numVerts-1];
1363  for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
1364  {
1365  int curVert = poly->m_faces[i].m_indices[v];
1366  centroid+=poly->m_vertices[curVert];
1367  getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
1368  lastV = curVert;
1369  }
1370  }
1371  centroid*= btScalar(1.f)/btScalar(numVerts);
1372  if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
1373  {
1374  btVector3 normalColor(1,1,0);
1375  btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
1376  getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
1377  }
1378 
1379  }
1380 
1381 
1382  } else
1383  {
1384  for (i=0;i<polyshape->getNumEdges();i++)
1385  {
1386  btVector3 a,b;
1387  polyshape->getEdge(i,a,b);
1388  btVector3 wa = worldTransform * a;
1389  btVector3 wb = worldTransform * b;
1390  getDebugDrawer()->drawLine(wa,wb,color);
1391  }
1392  }
1393 
1394 
1395  }
1396 
1397  if (shape->isConcave())
1398  {
1399  btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1400 
1402  btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1403  btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1404 
1405  DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1406  concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1407 
1408  }
1409 
1410  if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1411  {
1412  btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1413  //todo: pass camera for some culling
1414  btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1415  btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1416  //DebugDrawcallback drawCallback;
1417  DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1418  convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1419  }
1420 
1421 
1422 
1423  }
1424 
1425  }
1426  }
1427 }
1428 
1429 
1430 void btCollisionWorld::debugDrawWorld()
1431 {
1432  if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1433  {
1434  int numManifolds = getDispatcher()->getNumManifolds();
1435  btVector3 color(1,1,0);
1436  for (int i=0;i<numManifolds;i++)
1437  {
1438  btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1439  //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1440  //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1441 
1442  int numContacts = contactManifold->getNumContacts();
1443  for (int j=0;j<numContacts;j++)
1444  {
1445  btManifoldPoint& cp = contactManifold->getContactPoint(j);
1446  getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1447  }
1448  }
1449  }
1450 
1451  if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
1452  {
1453  int i;
1454 
1455  for ( i=0;i<m_collisionObjects.size();i++)
1456  {
1457  btCollisionObject* colObj = m_collisionObjects[i];
1458  if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1459  {
1460  if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
1461  {
1462  btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
1463  switch(colObj->getActivationState())
1464  {
1465  case ACTIVE_TAG:
1466  color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
1467  case ISLAND_SLEEPING:
1468  color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
1469  case WANTS_DEACTIVATION:
1470  color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
1471  case DISABLE_DEACTIVATION:
1472  color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
1473  case DISABLE_SIMULATION:
1474  color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
1475  default:
1476  {
1477  color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
1478  }
1479  };
1480 
1481  debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1482  }
1483  if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1484  {
1485  btVector3 minAabb,maxAabb;
1486  btVector3 colorvec(1,0,0);
1487  colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1488  btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
1489  minAabb -= contactThreshold;
1490  maxAabb += contactThreshold;
1491 
1492  btVector3 minAabb2,maxAabb2;
1493 
1494  if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
1495  {
1496  colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
1497  minAabb2 -= contactThreshold;
1498  maxAabb2 += contactThreshold;
1499  minAabb.setMin(minAabb2);
1500  maxAabb.setMax(maxAabb2);
1501  }
1502 
1503  m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1504  }
1505  }
1506 
1507  }
1508  }
1509 }
1510 
1511 
1512 void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1513 {
1514  int i;
1515  //serialize all collision objects
1516  for (i=0;i<m_collisionObjects.size();i++)
1517  {
1518  btCollisionObject* colObj = m_collisionObjects[i];
1519  if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1520  {
1521  colObj->serializeSingleObject(serializer);
1522  }
1523  }
1524 
1526  btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
1527 
1528  for (i=0;i<m_collisionObjects.size();i++)
1529  {
1530  btCollisionObject* colObj = m_collisionObjects[i];
1531  btCollisionShape* shape = colObj->getCollisionShape();
1532 
1533  if (!serializedShapes.find(shape))
1534  {
1535  serializedShapes.insert(shape,shape);
1536  shape->serializeSingleShape(serializer);
1537  }
1538  }
1539 
1540 }
1541 
1542 
1543 void btCollisionWorld::serialize(btSerializer* serializer)
1544 {
1545 
1546  serializer->startSerialization();
1547 
1548  serializeCollisionObjects(serializer);
1549 
1550  serializer->finishSerialization();
1551 }
1552 
Generated on Wed Apr 9 2014 02:38:22 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2