Fix/improve raycast performance for btBvhTriangleMeshShape (btQuantizedBvh, btOptimizedBvh)

Add raycast acceleration for btAxisSweep3, using btDbvtBroadphase, providing large speedup. Can be disabled in constructor to safe memory.
2008-10-14 20:24:28 +00:00
parent fffca75e9f
commit e6c850f13b
7 changed files with 207 additions and 61 deletions
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
@@ -21,8 +21,8 @@

 #include <assert.h>

-btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache)
-:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache)
+btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
 {
 	// 1 handle is reserved as sentinel
 	btAssert(maxHandles > 1 && maxHandles < 32767);
@@ -30,8 +30,8 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab
 }


-bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache )
-:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache)
+bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
 {
 	// 1 handle is reserved as sentinel
 	btAssert(maxHandles > 1 && maxHandles < 2147483647);
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -25,6 +25,7 @@
 #include "btBroadphaseInterface.h"
 #include "btBroadphaseProxy.h"
 #include "btOverlappingPairCallback.h"
+#include "btDbvtBroadphase.h"

 //#define DEBUG_BROADPHASE 1
 #define USE_OVERLAP_TEST_ON_REMOVES 1
@@ -61,7 +62,7 @@ public:
 		// indexes into the edge arrays
 		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
 //		BP_FP_INT_TYPE m_uniqueId;
-		BP_FP_INT_TYPE m_pad;
+		btBroadphaseProxy*	m_dbvtProxy;//for faster raycast
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
 	
 		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
@@ -93,6 +94,11 @@ protected:

 	int	m_invalidPair;

+	///additional dynamic aabb structure, used to accelerate ray cast queries.
+	///can be disabled using a optional argument in the constructor
+	btDbvtBroadphase*	m_raycastAccelerator;
+
+
 	// allocation/deallocation
 	BP_FP_INT_TYPE allocHandle();
 	void freeHandle(BP_FP_INT_TYPE handle);
@@ -116,7 +122,7 @@ protected:

 public:

-	btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0);
+	btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);

 	virtual	~btAxisSweep3Internal();

@@ -224,6 +230,11 @@ btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btV
 		
 		Handle* handle = getHandle(handleId);
 		
+		if (m_raycastAccelerator)
+		{
+			btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
+			handle->m_dbvtProxy = rayProxy;
+		}
 		return handle;
 }

@@ -233,6 +244,8 @@ template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
 	removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
 }

@@ -243,6 +256,8 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,cons
 	handle->m_aabbMin = aabbMin;
 	handle->m_aabbMax = aabbMax;
 	updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);

 }

@@ -250,6 +265,11 @@ template <typename BP_FP_INT_TYPE>

 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
 {
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback);
+	} else
+	{
 		//choose axis?
 		BP_FP_INT_TYPE axis = 0;
 		//for each proxy
@@ -260,6 +280,7 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,cons
 				rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
 			}
 		}
+	}
 }


@@ -298,13 +319,14 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,b


 template <typename BP_FP_INT_TYPE>
-btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache )
+btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
 :m_bpHandleMask(handleMask),
 m_handleSentinel(handleSentinel),
 m_pairCache(pairCache),
 m_userPairCallback(0),
 m_ownsPairCache(false),
-m_invalidPair(0)
+m_invalidPair(0),
+m_raycastAccelerator(0)
 {
 	BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle

@@ -315,6 +337,12 @@ m_invalidPair(0)
 		m_ownsPairCache = true;
 	}

+	if (!disableRaycastAccelerator)
+	{
+		m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase();//m_pairCache);
+		m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
+	}
+
 	//assert(bounds.HasVolume());

 	// init bounds
@@ -375,6 +403,8 @@ m_invalidPair(0)
 template <typename BP_FP_INT_TYPE>
 btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
 {
+	if (m_raycastAccelerator)
+		btAlignedFree (m_raycastAccelerator);

 	for (int i = 2; i >= 0; i--)
 	{
@@ -954,7 +984,7 @@ class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
 {
 public:

-	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
+	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);

 };

@@ -965,7 +995,7 @@ class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
 {
 public:

-	bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
+	bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);

 };

