From bb8d1b11df1b6588d808b9fd79c87cca5a9a4cf9 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Thu, 11 Feb 2010 18:25:50 +0000 Subject: [PATCH] Add btBroadphaseInterface::aabbTest. btDbvtBroadphase and btAxisSweep3 should perform well, as long as the raycast accelerator is enabled. This should fix the btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); Thanks to Ola for the report! --- .../CollisionInterfaceDemo.cpp | 30 +++++++++++++++++-- .../BroadphaseCollision/btAxisSweep3.h | 27 +++++++++++++++++ .../btBroadphaseInterface.h | 12 ++++++-- .../BroadphaseCollision/btDbvtBroadphase.cpp | 26 ++++++++++++++++ .../BroadphaseCollision/btDbvtBroadphase.h | 1 + .../btSimpleBroadphase.cpp | 19 ++++++++++++ .../BroadphaseCollision/btSimpleBroadphase.h | 1 + .../CollisionDispatch/btCollisionWorld.cpp | 6 ++-- 8 files changed, 115 insertions(+), 7 deletions(-) diff --git a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp index 2e3595a14..eaa4b922f 100644 --- a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp +++ b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp @@ -83,11 +83,10 @@ void CollisionInterfaceDemo::initPhysics() collisionWorld->setDebugDrawer(&debugDrawer); #ifdef TEST_NOT_ADDING_OBJECTS_TO_WORLD - collisionWorld->addCollisionObject(&objects[0]); +// collisionWorld->addCollisionObject(&objects[0]); collisionWorld->addCollisionObject(&objects[1]); #endif //TEST_NOT_ADDING_OBJECTS_TO_WORLD - } @@ -103,7 +102,24 @@ void CollisionInterfaceDemo::clientMoveAndDisplay() static btVoronoiSimplexSolver sGjkSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; +struct btDrawingResult : public btCollisionWorld::ContactResultCallback +{ + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) + { + glBegin(GL_LINES); + glColor3f(0, 0, 0); + + btVector3 ptA = cp.getPositionWorldOnA(); + btVector3 ptB = cp.getPositionWorldOnB(); + + glVertex3d(ptA.x(),ptA.y(),ptA.z()); + glVertex3d(ptB.x(),ptB.y(),ptB.z()); + glEnd(); + + return 0; + } +}; void CollisionInterfaceDemo::displayCallback(void) { @@ -166,12 +182,20 @@ void CollisionInterfaceDemo::displayCallback(void) { } #else + glDisable(GL_TEXTURE_2D); for (i=0;idebugDrawObject(objects[i].getWorldTransform(),objects[i].getCollisionShape(), btVector3(1,1,0)); } + btDrawingResult renderCallback; + + //collisionWorld->contactPairTest(&objects[0],&objects[1], renderCallback); + collisionWorld->contactTest(&objects[0],renderCallback); + +#if 0 + //another way is to directly query the dispatcher for both objects. The objects don't need to be inserted into the world btCollisionAlgorithm* algo = collisionWorld->getDispatcher()->findAlgorithm(&objects[0],&objects[1]); @@ -210,6 +234,8 @@ void CollisionInterfaceDemo::displayCallback(void) { //you can un-comment out this line, and then all points are removed //contactManifold->clearManifold(); } +#endif + #endif diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index cad21b4ca..07167af3b 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -150,6 +150,8 @@ public: virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result @@ -285,6 +287,31 @@ void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,cons } } +template +void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); + } else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i=1;im_aabbMin,handle->m_aabbMax)) + { + callback.process(handle); + } + } + } + } +} + template diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 1f036bd44..fe414effb 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -26,7 +26,14 @@ class btOverlappingPairCache; -struct btBroadphaseRayCallback +struct btBroadphaseAabbCallback +{ + virtual ~btBroadphaseAabbCallback() {} + virtual bool process(const btBroadphaseProxy* proxy) = 0; +}; + + +struct btBroadphaseRayCallback : public btBroadphaseAabbCallback { ///added some cached data to accelerate ray-AABB tests btVector3 m_rayDirectionInverse; @@ -34,7 +41,6 @@ struct btBroadphaseRayCallback btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} - virtual bool process(const btBroadphaseProxy* proxy) = 0; }; #include "LinearMath/btVector3.h" @@ -54,6 +60,8 @@ public: virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0; + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index b6b8d40cf..75cfac643 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -251,6 +251,32 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, } +struct BroadphaseAabbTester : btDbvt::ICollide +{ + btBroadphaseAabbCallback& m_aabbCallback; + BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) + :m_aabbCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_aabbCallback.process(proxy); + } +}; + +void btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback) +{ + BroadphaseAabbTester callback(aabbCallback); + + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); + m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); + +} + + // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 6c5657181..18b64ad0e 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -108,6 +108,7 @@ struct btDbvtBroadphase : btBroadphaseInterface virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void calculateOverlappingPairs(btDispatcher* dispatcher); diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index caed63db0..752fcd0fe 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAabbUtil2.h" + #include extern int gOverlappingPairs; @@ -166,6 +168,23 @@ void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo } +void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + for (int i=0; i <= m_LastHandleIndex; i++) + { + btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; + if(!proxy->m_clientObject) + { + continue; + } + if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax)) + { + callback.process(proxy); + } + } +} + + diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index deffb0a7a..3e7c7ee3b 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -136,6 +136,7 @@ public: virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); btOverlappingPairCache* getOverlappingPairCache() { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index a81fc505c..e20317e50 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -965,7 +965,7 @@ struct btBridgedManifoldResult : public btManifoldResult -struct btSingleContactCallback : public btBroadphaseRayCallback +struct btSingleContactCallback : public btBroadphaseAabbCallback { btCollisionObject* m_collisionObject; @@ -1012,8 +1012,8 @@ void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCall btVector3 aabbMin,aabbMax; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); btSingleContactCallback contactCB(colObj,this,resultCallback); - - m_broadphasePairCache->rayTest(colObj->getWorldTransform().getOrigin(),colObj->getWorldTransform().getOrigin(),contactCB,aabbMin,aabbMax); + + m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB); }