Make raycast benchmark default, to show improved raycasting performance.

Add rayTest to btBroadphaseInterface, and implement efficient version for btDbvtBroadphase to accelerate raycasting.
btAxisSweep3, btSimpleBroadphase and btMultiSapBroadphase implement brute-force method (as before). For now, it is recommended to use btDbvtBroadphase for fastest world raycast.

Demos/Benchmarks/BenchmarkDemo.cpp

@@ -273,9 +273,11 @@ void	BenchmarkDemo::initPhysics()
 	btVector3 worldAabbMin(-10000,-10000,-10000);
 	btVector3 worldAabbMax(10000,10000,10000);
 //	m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,3500);
-
+//	m_overlappingPairCache = new btSimpleBroadphase();
 	m_overlappingPairCache = new btDbvtBroadphase();
 	
+
+
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
 	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
 	

Demos/Benchmarks/main.cpp

@@ -26,13 +26,13 @@ int main(int argc,char** argv)
 {
 	GLDebugDrawer	gDebugDrawer;
 
-	BenchmarkDemo1 benchmarkDemo;
+//	BenchmarkDemo1 benchmarkDemo;
 //	BenchmarkDemo2 benchmarkDemo;
 //	BenchmarkDemo3 benchmarkDemo;
 //	BenchmarkDemo4 benchmarkDemo;
 //	BenchmarkDemo5 benchmarkDemo;
 //	BenchmarkDemo6 benchmarkDemo;
-//	BenchmarkDemo7 benchmarkDemo;
+	BenchmarkDemo7 benchmarkDemo;
 
 
 	benchmarkDemo.initPhysics();

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -140,6 +140,8 @@ public:
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 	
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
 	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
 	///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
 	void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
@@ -244,6 +246,23 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,cons
 
 }
 
+template <typename BP_FP_INT_TYPE>
+
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	//choose axis?
+	BP_FP_INT_TYPE axis = 0;
+	//for each proxy
+	for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
+	{
+		if (m_pEdges[axis][i].IsMax())
+		{
+			rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+		}
+	}
+}
+
+
 template <typename BP_FP_INT_TYPE>
 void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
 {

src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -23,6 +23,11 @@ class btDispatcher;
 #include "btBroadphaseProxy.h"
 class btOverlappingPairCache;
 
+struct	btBroadphaseRayCallback
+{
+	virtual bool	process(const btBroadphaseProxy* proxy) = 0;
+};
+
 #include "LinearMath/btVector3.h"
 
 ///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
@@ -38,6 +43,8 @@ public:
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
 	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
 
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback) = 0;
+
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;
 

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -210,6 +210,37 @@ void	btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, b
 	aabbMax = proxy->m_aabbMax;
 }
 
+void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+
+	struct	BroadphaseRayTester : btDbvt::ICollide
+	{
+		btBroadphaseRayCallback& m_rayCallback;
+		BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
+			:m_rayCallback(orgCallback)
+		{
+		}
+		void					Process(const btDbvtNode* leaf)
+		{
+			btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+			m_rayCallback.process(proxy);
+		}
+	};	
+
+	BroadphaseRayTester callback(rayCallback);
+
+	m_sets[0].collideRAY(	m_sets[0].m_root,
+								rayFrom,
+								rayTo,
+								callback);
+
+	m_sets[1].collideRAY(	m_sets[1].m_root,
+								rayFrom,
+								rayTo,
+								callback);
+
+}
+
 //
 void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 																const btVector3& aabbMin,

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -105,6 +105,8 @@ void							optimize();
 btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
 void							destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 void							setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
 virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 void							calculateOverlappingPairs(btDispatcher* dispatcher);
 btOverlappingPairCache*			getOverlappingPairCache();

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -156,6 +156,14 @@ void	btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin,
 	aabbMax = multiProxy->m_aabbMax;
 }
 
+void	btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	for (int i=0;i<m_multiSapProxies.size();i++)
+	{
+		rayCallback.process(m_multiSapProxies[i]);
+	}
+}
+
 
 //#include <stdio.h>
 

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -110,6 +110,8 @@ public:
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
 	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
 
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -151,7 +151,14 @@ void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbM
 	sbp->m_aabbMax = aabbMax;
 }
 
-
+void	btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	for (int i=0;i<m_numHandles;i++)
+	{
+		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+		rayCallback.process(proxy);
+	}
+}
 
 
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -122,6 +122,8 @@ public:
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
 
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+		
 	btOverlappingPairCache*	getOverlappingPairCache()
 	{
 		return m_pairCache;

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -599,52 +599,70 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
 	}
 }
 
-void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+
+struct btSingleRayCallback : public btBroadphaseRayCallback
 {
-	BT_PROFILE("rayTest");
 
+	btVector3	m_rayFromWorld;
+	btVector3	m_rayToWorld;
+	const btCollisionWorld*	m_world;
+	btCollisionWorld::RayResultCallback&	m_resultCallback;
 
-	btTransform	rayFromTrans,rayToTrans;
+	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-	rayFromTrans.setIdentity();
+	:m_rayFromWorld(rayFromWorld),
-	rayFromTrans.setOrigin(rayFromWorld);
+	m_rayToWorld(rayToWorld),
-	rayToTrans.setIdentity();
+	m_world(world),
+	m_resultCallback(resultCallback)
+	{
 
-	rayToTrans.setOrigin(rayToWorld);
+	}
 
-	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
+	virtual bool	process(const btBroadphaseProxy* proxy)
-
-	int i;
-	for (i=0;i<m_collisionObjects.size();i++)
 	{
 		///terminate further ray tests, once the closestHitFraction reached zero
-		if (resultCallback.m_closestHitFraction == btScalar(0.f))
+		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
-			break;
+			return false;
+
+		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
 
-		btCollisionObject*	collisionObject= m_collisionObjects[i];
 		//only perform raycast if filterMask matches
-		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
 		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 			//collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 
-			btScalar hitLambda = resultCallback.m_closestHitFraction;
+			btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 			btVector3 hitNormal;
 			{
-				if (btRayAabb(rayFromWorld,rayToWorld,collisionObject->getBroadphaseHandle()->m_aabbMin,collisionObject->getBroadphaseHandle()->m_aabbMax,hitLambda,hitNormal))
+				if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObject->getBroadphaseHandle()->m_aabbMin,collisionObject->getBroadphaseHandle()->m_aabbMax,hitLambda,hitNormal))
 				{
-					rayTestSingle(rayFromTrans,rayToTrans,
+					btTransform	rayFromTrans,rayToTrans;
+					rayFromTrans.setIdentity();
+					rayFromTrans.setOrigin(m_rayFromWorld);
+					rayToTrans.setIdentity();
+					rayToTrans.setOrigin(m_rayToWorld);
+
+					m_world->rayTestSingle(rayFromTrans,rayToTrans,
 						collisionObject,
 							collisionObject->getCollisionShape(),
 							collisionObject->getWorldTransform(),
-							resultCallback);
+							m_resultCallback);
 				}
 
 			}
 		}
-
+		return true;
 	}
+};
+
+void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+	BT_PROFILE("rayTest");
+	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
+	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
 
 }