Added 'cache friendly' tree traversal format, and traversal. Array of subtrees with specified maximum size. This is useful to fit tree traversals on SPU.

Demos/ConcaveDemo/ConcaveDemo.h

@@ -21,8 +21,15 @@ subject to the following restrictions:
 ///It also shows per-triangle material (friction/restitution) through CustomMaterialCombinerCallback
 class ConcaveDemo : public DemoApplication
 {
+
+	bool	m_animatedMesh;
+
 	public:
 
+	ConcaveDemo() : m_animatedMesh(false)
+	{
+
+	}
 	void	initPhysics();
 
 	virtual void clientMoveAndDisplay();
@@ -32,6 +39,7 @@ class ConcaveDemo : public DemoApplication
 	//to show refit works
 	void	setVertexPositions(float waveheight, float offset);
 	
+	virtual void keyboardCallback(unsigned char key, int x, int y);
 };
 
 #endif //CONCAVE_DEMO_H

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -22,7 +22,7 @@ subject to the following restrictions:
 
 
 GLDebugDrawer	debugDrawer;
-
+class btIDebugDraw* debugDrawerPtr=0;
 
 btVector3*	gVertices=0;
 int*	gIndices=0;
@@ -123,6 +123,27 @@ void	ConcaveDemo::setVertexPositions(float waveheight, float offset)
 		}
 	}
 }
+
+void ConcaveDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+	if (key == 'g')
+	{
+		m_animatedMesh = !m_animatedMesh;
+		if (m_animatedMesh)
+		{
+			staticBody->setCollisionFlags( staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+			staticBody->setActivationState(DISABLE_DEACTIVATION);
+		} else
+		{
+			staticBody->setCollisionFlags( staticBody->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
+			staticBody->forceActivationState(ACTIVE_TAG);
+		}
+	}
+
+	DemoApplication::keyboardCallback(key,x,y);
+
+}
+
 void	ConcaveDemo::initPhysics()
 {
 	#define TRISIZE 10.f
@@ -173,6 +194,7 @@ void	ConcaveDemo::initPhysics()
 	btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
 	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
+	debugDrawerPtr = &debugDrawer;
 	
 	float mass = 0.f;
 	btTransform	startTransform;
@@ -204,18 +226,18 @@ void ConcaveDemo::clientMoveAndDisplay()
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
 
-	
+	if (m_animatedMesh)
-	
+	{
+		static float offset=0.f;
+		offset+=0.01f;
 
-	static float offset=0.f;
+		setVertexPositions(waveheight,offset);
-	offset+=0.01f;
 
-	setVertexPositions(waveheight,offset);
+		trimeshShape->refitTree();
 
-	trimeshShape->refitTree();
+		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
-
+		m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(staticBody->getBroadphaseHandle());
-	//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
+	}
-	m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(staticBody->getBroadphaseHandle());
 
 	m_dynamicsWorld->stepSimulation(dt);
 	

Demos/OpenGL/DemoApplication.cpp

@@ -509,6 +509,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 		{
 			if (state==0)
 			{
+
 				shootBox(rayTo);
 			}
 			break;
@@ -519,6 +520,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 			if (state==0)
 			{
+				
 
 				//apply an impulse
 				if (m_dynamicsWorld)
@@ -554,6 +556,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 		{
 			if (state==0)
 			{
+		
 
 				//add a point to point constraint for picking
 				if (m_dynamicsWorld)
@@ -829,13 +832,12 @@ void DemoApplication::renderme()
 				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
 				yStart += yIncr;
 
-
+				
-			/*	
 				glRasterPos3f(xOffset,yStart,0);
-				sprintf(buf,"a to draw temporal AABBs");
+				sprintf(buf,"g to toggle mesh animation (ConcaveDemo)");
 				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
 				yStart += yIncr;
-			*/
+			
 
 				glRasterPos3f(xOffset,yStart,0);
 				sprintf(buf,"h to toggle help text");

Demos/OpenGL/GLDebugDrawer.cpp

@@ -68,35 +68,5 @@ void	GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3&
 }
 
 
-void GLDebugDrawer::drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
-{
-
-	btVector3 halfExtents = (to-from)* 0.5f;
-	btVector3 center = (to+from) *0.5f;
-	int i,j;
-
-	btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
-	for (i=0;i<4;i++)
-	{
-		for (j=0;j<3;j++)
-		{
-			pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
-				edgecoord[2]*halfExtents[2]);
-			pa+=center;
-
-			int othercoord = j%3;
-			edgecoord[othercoord]*=-1.f;
-			pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
-				edgecoord[2]*halfExtents[2]);
-			pb+=center;
-
-			drawLine(pa,pb,color);
-		}
-		edgecoord = btVector3(-1.f,-1.f,-1.f);
-		if (i<3)
-			edgecoord[i]*=-1.f;
-	}
 
 
-}
-

Demos/OpenGL/GLDebugDrawer.h

@@ -13,7 +13,6 @@ public:
 
 	GLDebugDrawer();
 
-	void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color);
 
 	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color);
 

src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -16,9 +16,15 @@ subject to the following restrictions:
 #include "btOptimizedBvh.h"
 #include "btStridingMeshInterface.h"
 #include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btIDebugDraw.h"
 
+//Note: currently we have 16 bytes per quantized node
+static const int MAX_SUBTREE_SIZE_IN_BYTES = 16384;
 
-btOptimizedBvh::btOptimizedBvh() : m_contiguousNodes(0), m_useQuantization(false)
+btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), 
+					m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
+					//m_traversalMode(TRAVERSAL_STACKLESS)
+					//m_traversalMode(TRAVERSAL_RECURSIVE)
 { 
 
 }