--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
@@ -25,6 +25,7 @@ class btOverlappingPairCache;

 struct	btBroadphaseRayCallback
 {
+	virtual ~btBroadphaseRayCallback() {}
 	virtual bool	process(const btBroadphaseProxy* proxy) = 0;
 };

--- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btIDebugDraw.h"

+#define RAYAABB2

 btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), 
 					//m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
@@ -25,7 +26,8 @@ btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false),
 					//m_traversalMode(TRAVERSAL_RECURSIVE)
 					,m_subtreeHeaderCount(0) //PCK: add this line
 {
-
+	m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
+	m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
 }


@@ -119,7 +121,7 @@ void	btQuantizedBvh::buildTree	(int startIndex,int endIndex)
 	int numIndices =endIndex-startIndex;
 	int curIndex = m_curNodeIndex;

-	assert(numIndices>0);
+	btAssert(numIndices>0);

 	if (numIndices==1)
 	{
@@ -140,8 +142,11 @@ void	btQuantizedBvh::buildTree	(int startIndex,int endIndex)

 	int internalNodeIndex = m_curNodeIndex;
 	
-	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
-	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
+	//set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
+	//the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
+	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
+	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
+	
 	
 	for (i=startIndex;i<endIndex;i++)
 	{
@@ -177,6 +182,9 @@ void	btQuantizedBvh::buildTree	(int startIndex,int endIndex)
 		{
 			updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
 		}
+	} else
+	{
+
 	}

 	setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
@@ -338,6 +346,7 @@ void	btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb

 int maxIterations = 0;

+
 void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
 	btAssert(!m_useQuantization);
@@ -352,7 +361,7 @@ void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const
 	while (curIndex < m_curNodeIndex)
 	{
 		//catch bugs in tree data
-		assert (walkIterations < m_curNodeIndex);
+		btAssert (walkIterations < m_curNodeIndex);

 		walkIterations++;
 		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
@@ -434,6 +443,96 @@ void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize



+void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+{
+	btAssert(!m_useQuantization);
+
+	const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned aabbOverlap=0;
+	unsigned rayBoxOverlap=0;
+	btScalar lambda_max = 1.0;
+	
+		/* Quick pruning by quantized box */
+	btVector3 rayAabbMin = raySource;
+	btVector3 rayAabbMax = raySource;
+	rayAabbMin.setMin(rayTarget);
+	rayAabbMax.setMax(rayTarget);
+
+	/* Add box cast extents to bounding box */
+	rayAabbMin += aabbMin;
+	rayAabbMax += aabbMax;
+
+#ifdef RAYAABB2
+	btVector3 rayFrom = raySource;
+	btVector3 rayDirection = (rayTarget-raySource);
+	rayDirection.normalize ();
+	lambda_max = rayDirection.dot(rayTarget-raySource);
+	///what about division by zero? --> just set rayDirection[i] to 1.0
+	rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0];
+	rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1];
+	rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2];
+	unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
+#endif
+
+	btVector3 bounds[2];
+
+	while (curIndex < m_curNodeIndex)
+	{
+		btScalar param = 1.0;
+		//catch bugs in tree data
+		btAssert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+
+		bounds[0] = rootNode->m_aabbMinOrg;
+		bounds[1] = rootNode->m_aabbMaxOrg;
+		/* Add box cast extents */
+		bounds[0] += aabbMin;
+		bounds[1] += aabbMax;
+
+		aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		//perhaps profile if it is worth doing the aabbOverlap test first
+
+#ifdef RAYAABB2
+			///careful with this check: need to check division by zero (above) and fix the unQuantize method
+			///thanks Joerg/hiker for the reproduction case!
+			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
+		rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max) : false;
+
+#else
+		btVector3 normal;
+		rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
+#endif
+
+		isLeafNode = rootNode->m_escapeIndex == -1;
+		
+		//PCK: unsigned instead of bool
+		if (isLeafNode && (rayBoxOverlap != 0))
+		{
+			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+		} 
+		
+		//PCK: unsigned instead of bool
+		if ((rayBoxOverlap != 0) || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->m_escapeIndex;
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+


 void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
@@ -454,7 +553,7 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 	unsigned rayBoxOverlap = 0;

 	btScalar lambda_max = 1.0;
-#define RAYAABB2
+
 #ifdef RAYAABB2
 	btVector3 rayFrom = raySource;
 	btVector3 rayDirection = (rayTarget-raySource);
@@ -502,7 +601,7 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 #endif//VISUALLY_ANALYZE_BVH

 		//catch bugs in tree data
-		assert (walkIterations < subTreeSize);
+		btAssert (walkIterations < subTreeSize);

 		walkIterations++;
 		//PCK: unsigned instead of bool
@@ -533,7 +632,9 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 			///thanks Joerg/hiker for the reproduction case!
 			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858

+			//BT_PROFILE("btRayAabb2");
 			rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
+			
 #else
 			rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
 #endif
@@ -597,7 +698,7 @@ void	btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 #endif//VISUALLY_ANALYZE_BVH

 		//catch bugs in tree data
-		assert (walkIterations < subTreeSize);
+		btAssert (walkIterations < subTreeSize);

 		walkIterations++;
 		//PCK: unsigned instead of bool
@@ -652,30 +753,25 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba

 void	btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
 {
-	bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
-	if (fast_path)
-	{
-		walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex);
-	} else {
-		/* Otherwise fallback to AABB overlap test */
-		btVector3 aabbMin = raySource;
-		btVector3 aabbMax = raySource;
-		aabbMin.setMin(rayTarget);
-		aabbMax.setMax(rayTarget);
-		reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax);
-	}
+	reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
 }


 void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
 {
-	bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
-	if (fast_path)
+	//always use stackless
+
+	if (m_useQuantization)
 	{
 		walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
-	} else {
-		/* Slow path:
-		   Construct the bounding box for the entire box cast and send that down the tree */
+	}
+	else
+	{
+		walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+	}
+	/*
+	{
+		//recursive traversal
 		btVector3 qaabbMin = raySource;
 		btVector3 qaabbMax = raySource;
 		qaabbMin.setMin(rayTarget);
@@ -684,6 +780,8 @@ void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa
 		qaabbMax += aabbMax;
 		reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
 	}
+	*/
+
 }


