2
Bullet Continuous Collision Detection and Physics Library
3
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
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"
38
//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
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
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"
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"
69
btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
70
:m_dispatcher1(dispatcher),
71
m_broadphasePairCache(pairCache),
73
m_forceUpdateAllAabbs(true)
75
m_stackAlloc = collisionConfiguration->getStackAllocator();
76
m_dispatchInfo.m_stackAllocator = m_stackAlloc;
80
btCollisionWorld::~btCollisionWorld()
83
//clean up remaining objects
85
for (i=0;i<m_collisionObjects.size();i++)
87
btCollisionObject* collisionObject= m_collisionObjects[i];
89
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
93
// only clear the cached algorithms
95
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
96
getBroadphase()->destroyProxy(bp,m_dispatcher1);
97
collisionObject->setBroadphaseHandle(0);
113
void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
116
btAssert(collisionObject);
118
//check that the object isn't already added
119
btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
121
m_collisionObjects.push_back(collisionObject);
124
btTransform trans = collisionObject->getWorldTransform();
128
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
130
int type = collisionObject->getCollisionShape()->getShapeType();
131
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
136
collisionFilterGroup,
149
void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
151
btVector3 minAabb,maxAabb;
152
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
153
//need to increase the aabb for contact thresholds
154
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
155
minAabb -= contactThreshold;
156
maxAabb += contactThreshold;
158
if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
160
btVector3 minAabb2,maxAabb2;
161
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
162
minAabb2 -= contactThreshold;
163
maxAabb2 += contactThreshold;
164
minAabb.setMin(minAabb2);
165
maxAabb.setMax(maxAabb2);
168
btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
170
//moving objects should be moderately sized, probably something wrong if not
171
if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
173
bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
176
//something went wrong, investigate
177
//this assert is unwanted in 3D modelers (danger of loosing work)
178
colObj->setActivationState(DISABLE_SIMULATION);
180
static bool reportMe = true;
181
if (reportMe && m_debugDrawer)
184
m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
185
m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
186
m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
187
m_debugDrawer->reportErrorWarning("Thanks.\n");
192
void btCollisionWorld::updateAabbs()
194
BT_PROFILE("updateAabbs");
196
btTransform predictedTrans;
197
for ( int i=0;i<m_collisionObjects.size();i++)
199
btCollisionObject* colObj = m_collisionObjects[i];
201
//only update aabb of active objects
202
if (m_forceUpdateAllAabbs || colObj->isActive())
204
updateSingleAabb(colObj);
211
void btCollisionWorld::performDiscreteCollisionDetection()
213
BT_PROFILE("performDiscreteCollisionDetection");
215
btDispatcherInfo& dispatchInfo = getDispatchInfo();
220
BT_PROFILE("calculateOverlappingPairs");
221
m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
225
btDispatcher* dispatcher = getDispatcher();
227
BT_PROFILE("dispatchAllCollisionPairs");
229
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
236
void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
240
//bool removeFromBroadphase = false;
244
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
248
// only clear the cached algorithms
250
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
251
getBroadphase()->destroyProxy(bp,m_dispatcher1);
252
collisionObject->setBroadphaseHandle(0);
258
m_collisionObjects.remove(collisionObject);
264
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
265
btCollisionObject* collisionObject,
266
const btCollisionShape* collisionShape,
267
const btTransform& colObjWorldTransform,
268
RayResultCallback& resultCallback)
270
btSphereShape pointShape(btScalar(0.0));
271
pointShape.setMargin(0.f);
272
const btConvexShape* castShape = &pointShape;
274
if (collisionShape->isConvex())
276
// BT_PROFILE("rayTestConvex");
277
btConvexCast::CastResult castResult;
278
castResult.m_fraction = resultCallback.m_closestHitFraction;
280
btConvexShape* convexShape = (btConvexShape*) collisionShape;
281
btVoronoiSimplexSolver simplexSolver;
282
#define USE_SUBSIMPLEX_CONVEX_CAST 1
283
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
284
btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
286
//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
287
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
288
#endif //#USE_SUBSIMPLEX_CONVEX_CAST
290
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
293
if (castResult.m_normal.length2() > btScalar(0.0001))
295
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
297
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
298
//rotate normal into worldspace
299
castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
300
#endif //USE_SUBSIMPLEX_CONVEX_CAST
302
castResult.m_normal.normalize();
303
btCollisionWorld::LocalRayResult localRayResult
308
castResult.m_fraction
311
bool normalInWorldSpace = true;
312
resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
318
if (collisionShape->isConcave())
320
// BT_PROFILE("rayTestConcave");
321
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
323
///optimized version for btBvhTriangleMeshShape
324
btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
325
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
326
btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
327
btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
329
//ConvexCast::CastResult
330
struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
332
btCollisionWorld::RayResultCallback* m_resultCallback;
333
btCollisionObject* m_collisionObject;
334
btTriangleMeshShape* m_triangleMesh;
336
btTransform m_colObjWorldTransform;
338
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
339
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
341
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
342
m_resultCallback(resultCallback),
343
m_collisionObject(collisionObject),
344
m_triangleMesh(triangleMesh),
345
m_colObjWorldTransform(colObjWorldTransform)
350
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
352
btCollisionWorld::LocalShapeInfo shapeInfo;
353
shapeInfo.m_shapePart = partId;
354
shapeInfo.m_triangleIndex = triangleIndex;
356
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
358
btCollisionWorld::LocalRayResult rayResult
364
bool normalInWorldSpace = true;
365
return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
370
BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
371
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
372
triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
375
//generic (slower) case
376
btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
378
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
380
btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
381
btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
383
//ConvexCast::CastResult
385
struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
387
btCollisionWorld::RayResultCallback* m_resultCallback;
388
btCollisionObject* m_collisionObject;
389
btConcaveShape* m_triangleMesh;
391
btTransform m_colObjWorldTransform;
393
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
394
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
396
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
397
m_resultCallback(resultCallback),
398
m_collisionObject(collisionObject),
399
m_triangleMesh(triangleMesh),
400
m_colObjWorldTransform(colObjWorldTransform)
405
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
407
btCollisionWorld::LocalShapeInfo shapeInfo;
408
shapeInfo.m_shapePart = partId;
409
shapeInfo.m_triangleIndex = triangleIndex;
411
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
413
btCollisionWorld::LocalRayResult rayResult
419
bool normalInWorldSpace = true;
420
return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
426
BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
427
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
429
btVector3 rayAabbMinLocal = rayFromLocal;
430
rayAabbMinLocal.setMin(rayToLocal);
431
btVector3 rayAabbMaxLocal = rayFromLocal;
432
rayAabbMaxLocal.setMax(rayToLocal);
434
concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
437
// BT_PROFILE("rayTestCompound");
438
if (collisionShape->isCompound())
440
struct LocalInfoAdder2 : public RayResultCallback
442
RayResultCallback* m_userCallback;
445
LocalInfoAdder2 (int i, RayResultCallback *user)
446
: m_userCallback(user), m_i(i)
448
m_closestHitFraction = m_userCallback->m_closestHitFraction;
450
virtual bool needsCollision(btBroadphaseProxy* p) const
452
return m_userCallback->needsCollision(p);
455
virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
457
btCollisionWorld::LocalShapeInfo shapeInfo;
458
shapeInfo.m_shapePart = -1;
459
shapeInfo.m_triangleIndex = m_i;
460
if (r.m_localShapeInfo == NULL)
461
r.m_localShapeInfo = &shapeInfo;
463
const btScalar result = m_userCallback->addSingleResult(r, b);
464
m_closestHitFraction = m_userCallback->m_closestHitFraction;
469
struct RayTester : btDbvt::ICollide
471
btCollisionObject* m_collisionObject;
472
const btCompoundShape* m_compoundShape;
473
const btTransform& m_colObjWorldTransform;
474
const btTransform& m_rayFromTrans;
475
const btTransform& m_rayToTrans;
476
RayResultCallback& m_resultCallback;
478
RayTester(btCollisionObject* collisionObject,
479
const btCompoundShape* compoundShape,
480
const btTransform& colObjWorldTransform,
481
const btTransform& rayFromTrans,
482
const btTransform& rayToTrans,
483
RayResultCallback& resultCallback):
484
m_collisionObject(collisionObject),
485
m_compoundShape(compoundShape),
486
m_colObjWorldTransform(colObjWorldTransform),
487
m_rayFromTrans(rayFromTrans),
488
m_rayToTrans(rayToTrans),
489
m_resultCallback(resultCallback)
496
const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
497
const btTransform& childTrans = m_compoundShape->getChildTransform(i);
498
btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
500
// replace collision shape so that callback can determine the triangle
501
btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
502
m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
504
LocalInfoAdder2 my_cb(i, &m_resultCallback);
515
m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
518
void Process(const btDbvtNode* leaf)
520
Process(leaf->dataAsInt);
524
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
525
const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
531
colObjWorldTransform,
535
#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
538
btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
539
btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
540
btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
543
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
545
for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
555
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
556
btCollisionObject* collisionObject,
557
const btCollisionShape* collisionShape,
558
const btTransform& colObjWorldTransform,
559
ConvexResultCallback& resultCallback, btScalar allowedPenetration)
561
if (collisionShape->isConvex())
563
//BT_PROFILE("convexSweepConvex");
564
btConvexCast::CastResult castResult;
565
castResult.m_allowedPenetration = allowedPenetration;
566
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
568
btConvexShape* convexShape = (btConvexShape*) collisionShape;
569
btVoronoiSimplexSolver simplexSolver;
570
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
572
btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
573
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
574
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
576
btConvexCast* castPtr = &convexCaster1;
580
if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
583
if (castResult.m_normal.length2() > btScalar(0.0001))
585
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
587
castResult.m_normal.normalize();
588
btCollisionWorld::LocalConvexResult localConvexResult
593
castResult.m_hitPoint,
594
castResult.m_fraction
597
bool normalInWorldSpace = true;
598
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
604
if (collisionShape->isConcave())
606
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
608
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
609
btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
610
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
611
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
612
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
613
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
614
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
616
//ConvexCast::CastResult
617
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
619
btCollisionWorld::ConvexResultCallback* m_resultCallback;
620
btCollisionObject* m_collisionObject;
621
btTriangleMeshShape* m_triangleMesh;
623
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
624
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
625
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
626
m_resultCallback(resultCallback),
627
m_collisionObject(collisionObject),
628
m_triangleMesh(triangleMesh)
633
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
635
btCollisionWorld::LocalShapeInfo shapeInfo;
636
shapeInfo.m_shapePart = partId;
637
shapeInfo.m_triangleIndex = triangleIndex;
638
if (hitFraction <= m_resultCallback->m_closestHitFraction)
641
btCollisionWorld::LocalConvexResult convexResult
648
bool normalInWorldSpace = true;
651
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
658
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
659
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
660
tccb.m_allowedPenetration = allowedPenetration;
661
btVector3 boxMinLocal, boxMaxLocal;
662
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
663
triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
666
if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
668
btConvexCast::CastResult castResult;
669
castResult.m_allowedPenetration = allowedPenetration;
670
castResult.m_fraction = resultCallback.m_closestHitFraction;
671
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
672
btContinuousConvexCollision convexCaster1(castShape,planeShape);
673
btConvexCast* castPtr = &convexCaster1;
675
if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
678
if (castResult.m_normal.length2() > btScalar(0.0001))
680
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
682
castResult.m_normal.normalize();
683
btCollisionWorld::LocalConvexResult localConvexResult
688
castResult.m_hitPoint,
689
castResult.m_fraction
692
bool normalInWorldSpace = true;
693
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
700
//BT_PROFILE("convexSweepConcave");
701
btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
702
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
703
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
704
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
705
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
706
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
708
//ConvexCast::CastResult
709
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
711
btCollisionWorld::ConvexResultCallback* m_resultCallback;
712
btCollisionObject* m_collisionObject;
713
btConcaveShape* m_triangleMesh;
715
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
716
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
717
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
718
m_resultCallback(resultCallback),
719
m_collisionObject(collisionObject),
720
m_triangleMesh(triangleMesh)
725
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
727
btCollisionWorld::LocalShapeInfo shapeInfo;
728
shapeInfo.m_shapePart = partId;
729
shapeInfo.m_triangleIndex = triangleIndex;
730
if (hitFraction <= m_resultCallback->m_closestHitFraction)
733
btCollisionWorld::LocalConvexResult convexResult
740
bool normalInWorldSpace = false;
742
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
749
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
750
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
751
tccb.m_allowedPenetration = allowedPenetration;
752
btVector3 boxMinLocal, boxMaxLocal;
753
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
755
btVector3 rayAabbMinLocal = convexFromLocal;
756
rayAabbMinLocal.setMin(convexToLocal);
757
btVector3 rayAabbMaxLocal = convexFromLocal;
758
rayAabbMaxLocal.setMax(convexToLocal);
759
rayAabbMinLocal += boxMinLocal;
760
rayAabbMaxLocal += boxMaxLocal;
761
concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
765
///@todo : use AABB tree or other BVH acceleration structure!
766
if (collisionShape->isCompound())
768
BT_PROFILE("convexSweepCompound");
769
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
771
for (i=0;i<compoundShape->getNumChildShapes();i++)
773
btTransform childTrans = compoundShape->getChildTransform(i);
774
const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
775
btTransform childWorldTrans = colObjWorldTransform * childTrans;
776
// replace collision shape so that callback can determine the triangle
777
btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
778
collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
779
struct LocalInfoAdder : public ConvexResultCallback {
780
ConvexResultCallback* m_userCallback;
783
LocalInfoAdder (int i, ConvexResultCallback *user)
784
: m_userCallback(user), m_i(i)
786
m_closestHitFraction = m_userCallback->m_closestHitFraction;
788
virtual bool needsCollision(btBroadphaseProxy* p) const
790
return m_userCallback->needsCollision(p);
792
virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
794
btCollisionWorld::LocalShapeInfo shapeInfo;
795
shapeInfo.m_shapePart = -1;
796
shapeInfo.m_triangleIndex = m_i;
797
if (r.m_localShapeInfo == NULL)
798
r.m_localShapeInfo = &shapeInfo;
799
const btScalar result = m_userCallback->addSingleResult(r, b);
800
m_closestHitFraction = m_userCallback->m_closestHitFraction;
806
LocalInfoAdder my_cb(i, &resultCallback);
809
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
813
my_cb, allowedPenetration);
815
collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
823
struct btSingleRayCallback : public btBroadphaseRayCallback
826
btVector3 m_rayFromWorld;
827
btVector3 m_rayToWorld;
828
btTransform m_rayFromTrans;
829
btTransform m_rayToTrans;
830
btVector3 m_hitNormal;
832
const btCollisionWorld* m_world;
833
btCollisionWorld::RayResultCallback& m_resultCallback;
835
btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
836
:m_rayFromWorld(rayFromWorld),
837
m_rayToWorld(rayToWorld),
839
m_resultCallback(resultCallback)
841
m_rayFromTrans.setIdentity();
842
m_rayFromTrans.setOrigin(m_rayFromWorld);
843
m_rayToTrans.setIdentity();
844
m_rayToTrans.setOrigin(m_rayToWorld);
846
btVector3 rayDir = (rayToWorld-rayFromWorld);
849
///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
850
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
851
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
852
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
853
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
854
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
855
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
857
m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
863
virtual bool process(const btBroadphaseProxy* proxy)
865
///terminate further ray tests, once the closestHitFraction reached zero
866
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
869
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
871
//only perform raycast if filterMask matches
872
if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
874
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
875
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
877
#ifdef RECALCULATE_AABB
878
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
879
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
881
//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
882
const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
883
const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
886
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
887
//culling already done by broadphase
888
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
890
m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
892
collisionObject->getCollisionShape(),
893
collisionObject->getWorldTransform(),
901
void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
903
//BT_PROFILE("rayTest");
904
/// use the broadphase to accelerate the search for objects, based on their aabb
905
/// and for each object with ray-aabb overlap, perform an exact ray test
906
btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
908
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
909
m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
911
for (int i=0;i<this->getNumCollisionObjects();i++)
913
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
915
#endif //USE_BRUTEFORCE_RAYBROADPHASE
920
struct btSingleSweepCallback : public btBroadphaseRayCallback
923
btTransform m_convexFromTrans;
924
btTransform m_convexToTrans;
925
btVector3 m_hitNormal;
926
const btCollisionWorld* m_world;
927
btCollisionWorld::ConvexResultCallback& m_resultCallback;
928
btScalar m_allowedCcdPenetration;
929
const btConvexShape* m_castShape;
932
btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
933
:m_convexFromTrans(convexFromTrans),
934
m_convexToTrans(convexToTrans),
936
m_resultCallback(resultCallback),
937
m_allowedCcdPenetration(allowedPenetration),
938
m_castShape(castShape)
940
btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
941
btVector3 rayDir = unnormalizedRayDir.normalized();
942
///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
943
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
944
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
945
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
946
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
947
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
948
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
950
m_lambda_max = rayDir.dot(unnormalizedRayDir);
954
virtual bool process(const btBroadphaseProxy* proxy)
956
///terminate further convex sweep tests, once the closestHitFraction reached zero
957
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
960
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
962
//only perform raycast if filterMask matches
963
if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
964
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
965
m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
967
collisionObject->getCollisionShape(),
968
collisionObject->getWorldTransform(),
970
m_allowedCcdPenetration);
979
void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
982
BT_PROFILE("convexSweepTest");
983
/// use the broadphase to accelerate the search for objects, based on their aabb
984
/// and for each object with ray-aabb overlap, perform an exact ray test
985
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
989
btTransform convexFromTrans,convexToTrans;
990
convexFromTrans = convexFromWorld;
991
convexToTrans = convexToWorld;
992
btVector3 castShapeAabbMin, castShapeAabbMax;
993
/* Compute AABB that encompasses angular movement */
995
btVector3 linVel, angVel;
996
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
997
btVector3 zeroLinVel;
998
zeroLinVel.setValue(0,0,0);
1001
R.setRotation (convexFromTrans.getRotation());
1002
castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
1005
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
1007
btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
1009
m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
1012
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
1013
// do a ray-shape query using convexCaster (CCD)
1015
for (i=0;i<m_collisionObjects.size();i++)
1017
btCollisionObject* collisionObject= m_collisionObjects[i];
1018
//only perform raycast if filterMask matches
1019
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
1020
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
1021
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
1022
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
1023
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
1024
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
1025
btVector3 hitNormal;
1026
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
1028
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
1030
collisionObject->getCollisionShape(),
1031
collisionObject->getWorldTransform(),
1033
allowedCcdPenetration);
1037
#endif //USE_BRUTEFORCE_RAYBROADPHASE
1042
struct btBridgedManifoldResult : public btManifoldResult
1045
btCollisionWorld::ContactResultCallback& m_resultCallback;
1047
btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
1048
:btManifoldResult(obj0,obj1),
1049
m_resultCallback(resultCallback)
1053
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
1055
bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
1056
btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
1061
localA = m_rootTransB.invXform(pointA );
1062
localB = m_rootTransA.invXform(pointInWorld);
1065
localA = m_rootTransA.invXform(pointA );
1066
localB = m_rootTransB.invXform(pointInWorld);
1069
btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
1070
newPt.m_positionWorldOnA = pointA;
1071
newPt.m_positionWorldOnB = pointInWorld;
1073
//BP mod, store contact triangles.
1076
newPt.m_partId0 = m_partId1;
1077
newPt.m_partId1 = m_partId0;
1078
newPt.m_index0 = m_index1;
1079
newPt.m_index1 = m_index0;
1082
newPt.m_partId0 = m_partId0;
1083
newPt.m_partId1 = m_partId1;
1084
newPt.m_index0 = m_index0;
1085
newPt.m_index1 = m_index1;
1088
//experimental feature info, for per-triangle material etc.
1089
btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
1090
btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
1091
m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
1099
struct btSingleContactCallback : public btBroadphaseAabbCallback
1102
btCollisionObject* m_collisionObject;
1103
btCollisionWorld* m_world;
1104
btCollisionWorld::ContactResultCallback& m_resultCallback;
1107
btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
1108
:m_collisionObject(collisionObject),
1110
m_resultCallback(resultCallback)
1114
virtual bool process(const btBroadphaseProxy* proxy)
1116
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
1117
if (collisionObject == m_collisionObject)
1120
//only perform raycast if filterMask matches
1121
if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
1123
btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
1126
btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
1127
//discrete collision detection query
1128
algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
1130
algorithm->~btCollisionAlgorithm();
1131
m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
1139
///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
1140
///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
1141
void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
1143
btVector3 aabbMin,aabbMax;
1144
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
1145
btSingleContactCallback contactCB(colObj,this,resultCallback);
1147
m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
1151
///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
1152
///it reports one or more contact points (including the one with deepest penetration)
1153
void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
1155
btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
1158
btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
1159
//discrete collision detection query
1160
algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
1162
algorithm->~btCollisionAlgorithm();
1163
getDispatcher()->freeCollisionAlgorithm(algorithm);
1171
class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
1173
btIDebugDraw* m_debugDrawer;
1175
btTransform m_worldTrans;
1179
DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
1180
m_debugDrawer(debugDrawer),
1182
m_worldTrans(worldTrans)
1186
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
1188
processTriangle(triangle,partId,triangleIndex);
1191
virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
1194
(void)triangleIndex;
1196
btVector3 wv0,wv1,wv2;
1197
wv0 = m_worldTrans*triangle[0];
1198
wv1 = m_worldTrans*triangle[1];
1199
wv2 = m_worldTrans*triangle[2];
1200
btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
1202
btVector3 normal = (wv1-wv0).cross(wv2-wv0);
1204
btVector3 normalColor(1,1,0);
1205
m_debugDrawer->drawLine(center,center+normal,normalColor);
1210
m_debugDrawer->drawLine(wv0,wv1,m_color);
1211
m_debugDrawer->drawLine(wv1,wv2,m_color);
1212
m_debugDrawer->drawLine(wv2,wv0,m_color);
1217
void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1219
// Draw a small simplex at the center of the object
1220
getDebugDrawer()->drawTransform(worldTransform,1);
1222
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1224
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1225
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1227
btTransform childTrans = compoundShape->getChildTransform(i);
1228
const btCollisionShape* colShape = compoundShape->getChildShape(i);
1229
debugDrawObject(worldTransform*childTrans,colShape,color);
1234
switch (shape->getShapeType())
1237
case BOX_SHAPE_PROXYTYPE:
1239
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
1240
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
1241
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
1245
case SPHERE_SHAPE_PROXYTYPE:
1247
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1248
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1250
getDebugDrawer()->drawSphere(radius, worldTransform, color);
1253
case MULTI_SPHERE_SHAPE_PROXYTYPE:
1255
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1257
btTransform childTransform;
1258
childTransform.setIdentity();
1260
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1262
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
1263
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
1268
case CAPSULE_SHAPE_PROXYTYPE:
1270
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1272
btScalar radius = capsuleShape->getRadius();
1273
btScalar halfHeight = capsuleShape->getHalfHeight();
1275
int upAxis = capsuleShape->getUpAxis();
1276
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
1279
case CONE_SHAPE_PROXYTYPE:
1281
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1282
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1283
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1285
int upAxis= coneShape->getConeUpIndex();
1286
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
1290
case CYLINDER_SHAPE_PROXYTYPE:
1292
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1293
int upAxis = cylinder->getUpAxis();
1294
btScalar radius = cylinder->getRadius();
1295
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1296
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
1300
case STATIC_PLANE_PROXYTYPE:
1302
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1303
btScalar planeConst = staticPlaneShape->getPlaneConstant();
1304
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1305
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
1312
if (shape->isConcave())
1314
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1316
///@todo pass camera, for some culling? no -> we are not a graphics lib
1317
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1318
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1320
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1321
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1325
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1327
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1328
//todo: pass camera for some culling
1329
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
1330
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
1331
//DebugDrawcallback drawCallback;
1332
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1333
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1337
/// for polyhedral shapes
1338
if (shape->isPolyhedral())
1340
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1343
if (polyshape->getConvexPolyhedron())
1345
const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
1346
for (i=0;i<poly->m_faces.size();i++)
1348
btVector3 centroid(0,0,0);
1349
int numVerts = poly->m_faces[i].m_indices.size();
1352
int lastV = poly->m_faces[i].m_indices[numVerts-1];
1353
for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
1355
int curVert = poly->m_faces[i].m_indices[v];
1356
centroid+=poly->m_vertices[curVert];
1357
getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
1361
centroid*= 1./btScalar(numVerts);
1363
btVector3 normalColor(1,1,0);
1364
btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
1365
getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
1373
for (i=0;i<polyshape->getNumEdges();i++)
1376
polyshape->getEdge(i,a,b);
1377
btVector3 wa = worldTransform * a;
1378
btVector3 wb = worldTransform * b;
1379
getDebugDrawer()->drawLine(wa,wb,color);
1391
void btCollisionWorld::debugDrawWorld()
1393
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
1395
int numManifolds = getDispatcher()->getNumManifolds();
1396
btVector3 color(0,0,0);
1397
for (int i=0;i<numManifolds;i++)
1399
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
1400
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
1401
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
1403
int numContacts = contactManifold->getNumContacts();
1404
for (int j=0;j<numContacts;j++)
1406
btManifoldPoint& cp = contactManifold->getContactPoint(j);
1407
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1412
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
1416
for ( i=0;i<m_collisionObjects.size();i++)
1418
btCollisionObject* colObj = m_collisionObjects[i];
1419
if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
1421
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
1423
btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
1424
switch(colObj->getActivationState())
1427
color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
1428
case ISLAND_SLEEPING:
1429
color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
1430
case WANTS_DEACTIVATION:
1431
color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
1432
case DISABLE_DEACTIVATION:
1433
color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
1434
case DISABLE_SIMULATION:
1435
color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
1438
color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
1442
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
1444
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
1446
btVector3 minAabb,maxAabb;
1447
btVector3 colorvec(1,0,0);
1448
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
1449
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
1450
minAabb -= contactThreshold;
1451
maxAabb += contactThreshold;
1453
btVector3 minAabb2,maxAabb2;
1455
if(colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
1457
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
1458
minAabb2 -= contactThreshold;
1459
maxAabb2 += contactThreshold;
1460
minAabb.setMin(minAabb2);
1461
maxAabb.setMax(maxAabb2);
1464
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
1473
void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
1476
//serialize all collision objects
1477
for (i=0;i<m_collisionObjects.size();i++)
1479
btCollisionObject* colObj = m_collisionObjects[i];
1480
if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
1482
colObj->serializeSingleObject(serializer);
1486
///keep track of shapes already serialized
1487
btHashMap<btHashPtr,btCollisionShape*> serializedShapes;
1489
for (i=0;i<m_collisionObjects.size();i++)
1491
btCollisionObject* colObj = m_collisionObjects[i];
1492
btCollisionShape* shape = colObj->getCollisionShape();
1494
if (!serializedShapes.find(shape))
1496
serializedShapes.insert(shape,shape);
1497
shape->serializeSingleShape(serializer);
1504
void btCollisionWorld::serialize(btSerializer* serializer)
1507
serializer->startSerialization();
1509
serializeCollisionObjects(serializer);
1511
serializer->finishSerialization();
added
removed
tests/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
