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
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
51 ///for debug drawing
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
btCollisionWorld(btDispatcher * dispatcher,btBroadphaseInterface * pairCache,btCollisionConfiguration * collisionConfiguration)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
~btCollisionWorld()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
addCollisionObject(btCollisionObject * collisionObject,short int collisionFilterGroup,short int collisionFilterMask)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
updateSingleAabb(btCollisionObject * colObj)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
updateAabbs()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
computeOverlappingPairs()208 void btCollisionWorld::computeOverlappingPairs()
209 {
210 BT_PROFILE("calculateOverlappingPairs");
211 m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
212 }
213
performDiscreteCollisionDetection()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
removeCollisionObject(btCollisionObject * collisionObject)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
rayTestSingle(const btTransform & rayFromTrans,const btTransform & rayToTrans,btCollisionObject * collisionObject,const btCollisionShape * collisionShape,const btTransform & colObjWorldTransform,RayResultCallback & resultCallback)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
rayTestSingleInternal(const btTransform & rayFromTrans,const btTransform & rayToTrans,const btCollisionObjectWrapper * collisionObjectWrap,RayResultCallback & resultCallback)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
296 btConvexCast* convexCasterPtr = 0;
297 //use kF_UseSubSimplexConvexCastRaytest by default
298 if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
299 convexCasterPtr = &gjkConvexCaster;
300 else
301 convexCasterPtr = &subSimplexConvexCaster;
302
303 btConvexCast& convexCaster = *convexCasterPtr;
304
305 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
306 {
307 //add hit
308 if (castResult.m_normal.length2() > btScalar(0.0001))
309 {
310 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
311 {
312 //todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
313 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
314 //rotate normal into worldspace
315 castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
316 #endif //USE_SUBSIMPLEX_CONVEX_CAST
317
318 castResult.m_normal.normalize();
319 btCollisionWorld::LocalRayResult localRayResult
320 (
321 collisionObjectWrap->getCollisionObject(),
322 0,
323 castResult.m_normal,
324 castResult.m_fraction
325 );
326
327 bool normalInWorldSpace = true;
328 resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
329
330 }
331 }
332 }
333 } else {
334 if (collisionShape->isConcave())
335 {
336
337 //ConvexCast::CastResult
338 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
339 {
340 btCollisionWorld::RayResultCallback* m_resultCallback;
341 const btCollisionObject* m_collisionObject;
342 const btConcaveShape* m_triangleMesh;
343
344 btTransform m_colObjWorldTransform;
345
346 BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
347 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform):
348 //@BP Mod
349 btTriangleRaycastCallback(from,to, resultCallback->m_flags),
350 m_resultCallback(resultCallback),
351 m_collisionObject(collisionObject),
352 m_triangleMesh(triangleMesh),
353 m_colObjWorldTransform(colObjWorldTransform)
354 {
355 }
356
357
358 virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
359 {
360 btCollisionWorld::LocalShapeInfo shapeInfo;
361 shapeInfo.m_shapePart = partId;
362 shapeInfo.m_triangleIndex = triangleIndex;
363
364 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
365
366 btCollisionWorld::LocalRayResult rayResult
367 (m_collisionObject,
368 &shapeInfo,
369 hitNormalWorld,
370 hitFraction);
371
372 bool normalInWorldSpace = true;
373 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
374 }
375
376 };
377
378 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
379 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
380 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
381
382 // BT_PROFILE("rayTestConcave");
383 if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
384 {
385 ///optimized version for btBvhTriangleMeshShape
386 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
387
388 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
389 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
390 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
391 }
392 else
393 {
394 //generic (slower) case
395 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
396
397 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
398
399 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
400 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
401
402 //ConvexCast::CastResult
403
404 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
405 {
406 btCollisionWorld::RayResultCallback* m_resultCallback;
407 const btCollisionObject* m_collisionObject;
408 btConcaveShape* m_triangleMesh;
409
410 btTransform m_colObjWorldTransform;
411
412 BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
413 btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
414 //@BP Mod
415 btTriangleRaycastCallback(from,to, resultCallback->m_flags),
416 m_resultCallback(resultCallback),
417 m_collisionObject(collisionObject),
418 m_triangleMesh(triangleMesh),
419 m_colObjWorldTransform(colObjWorldTransform)
420 {
421 }
422
423
424 virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
425 {
426 btCollisionWorld::LocalShapeInfo shapeInfo;
427 shapeInfo.m_shapePart = partId;
428 shapeInfo.m_triangleIndex = triangleIndex;
429
430 btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
431
432 btCollisionWorld::LocalRayResult rayResult
433 (m_collisionObject,
434 &shapeInfo,
435 hitNormalWorld,
436 hitFraction);
437
438 bool normalInWorldSpace = true;
439 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
440 }
441
442 };
443
444
445 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
446 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
447
448 btVector3 rayAabbMinLocal = rayFromLocal;
449 rayAabbMinLocal.setMin(rayToLocal);
450 btVector3 rayAabbMaxLocal = rayFromLocal;
451 rayAabbMaxLocal.setMax(rayToLocal);
452
453 concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
454 }
455 } else {
456 // BT_PROFILE("rayTestCompound");
457 if (collisionShape->isCompound())
458 {
459 struct LocalInfoAdder2 : public RayResultCallback
460 {
461 RayResultCallback* m_userCallback;
462 int m_i;
463
464 LocalInfoAdder2 (int i, RayResultCallback *user)
465 : m_userCallback(user), m_i(i)
466 {
467 m_closestHitFraction = m_userCallback->m_closestHitFraction;
468 m_flags = m_userCallback->m_flags;
469 }
470 virtual bool needsCollision(btBroadphaseProxy* p) const
471 {
472 return m_userCallback->needsCollision(p);
473 }
474
475 virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
476 {
477 btCollisionWorld::LocalShapeInfo shapeInfo;
478 shapeInfo.m_shapePart = -1;
479 shapeInfo.m_triangleIndex = m_i;
480 if (r.m_localShapeInfo == NULL)
481 r.m_localShapeInfo = &shapeInfo;
482
483 const btScalar result = m_userCallback->addSingleResult(r, b);
484 m_closestHitFraction = m_userCallback->m_closestHitFraction;
485 return result;
486 }
487 };
488
489 struct RayTester : btDbvt::ICollide
490 {
491 const btCollisionObject* m_collisionObject;
492 const btCompoundShape* m_compoundShape;
493 const btTransform& m_colObjWorldTransform;
494 const btTransform& m_rayFromTrans;
495 const btTransform& m_rayToTrans;
496 RayResultCallback& m_resultCallback;
497
498 RayTester(const btCollisionObject* collisionObject,
499 const btCompoundShape* compoundShape,
500 const btTransform& colObjWorldTransform,
501 const btTransform& rayFromTrans,
502 const btTransform& rayToTrans,
503 RayResultCallback& resultCallback):
504 m_collisionObject(collisionObject),
505 m_compoundShape(compoundShape),
506 m_colObjWorldTransform(colObjWorldTransform),
507 m_rayFromTrans(rayFromTrans),
508 m_rayToTrans(rayToTrans),
509 m_resultCallback(resultCallback)
510 {
511
512 }
513
514 void ProcessLeaf(int i)
515 {
516 const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
517 const btTransform& childTrans = m_compoundShape->getChildTransform(i);
518 btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
519
520 btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i);
521 // replace collision shape so that callback can determine the triangle
522
523
524
525 LocalInfoAdder2 my_cb(i, &m_resultCallback);
526
527 rayTestSingleInternal(
528 m_rayFromTrans,
529 m_rayToTrans,
530 &tmpOb,
531 my_cb);
532
533 }
534
535 void Process(const btDbvtNode* leaf)
536 {
537 ProcessLeaf(leaf->dataAsInt);
538 }
539 };
540
541 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
542 const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
543
544
545 RayTester rayCB(
546 collisionObjectWrap->getCollisionObject(),
547 compoundShape,
548 colObjWorldTransform,
549 rayFromTrans,
550 rayToTrans,
551 resultCallback);
552 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
553 if (dbvt)
554 {
555 btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
556 btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
557 btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
558 }
559 else
560 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
561 {
562 for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
563 {
564 rayCB.ProcessLeaf(i);
565 }
566 }
567 }
568 }
569 }
570 }
571
objectQuerySingle(const btConvexShape * castShape,const btTransform & convexFromTrans,const btTransform & convexToTrans,btCollisionObject * collisionObject,const btCollisionShape * collisionShape,const btTransform & colObjWorldTransform,ConvexResultCallback & resultCallback,btScalar allowedPenetration)572 void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
573 btCollisionObject* collisionObject,
574 const btCollisionShape* collisionShape,
575 const btTransform& colObjWorldTransform,
576 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
577 {
578 btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
579 btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration);
580 }
581
objectQuerySingleInternal(const btConvexShape * castShape,const btTransform & convexFromTrans,const btTransform & convexToTrans,const btCollisionObjectWrapper * colObjWrap,ConvexResultCallback & resultCallback,btScalar allowedPenetration)582 void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
583 const btCollisionObjectWrapper* colObjWrap,
584 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
585 {
586 const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
587 const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
588
589 if (collisionShape->isConvex())
590 {
591 //BT_PROFILE("convexSweepConvex");
592 btConvexCast::CastResult castResult;
593 castResult.m_allowedPenetration = allowedPenetration;
594 castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
595
596 btConvexShape* convexShape = (btConvexShape*) collisionShape;
597 btVoronoiSimplexSolver simplexSolver;
598 btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
599
600 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
601 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
602 //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
603
604 btConvexCast* castPtr = &convexCaster1;
605
606
607
608 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
609 {
610 //add hit
611 if (castResult.m_normal.length2() > btScalar(0.0001))
612 {
613 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
614 {
615 castResult.m_normal.normalize();
616 btCollisionWorld::LocalConvexResult localConvexResult
617 (
618 colObjWrap->getCollisionObject(),
619 0,
620 castResult.m_normal,
621 castResult.m_hitPoint,
622 castResult.m_fraction
623 );
624
625 bool normalInWorldSpace = true;
626 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
627
628 }
629 }
630 }
631 } else {
632 if (collisionShape->isConcave())
633 {
634 if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
635 {
636 //BT_PROFILE("convexSweepbtBvhTriangleMesh");
637 btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
638 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
639 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
640 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
641 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
642 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
643
644 //ConvexCast::CastResult
645 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
646 {
647 btCollisionWorld::ConvexResultCallback* m_resultCallback;
648 const btCollisionObject* m_collisionObject;
649 btTriangleMeshShape* m_triangleMesh;
650
651 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
652 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
653 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
654 m_resultCallback(resultCallback),
655 m_collisionObject(collisionObject),
656 m_triangleMesh(triangleMesh)
657 {
658 }
659
660
661 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
662 {
663 btCollisionWorld::LocalShapeInfo shapeInfo;
664 shapeInfo.m_shapePart = partId;
665 shapeInfo.m_triangleIndex = triangleIndex;
666 if (hitFraction <= m_resultCallback->m_closestHitFraction)
667 {
668
669 btCollisionWorld::LocalConvexResult convexResult
670 (m_collisionObject,
671 &shapeInfo,
672 hitNormalLocal,
673 hitPointLocal,
674 hitFraction);
675
676 bool normalInWorldSpace = true;
677
678
679 return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
680 }
681 return hitFraction;
682 }
683
684 };
685
686 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform);
687 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
688 tccb.m_allowedPenetration = allowedPenetration;
689 btVector3 boxMinLocal, boxMaxLocal;
690 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
691 triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
692 } else
693 {
694 if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
695 {
696 btConvexCast::CastResult castResult;
697 castResult.m_allowedPenetration = allowedPenetration;
698 castResult.m_fraction = resultCallback.m_closestHitFraction;
699 btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
700 btContinuousConvexCollision convexCaster1(castShape,planeShape);
701 btConvexCast* castPtr = &convexCaster1;
702
703 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
704 {
705 //add hit
706 if (castResult.m_normal.length2() > btScalar(0.0001))
707 {
708 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
709 {
710 castResult.m_normal.normalize();
711 btCollisionWorld::LocalConvexResult localConvexResult
712 (
713 colObjWrap->getCollisionObject(),
714 0,
715 castResult.m_normal,
716 castResult.m_hitPoint,
717 castResult.m_fraction
718 );
719
720 bool normalInWorldSpace = true;
721 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
722 }
723 }
724 }
725
726 } else
727 {
728 //BT_PROFILE("convexSweepConcave");
729 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
730 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
731 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
732 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
733 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
734 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
735
736 //ConvexCast::CastResult
737 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
738 {
739 btCollisionWorld::ConvexResultCallback* m_resultCallback;
740 const btCollisionObject* m_collisionObject;
741 btConcaveShape* m_triangleMesh;
742
743 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
744 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
745 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
746 m_resultCallback(resultCallback),
747 m_collisionObject(collisionObject),
748 m_triangleMesh(triangleMesh)
749 {
750 }
751
752
753 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
754 {
755 btCollisionWorld::LocalShapeInfo shapeInfo;
756 shapeInfo.m_shapePart = partId;
757 shapeInfo.m_triangleIndex = triangleIndex;
758 if (hitFraction <= m_resultCallback->m_closestHitFraction)
759 {
760
761 btCollisionWorld::LocalConvexResult convexResult
762 (m_collisionObject,
763 &shapeInfo,
764 hitNormalLocal,
765 hitPointLocal,
766 hitFraction);
767
768 bool normalInWorldSpace = true;
769
770 return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
771 }
772 return hitFraction;
773 }
774
775 };
776
777 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
778 tccb.m_hitFraction = resultCallback.m_closestHitFraction;
779 tccb.m_allowedPenetration = allowedPenetration;
780 btVector3 boxMinLocal, boxMaxLocal;
781 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
782
783 btVector3 rayAabbMinLocal = convexFromLocal;
784 rayAabbMinLocal.setMin(convexToLocal);
785 btVector3 rayAabbMaxLocal = convexFromLocal;
786 rayAabbMaxLocal.setMax(convexToLocal);
787 rayAabbMinLocal += boxMinLocal;
788 rayAabbMaxLocal += boxMaxLocal;
789 concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
790 }
791 }
792 } else {
793 ///@todo : use AABB tree or other BVH acceleration structure!
794 if (collisionShape->isCompound())
795 {
796 BT_PROFILE("convexSweepCompound");
797 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
798 int i=0;
799 for (i=0;i<compoundShape->getNumChildShapes();i++)
800 {
801 btTransform childTrans = compoundShape->getChildTransform(i);
802 const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
803 btTransform childWorldTrans = colObjWorldTransform * childTrans;
804
805 struct LocalInfoAdder : public ConvexResultCallback {
806 ConvexResultCallback* m_userCallback;
807 int m_i;
808
809 LocalInfoAdder (int i, ConvexResultCallback *user)
810 : m_userCallback(user), m_i(i)
811 {
812 m_closestHitFraction = m_userCallback->m_closestHitFraction;
813 }
814 virtual bool needsCollision(btBroadphaseProxy* p) const
815 {
816 return m_userCallback->needsCollision(p);
817 }
818 virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
819 {
820 btCollisionWorld::LocalShapeInfo shapeInfo;
821 shapeInfo.m_shapePart = -1;
822 shapeInfo.m_triangleIndex = m_i;
823 if (r.m_localShapeInfo == NULL)
824 r.m_localShapeInfo = &shapeInfo;
825 const btScalar result = m_userCallback->addSingleResult(r, b);
826 m_closestHitFraction = m_userCallback->m_closestHitFraction;
827 return result;
828
829 }
830 };
831
832 LocalInfoAdder my_cb(i, &resultCallback);
833
834 btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i);
835
836 objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
837 &tmpObj,my_cb, allowedPenetration);
838
839 }
840 }
841 }
842 }
843 }
844
845
846 struct btSingleRayCallback : public btBroadphaseRayCallback
847 {
848
849 btVector3 m_rayFromWorld;
850 btVector3 m_rayToWorld;
851 btTransform m_rayFromTrans;
852 btTransform m_rayToTrans;
853 btVector3 m_hitNormal;
854
855 const btCollisionWorld* m_world;
856 btCollisionWorld::RayResultCallback& m_resultCallback;
857
btSingleRayCallbackbtSingleRayCallback858 btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
859 :m_rayFromWorld(rayFromWorld),
860 m_rayToWorld(rayToWorld),
861 m_world(world),
862 m_resultCallback(resultCallback)
863 {
864 m_rayFromTrans.setIdentity();
865 m_rayFromTrans.setOrigin(m_rayFromWorld);
866 m_rayToTrans.setIdentity();
867 m_rayToTrans.setOrigin(m_rayToWorld);
868
869 btVector3 rayDir = (rayToWorld-rayFromWorld);
870
871 rayDir.normalize ();
872 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
873 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
874 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
875 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
876 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
877 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
878 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
879
880 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
881
882 }
883
884
885
processbtSingleRayCallback886 virtual bool process(const btBroadphaseProxy* proxy)
887 {
888 ///terminate further ray tests, once the closestHitFraction reached zero
889 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
890 return false;
891
892 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
893
894 //only perform raycast if filterMask matches
895 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
896 {
897 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
898 //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
899 #if 0
900 #ifdef RECALCULATE_AABB
901 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
902 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
903 #else
904 //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
905 const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
906 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
907 #endif
908 #endif
909 //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
910 //culling already done by broadphase
911 //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
912 {
913 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
914 collisionObject,
915 collisionObject->getCollisionShape(),
916 collisionObject->getWorldTransform(),
917 m_resultCallback);
918 }
919 }
920 return true;
921 }
922 };
923
rayTest(const btVector3 & rayFromWorld,const btVector3 & rayToWorld,RayResultCallback & resultCallback) const924 void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
925 {
926 //BT_PROFILE("rayTest");
927 /// use the broadphase to accelerate the search for objects, based on their aabb
928 /// and for each object with ray-aabb overlap, perform an exact ray test
929 btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
930
931 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
932 m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
933 #else
934 for (int i=0;i<this->getNumCollisionObjects();i++)
935 {
936 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
937 }
938 #endif //USE_BRUTEFORCE_RAYBROADPHASE
939
940 }
941
942
943 struct btSingleSweepCallback : public btBroadphaseRayCallback
944 {
945
946 btTransform m_convexFromTrans;
947 btTransform m_convexToTrans;
948 btVector3 m_hitNormal;
949 const btCollisionWorld* m_world;
950 btCollisionWorld::ConvexResultCallback& m_resultCallback;
951 btScalar m_allowedCcdPenetration;
952 const btConvexShape* m_castShape;
953
954
btSingleSweepCallbackbtSingleSweepCallback955 btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
956 :m_convexFromTrans(convexFromTrans),
957 m_convexToTrans(convexToTrans),
958 m_world(world),
959 m_resultCallback(resultCallback),
960 m_allowedCcdPenetration(allowedPenetration),
961 m_castShape(castShape)
962 {
963 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
964 btVector3 rayDir = unnormalizedRayDir.normalized();
965 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
966 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
967 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
968 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
969 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
970 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
971 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
972
973 m_lambda_max = rayDir.dot(unnormalizedRayDir);
974
975 }
976
processbtSingleSweepCallback977 virtual bool process(const btBroadphaseProxy* proxy)
978 {
979 ///terminate further convex sweep tests, once the closestHitFraction reached zero
980 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
981 return false;
982
983 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
984
985 //only perform raycast if filterMask matches
986 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
987 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
988 m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
989 collisionObject,
990 collisionObject->getCollisionShape(),
991 collisionObject->getWorldTransform(),
992 m_resultCallback,
993 m_allowedCcdPenetration);
994 }
995
996 return true;
997 }
998 };
999
1000
1001
convexSweepTest(const btConvexShape * castShape,const btTransform & convexFromWorld,const btTransform & convexToWorld,ConvexResultCallback & resultCallback,btScalar allowedCcdPenetration) const1002 void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
1003 {
1004
1005 BT_PROFILE("convexSweepTest");
1006 /// use the broadphase to accelerate the search for objects, based on their aabb
1007 /// and for each object with ray-aabb overlap, perform an exact ray test
1008 /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
1009
1010
1011
1012 btTransform convexFromTrans,convexToTrans;
1013 convexFromTrans = convexFromWorld;
1014 convexToTrans = convexToWorld;
1015 btVector3 castShapeAabbMin, castShapeAabbMax;
1016 /* Compute AABB that encompasses angular movement */
1017 {
1018 btVector3 linVel, angVel;
1019 btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
1020 btVector3 zeroLinVel;
1021 zeroLinVel.setValue(0,0,0);
1022 btTransform R;
1023 R.setIdentity ();
1024 R.setRotation (convexFromTrans.getRotation());
1025 castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
1026 }
1027
1028 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
1029
1030 btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
1031
1032 m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
1033
1034 #else
1035 /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
1036 // do a ray-shape query using convexCaster (CCD)
1037 int i;
1038 for (i=0;i<m_collisionObjects.size();i++)
1039 {
1040 btCollisionObject* collisionObject= m_collisionObjects[i];
1041 //only perform raycast if filterMask matches
1042 if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
1043 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1044 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
1045 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
1046 AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1047 btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1048 btVector3 hitNormal;
1049 if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
1050 {
1051 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
1052 collisionObject,
1053 collisionObject->getCollisionShape(),
1054 collisionObject->getWorldTransform(),
1055 resultCallback,
1056 allowedCcdPenetration);
1057 }
1058 }
1059 }
1060 #endif //USE_BRUTEFORCE_RAYBROADPHASE
1061 }
1062
1063
1064
1065 struct btBridgedManifoldResult : public btManifoldResult
1066 {
1067
1068 btCollisionWorld::ContactResultCallback& m_resultCallback;
1069
btBridgedManifoldResultbtBridgedManifoldResult1070 btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback )
1071 :btManifoldResult(obj0Wrap,obj1Wrap),
1072 m_resultCallback(resultCallback)
1073 {
1074 }
1075
addContactPointbtBridgedManifoldResult1076 virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
1077 {
1078 bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
1079 btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1080 btVector3 localA;
1081 btVector3 localB;
1082 if (isSwapped)
1083 {
1084 localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1085 localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1086 } else
1087 {
1088 localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
1089 localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
1090 }
1091
1092 btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
1093 newPt.m_positionWorldOnA = pointA;
1094 newPt.m_positionWorldOnB = pointInWorld;
1095
1096 //BP mod, store contact triangles.
1097 if (isSwapped)
1098 {
1099 newPt.m_partId0 = m_partId1;
1100 newPt.m_partId1 = m_partId0;
1101 newPt.m_index0 = m_index1;
1102 newPt.m_index1 = m_index0;
1103 } else
1104 {
1105 newPt.m_partId0 = m_partId0;
1106 newPt.m_partId1 = m_partId1;
1107 newPt.m_index0 = m_index0;
1108 newPt.m_index1 = m_index1;
1109 }
1110
1111 //experimental feature info, for per-triangle material etc.
1112 const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
1113 const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
1114 m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
1115
1116 }
1117
1118 };
1119
1120
1121
1122 struct btSingleContactCallback : public btBroadphaseAabbCallback
1123 {
1124
1125 btCollisionObject* m_collisionObject;
1126 btCollisionWorld* m_world;
1127 btCollisionWorld::ContactResultCallback& m_resultCallback;
1128
1129
btSingleContactCallbackbtSingleContactCallback1130 btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
1131 :m_collisionObject(collisionObject),
1132 m_world(world),
1133 m_resultCallback(resultCallback)
1134 {
1135 }
1136
processbtSingleContactCallback1137 virtual bool process(const btBroadphaseProxy* proxy)
1138 {
1139 btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1140 if (collisionObject == m_collisionObject)
1141 return true;
1142
1143 //only perform raycast if filterMask matches
1144 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1145 {
1146 btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
1147 btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
1148
1149 btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
1150 if (algorithm)
1151 {
1152 btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
1153 //discrete collision detection query
1154
1155 algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult);
1156
1157 algorithm->~btCollisionAlgorithm();
1158 m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1159 }
1160 }
1161 return true;
1162 }
1163 };
1164
1165
1166 ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1167 ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
contactTest(btCollisionObject * colObj,ContactResultCallback & resultCallback)1168 void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1169 {
1170 btVector3 aabbMin,aabbMax;
1171 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1172 btSingleContactCallback contactCB(colObj,this,resultCallback);
1173
1174 m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1175 }
1176
1177
1178 ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1179 ///it reports one or more contact points (including the one with deepest penetration)
contactPairTest(btCollisionObject * colObjA,btCollisionObject * colObjB,ContactResultCallback & resultCallback)1180 void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1181 {
1182 btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
1183 btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
1184
1185 btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
1186 if (algorithm)
1187 {
1188 btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
1189 //discrete collision detection query
1190 algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
1191
1192 algorithm->~btCollisionAlgorithm();
1193 getDispatcher()->freeCollisionAlgorithm(algorithm);
1194 }
1195
1196 }
1197
1198
1199
1200
1201 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1202 {
1203 btIDebugDraw* m_debugDrawer;
1204 btVector3 m_color;
1205 btTransform m_worldTrans;
1206
1207 public:
1208
DebugDrawcallback(btIDebugDraw * debugDrawer,const btTransform & worldTrans,const btVector3 & color)1209 DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1210 m_debugDrawer(debugDrawer),
1211 m_color(color),
1212 m_worldTrans(worldTrans)
1213 {
1214 }
1215
internalProcessTriangleIndex(btVector3 * triangle,int partId,int triangleIndex)1216 virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
1217 {
1218 processTriangle(triangle,partId,triangleIndex);
1219 }
1220
processTriangle(btVector3 * triangle,int partId,int triangleIndex)1221 virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1222 {
1223 (void)partId;
1224 (void)triangleIndex;
1225
1226 btVector3 wv0,wv1,wv2;
1227 wv0 = m_worldTrans*triangle[0];
1228 wv1 = m_worldTrans*triangle[1];
1229 wv2 = m_worldTrans*triangle[2];
1230 btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1231
1232 if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals )
1233 {
1234 btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1235 normal.normalize();
1236 btVector3 normalColor(1,1,0);
1237 m_debugDrawer->drawLine(center,center+normal,normalColor);
1238 }
1239 m_debugDrawer->drawLine(wv0,wv1,m_color);
1240 m_debugDrawer->drawLine(wv1,wv2,m_color);
1241 m_debugDrawer->drawLine(wv2,wv0,m_color);
1242 }
1243 };
1244
1245
debugDrawObject(const btTransform & worldTransform,const btCollisionShape * shape,const btVector3 & color)1246 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1247 {
1248 // Draw a small simplex at the center of the object
1249 if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
1250 {
1251 getDebugDrawer()->drawTransform(worldTransform,1);
1252 }
1253
1254 if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1255 {
1256 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1257 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1258 {
1259 btTransform childTrans = compoundShape->getChildTransform(i);
1260 const btCollisionShape* colShape = compoundShape->getChildShape(i);
1261 debugDrawObject(worldTransform*childTrans,colShape,color);
1262 }
1263
1264 } else
1265 {
1266
1267 switch (shape->getShapeType())
1268 {
1269
1270 case BOX_SHAPE_PROXYTYPE:
1271 {
1272 const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1273 btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1274 getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1275 break;
1276 }
1277
1278 case SPHERE_SHAPE_PROXYTYPE:
1279 {
1280 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1281 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1282
1283 getDebugDrawer()->drawSphere(radius, worldTransform, color);
1284 break;
1285 }
1286 case MULTI_SPHERE_SHAPE_PROXYTYPE:
1287 {
1288 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1289
1290 btTransform childTransform;
1291 childTransform.setIdentity();
1292
1293 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1294 {
1295 childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1296 getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1297 }
1298
1299 break;
1300 }
1301 case CAPSULE_SHAPE_PROXYTYPE:
1302 {
1303 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1304
1305 btScalar radius = capsuleShape->getRadius();
1306 btScalar halfHeight = capsuleShape->getHalfHeight();
1307
1308 int upAxis = capsuleShape->getUpAxis();
1309 getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1310 break;
1311 }
1312 case CONE_SHAPE_PROXYTYPE:
1313 {
1314 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1315 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1316 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1317
1318 int upAxis= coneShape->getConeUpIndex();
1319 getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1320 break;
1321
1322 }
1323 case CYLINDER_SHAPE_PROXYTYPE:
1324 {
1325 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1326 int upAxis = cylinder->getUpAxis();
1327 btScalar radius = cylinder->getRadius();
1328 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1329 getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1330 break;
1331 }
1332
1333 case STATIC_PLANE_PROXYTYPE:
1334 {
1335 const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1336 btScalar planeConst = staticPlaneShape->getPlaneConstant();
1337 const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1338 getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
1339 break;
1340
1341 }
1342 default:
1343 {
1344
1345 /// for polyhedral shapes
1346 if (shape->isPolyhedral())
1347 {
1348 btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1349
1350 int i;
1351 if (polyshape->getConvexPolyhedron())
1352 {
1353 const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1354 for (i=0;i<poly->m_faces.size();i++)
1355 {
1356 btVector3 centroid(0,0,0);
1357 int numVerts = poly->m_faces[i].m_indices.size();
1358 if (numVerts)
1359 {
1360 int lastV = poly->m_faces[i].m_indices[numVerts-1];
1361 for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
1362 {
1363 int curVert = poly->m_faces[i].m_indices[v];
1364 centroid+=poly->m_vertices[curVert];
1365 getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
1366 lastV = curVert;
1367 }
1368 }
1369 centroid*= btScalar(1.f)/btScalar(numVerts);
1370 if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
1371 {
1372 btVector3 normalColor(1,1,0);
1373 btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
1374 getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
1375 }
1376
1377 }
1378
1379
1380 } else
1381 {
1382 for (i=0;i<polyshape->getNumEdges();i++)
1383 {
1384 btVector3 a,b;
1385 polyshape->getEdge(i,a,b);
1386 btVector3 wa = worldTransform * a;
1387 btVector3 wb = worldTransform * b;
1388 getDebugDrawer()->drawLine(wa,wb,color);
1389 }
1390 }
1391
1392
1393 }
1394
1395 if (shape->isConcave())
1396 {
1397 btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1398
1399 ///@todo pass camera, for some culling? no -> we are not a graphics lib
1400 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1401 btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1402
1403 DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1404 concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1405
1406 }
1407
1408 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1409 {
1410 btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1411 //todo: pass camera for some culling
1412 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1413 btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1414 //DebugDrawcallback drawCallback;
1415 DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1416 convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1417 }
1418
1419
1420
1421 }
1422
1423 }
1424 }
1425 }
1426
1427
debugDrawWorld()1428 void btCollisionWorld::debugDrawWorld()
1429 {
1430 if (getDebugDrawer())
1431 {
1432 btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
1433
1434 if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1435 {
1436
1437
1438 if (getDispatcher())
1439 {
1440 int numManifolds = getDispatcher()->getNumManifolds();
1441
1442 for (int i=0;i<numManifolds;i++)
1443 {
1444 btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1445 //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1446 //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1447
1448 int numContacts = contactManifold->getNumContacts();
1449 for (int j=0;j<numContacts;j++)
1450 {
1451 btManifoldPoint& cp = contactManifold->getContactPoint(j);
1452 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint);
1453 }
1454 }
1455 }
1456 }
1457
1458 if ((getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
1459 {
1460 int i;
1461
1462 for ( i=0;i<m_collisionObjects.size();i++)
1463 {
1464 btCollisionObject* colObj = m_collisionObjects[i];
1465 if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1466 {
1467 if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
1468 {
1469 btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4));
1470
1471 switch(colObj->getActivationState())
1472 {
1473 case ACTIVE_TAG:
1474 color = defaultColors.m_activeObject; break;
1475 case ISLAND_SLEEPING:
1476 color = defaultColors.m_deactivatedObject;break;
1477 case WANTS_DEACTIVATION:
1478 color = defaultColors.m_wantsDeactivationObject;break;
1479 case DISABLE_DEACTIVATION:
1480 color = defaultColors.m_disabledDeactivationObject;break;
1481 case DISABLE_SIMULATION:
1482 color = defaultColors.m_disabledSimulationObject;break;
1483 default:
1484 {
1485 color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3));
1486 }
1487 };
1488
1489 debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1490 }
1491 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1492 {
1493 btVector3 minAabb,maxAabb;
1494 btVector3 colorvec = defaultColors.m_aabb;
1495 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1496 btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
1497 minAabb -= contactThreshold;
1498 maxAabb += contactThreshold;
1499
1500 btVector3 minAabb2,maxAabb2;
1501
1502 if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
1503 {
1504 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
1505 minAabb2 -= contactThreshold;
1506 maxAabb2 += contactThreshold;
1507 minAabb.setMin(minAabb2);
1508 maxAabb.setMax(maxAabb2);
1509 }
1510
1511 m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1512 }
1513 }
1514 }
1515 }
1516 }
1517 }
1518
1519
serializeCollisionObjects(btSerializer * serializer)1520 void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1521 {
1522 int i;
1523
1524 ///keep track of shapes already serialized
1525 btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
1526
1527 for (i=0;i<m_collisionObjects.size();i++)
1528 {
1529 btCollisionObject* colObj = m_collisionObjects[i];
1530 btCollisionShape* shape = colObj->getCollisionShape();
1531
1532 if (!serializedShapes.find(shape))
1533 {
1534 serializedShapes.insert(shape,shape);
1535 shape->serializeSingleShape(serializer);
1536 }
1537 }
1538
1539 //serialize all collision objects
1540 for (i=0;i<m_collisionObjects.size();i++)
1541 {
1542 btCollisionObject* colObj = m_collisionObjects[i];
1543 if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
1544 {
1545 colObj->serializeSingleObject(serializer);
1546 }
1547 }
1548 }
1549
1550
serialize(btSerializer * serializer)1551 void btCollisionWorld::serialize(btSerializer* serializer)
1552 {
1553
1554 serializer->startSerialization();
1555
1556 serializeCollisionObjects(serializer);
1557
1558 serializer->finishSerialization();
1559 }
1560
1561