@@ -135,13 +141,21 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 		//now we have an array of leafnodes in m_leafNodes
 		numLeafNodes = m_leafNodes.size();
 
-		m_contiguousNodes = new btOptimizedBvhNode[2*numLeafNodes];
+		m_contiguousNodes.resize(2*numLeafNodes);
 	}
 
 	m_curNodeIndex = 0;
 
 	buildTree(0,numLeafNodes);
 
+	///if the entire tree is small then subtree size, we need to create a header info for the tree
+	if(m_useQuantization && !m_SubtreeHeaders.size())
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
+		subtree.m_rootNodeIndex = 0;
+		subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
+	}
 }
 
 
@@ -238,6 +252,14 @@ void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface)
 
 		meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 
+		///now update all subtree headers
+
+
+		for (i=0;i<m_SubtreeHeaders.size();i++)
+		{
+			btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+			subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+		}
 	} else
 	{
 
@@ -256,8 +278,8 @@ void	btOptimizedBvh::initQuantizationValues(btStridingMeshInterface* triangles)
 
 		AabbCalculationCallback()
 		{
-			m_aabbMin.setValue(1e30,1e30,1e30);
+			m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-			m_aabbMax.setValue(-1e30,-1e30,-1e30);
+			m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
 		}
 
 		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
@@ -287,8 +309,6 @@ void	btOptimizedBvh::initQuantizationValues(btStridingMeshInterface* triangles)
 }
 btOptimizedBvh::~btOptimizedBvh()
 {
-	if (m_contiguousNodes)
-		delete []m_contiguousNodes;
 }
 
 #ifdef DEBUG_TREE_BUILDING
@@ -343,20 +363,66 @@ void	btOptimizedBvh::buildTree	(int startIndex,int endIndex)
 
 	//internalNode->m_escapeIndex;
 	
+	int leftChildNodexIndex = m_curNodeIndex;
+
 	//build left child tree
 	buildTree(startIndex,splitIndex);
 
+	int rightChildNodexIndex = m_curNodeIndex;
 	//build right child tree
 	buildTree(splitIndex,endIndex);
 
 #ifdef DEBUG_TREE_BUILDING
 	gStackDepth--;
 #endif //DEBUG_TREE_BUILDING
-	
+
-	setInternalNodeEscapeIndex(internalNodeIndex,m_curNodeIndex - curIndex);
+	int escapeIndex = m_curNodeIndex - curIndex;
+
+	if (m_useQuantization)
+	{
+		//escapeIndex is the number of nodes of this subtree
+		const int sizeQuantizedNode =sizeof(btQuantizedBvhNode);
+		const int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
+		if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES)
+		{
+			updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
+		}
+	}
+
+	setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
 
 }
 
