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