@@ -743,7 +841,7 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize()

 bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
 {
-	assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
+	btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();

 /*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
--- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
+++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
@@ -296,6 +296,7 @@ protected:

 	void	walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
 	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
+	void	walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;

 	///tree traversal designed for small-memory processors like PS3 SPU
 	void	walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -32,6 +32,9 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btStackAlloc.h"

+//#define USE_BRUTEFORCE_RAYBROADPHASE 1
+//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
+//#define RECALCULATE_AABB_RAYCAST 1

 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -630,13 +633,18 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			//collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
-			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 			
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
 			btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 			btVector3 hitNormal;
-			{
-				if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObject->getBroadphaseHandle()->m_aabbMin,collisionObject->getBroadphaseHandle()->m_aabbMax,hitLambda,hitNormal))
+			if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
 			{
 				btTransform	rayFromTrans,rayToTrans;
 				rayFromTrans.setIdentity();
@@ -650,8 +658,6 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 						collisionObject->getWorldTransform(),
 						m_resultCallback);
 			}
-
-			}
 		}
 		return true;
 	}
@@ -662,7 +668,15 @@ void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 	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);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
 	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+#else
+	for (int i=0;i<this->getNumCollisionObjects();i++)
+	{
+		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+	}	
+#endif //USE_BRUTEFORCE_RAYBROADPHASE

 }

--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -125,6 +125,7 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)

 void	btDiscreteDynamicsWorld::debugDrawWorld()
 {
+	BT_PROFILE("debugDrawWorld");

 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
 	{
@@ -253,6 +254,7 @@ void	btDiscreteDynamicsWorld::applyGravity()

 void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
+	BT_PROFILE("synchronizeMotionStates");
 	{
 		//todo: iterate over awake simulation islands!
 		for ( int i=0;i<m_collisionObjects.size();i++)