+void	btOptimizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex)
+{
+	btAssert(m_useQuantization);
+
+	btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
+	int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex();
+	int leftSubTreeSizeInBytes =  leftSubTreeSize * sizeof(btQuantizedBvhNode);
+	
+	btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
+	int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex();
+	int rightSubTreeSizeInBytes =  rightSubTreeSize * sizeof(btQuantizedBvhNode);
+
+	if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(leftChildNode);
+		subtree.m_rootNodeIndex = leftChildNodexIndex;
+		subtree.m_subtreeSize = leftSubTreeSize;
+	}
+
+	if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(rightChildNode);
+		subtree.m_rootNodeIndex = rightChildNodexIndex;
+		subtree.m_subtreeSize = rightSubTreeSize;
+	}
+}
+
+
 int	btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
 {
 	int i;
@@ -446,22 +512,29 @@ void	btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
 
 	if (m_useQuantization)
 	{
-//USE_RECURSION shows you can still do a recursive traversal on the stackless 'skip index' tree data without the explicit left/right child pointer
+		///quantize query AABB
-//#define USE_RECURSION 1
+		unsigned short int quantizedQueryAabbMin[3];
-#ifdef USE_RECURSION
+		unsigned short int quantizedQueryAabbMax[3];
-		bool useRecursion = true;
+		quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
-		if (useRecursion)
+		quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
+
+		switch (m_traversalMode)
 		{
-			unsigned short int quantizedQueryAabbMin[3];
+		case TRAVERSAL_STACKLESS:
-			unsigned short int quantizedQueryAabbMax[3];
+				walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex);
-			quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
+			break;
-			quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
+		case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
-			const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+				walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
-			walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			break;
-		} else
+		case TRAVERSAL_RECURSIVE:
-#endif //USE_RECURSION
+			{
-		{
+				const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
-			walkStacklessQuantizedTree(nodeCallback,aabbMin,aabbMax);
+				walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			}
+			break;
+		default:
+			//unsupported
+			btAssert(0);
 		}
 	} else
 	{
@@ -476,7 +549,7 @@ void	btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const
 {
 	btAssert(!m_useQuantization);
 
-	btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+	const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
 	int escapeIndex, curIndex = 0;
 	int walkIterations = 0;
 	bool aabbOverlap, isLeafNode;
@@ -511,13 +584,31 @@ void	btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const
 
 }
 
+/*
+///this was the original recursive traversal, before we optimized towards stackless traversal
+void	btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+	if (aabbOverlap)
+	{
+		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+		if (isLeafNode)
+		{
+			nodeCallback->processNode(rootNode);
+		} else
+		{
+			walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
+			walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
+		}
+	}
 
+}
+*/
 
 void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
 {
 	btAssert(m_useQuantization);
 	
-	int escapeIndex;
 	bool aabbOverlap, isLeafNode;
 
 	aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
@@ -534,33 +625,52 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize
 			const btQuantizedBvhNode* leftChildNode = currentNode+1;
 			walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
 
-			const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? 
+			const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex();
-								leftChildNode+1:
-								leftChildNode+leftChildNode->getEscapeIndex();
 			walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
 		}
 	}		
 }
 
 
-void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+
+
+
+
+
+void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const
 {
 	btAssert(m_useQuantization);
-
-	unsigned short int quantizedQueryAabbMin[3];
-	unsigned short int quantizedQueryAabbMax[3];
-	quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
-	quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
 	
-	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+	int curIndex = startNodeIndex;
-	int escapeIndex, curIndex = 0;
 	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+
+	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
+	int escapeIndex;
+	
 	bool aabbOverlap, isLeafNode;
 
-	while (curIndex < m_curNodeIndex)
+	while (curIndex < endNodeIndex)
 	{
+
+//#define VISUALLY_ANALYZE_BVH 1
+#ifdef VISUALLY_ANALYZE_BVH
+		//some code snippet to debugDraw aabb, to visually analyze bvh structure
+		static int drawPatch = 0;
+		//need some global access to a debugDrawer
+		extern btIDebugDraw* debugDrawerPtr;
+		if (curIndex==drawPatch)
+		{
+			btVector3 aabbMin,aabbMax;
+			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
+			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
+			btVector3	color(1,0,0);
+			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+		}
+#endif//VISUALLY_ANALYZE_BVH
+
 		//catch bugs in tree data
-		assert (walkIterations < m_curNodeIndex);
+		assert (walkIterations < subTreeSize);
 
 		walkIterations++;
 		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
@@ -587,32 +697,35 @@ void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 
 }
 
-/*
+//This traversal can be called from Playstation 3 SPU
-///this was the original recursive traversal, before we optimized towards stackless traversal
+void	btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
-void	btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
-	bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+	btAssert(m_useQuantization);
-	if (aabbOverlap)
+
+	int i;
+
+
+	for (i=0;i<this->m_SubtreeHeaders.size();i++)
 	{
-		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+		const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
-		if (isLeafNode)
+
+		bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		if (overlap)
 		{
-			nodeCallback->processNode(rootNode);
+			walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
-		} else
+				subtree.m_rootNodeIndex,
-		{
+				subtree.m_rootNodeIndex+subtree.m_subtreeSize);
-			walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
-			walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
 		}
 	}
-
 }
-*/
+
 
 
 
 void	btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
-
+	//not yet, please use aabb
+	btAssert(0);
 }
 
 

src/BulletCollision/CollisionShapes/btOptimizedBvh.h

@@ -31,6 +31,7 @@ class btStridingMeshInterface;
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
 ATTRIBUTE_ALIGNED16	(struct btQuantizedBvhNode)
 {
+	
 	//12 bytes
 	unsigned short int	m_quantizedAabbMin[3];
 	unsigned short int	m_quantizedAabbMax[3];
@@ -86,15 +87,17 @@ public:
 #include "../../LinearMath/btAlignedObjectArray.h"
 
 
+
+
 typedef btAlignedObjectArray<btOptimizedBvhNode>	NodeArray;
 
 typedef btAlignedObjectArray<btQuantizedBvhNode>	QuantizedNodeArray;
 
 ///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
-class btOptimizedBvh
+ATTRIBUTE_ALIGNED16(class btOptimizedBvh)
 {
 	NodeArray			m_leafNodes;
-	btOptimizedBvhNode*	m_contiguousNodes;
+	NodeArray			m_contiguousNodes;
 
 	QuantizedNodeArray	m_quantizedLeafNodes;
 	
@@ -109,7 +112,43 @@ class btOptimizedBvh
 	btVector3			m_bvhAabbMax;
 	btVector3			m_bvhQuantization;
 
-	//two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+	enum btTraversalMode
+	{
+		TRAVERSAL_STACKLESS = 0,
+		TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
+		TRAVERSAL_RECURSIVE
+	};
+
+	btTraversalMode	m_traversalMode;
+
+	///btBvhSubtreeInfo provides info to gather a subtree of limited size
+	class btBvhSubtreeInfo
+	{
+	public:
+		//12 bytes
+		unsigned short int	m_quantizedAabbMin[3];
+		unsigned short int	m_quantizedAabbMax[3];
+		//4 bytes, points to the root of the subtree
+		int			m_rootNodeIndex;
+		//4 bytes
+		int			m_subtreeSize;
+
+		void	setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
+		{
+			m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
+			m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
+			m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
+			m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
+			m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
+			m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
+		}
+	};
+
+	btAlignedObjectArray<btBvhSubtreeInfo>	m_SubtreeHeaders;
+
+
+	///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+	///this might be refactored into a virtual, it is usually not calculated at run-time
 	void	setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
 	{
 		if (m_useQuantization)
@@ -211,13 +250,17 @@ protected:
 	
 	void	walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
-	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,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;
 
 	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
 	void	walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
 
 	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
 	void	walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
+	
 
 	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{
@@ -228,6 +271,7 @@ protected:
 		return overlap;
 	}
 
+	void	updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
 
 public:
 	btOptimizedBvh();
@@ -244,6 +288,12 @@ public:
 	
 	btVector3	unQuantize(const unsigned short* vecIn) const;
 
+	///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
+	void	setTraversalMode(btTraversalMode	traversalMode)
+	{
+		m_traversalMode = traversalMode;
+	}
+
 	void	refit(btStridingMeshInterface* triangles);
 };
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -545,37 +545,6 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 
 }
 
-static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
-{
-
-	btVector3 halfExtents = (to-from)* btScalar(0.5);
-	btVector3 center = (to+from) *btScalar(0.5);
-	int i,j;
-
-	btVector3 edgecoord(btScalar(1.),btScalar(1.),btScalar(1.)),pa,pb;
-	for (i=0;i<4;i++)
-	{
-		for (j=0;j<3;j++)
-		{
-			pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
-				edgecoord[2]*halfExtents[2]);
-			pa+=center;
-
-			int othercoord = j%3;
-			edgecoord[othercoord]*=btScalar(-1.);
-			pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
-				edgecoord[2]*halfExtents[2]);
-			pb+=center;
-
-			debugDrawer->drawLine(pa,pb,color);
-		}
-		edgecoord = btVector3(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		if (i<3)
-			edgecoord[i]*=btScalar(-1.);
-	}
-
-
-}
 
 void	btDiscreteDynamicsWorld::updateAabbs()
 {
@@ -620,7 +589,7 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 				}
 				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 				{
-					DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec);
+					m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 				}
 			}
 		}

src/LinearMath/btIDebugDraw.h

@@ -64,9 +64,37 @@ class	btIDebugDraw
 	
 	virtual int		getDebugMode() const = 0;
 
+	inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+	{
 
+		btVector3 halfExtents = (to-from)* 0.5f;
+		btVector3 center = (to+from) *0.5f;
+		int i,j;
 
+		btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
+		for (i=0;i<4;i++)
+		{
+			for (j=0;j<3;j++)
+			{
+				pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
+					edgecoord[2]*halfExtents[2]);
+				pa+=center;
+
+				int othercoord = j%3;
+				edgecoord[othercoord]*=-1.f;
+				pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
+					edgecoord[2]*halfExtents[2]);
+				pb+=center;
+
+				drawLine(pa,pb,color);
+			}
+			edgecoord = btVector3(-1.f,-1.f,-1.f);
+			if (i<3)
+				edgecoord[i]*=-1.f;
+		}
+	}
 };
 
+
 #endif //IDEBUG_DRAW__H