Added a refit to the quantized stackless tree, with updated ConcaveDemo.

Demos/ConcaveDemo/ConcaveDemo.h

@@ -29,6 +29,8 @@ class ConcaveDemo : public DemoApplication
 
 	virtual void displayCallback();
 	
+	//to show refit works
+	void	setVertexPositions(float waveheight, float offset);
 	
 };
 

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -23,12 +23,14 @@ subject to the following restrictions:
 
 GLDebugDrawer	debugDrawer;
 
-static const int NUM_VERTICES = 5;
-static const int NUM_TRIANGLES=4;
 
-btVector3	gVertices[NUM_VERTICES];
+btVector3*	gVertices=0;
-int	gIndices[NUM_TRIANGLES*3];
+int*	gIndices=0;
-const float TRIANGLE_SIZE=80.f;
+btBvhTriangleMeshShape* trimeshShape =0;
+btRigidBody* staticBody = 0;
+static float waveheight = 5.f;
+
+const float TRIANGLE_SIZE=8.f;
 
 
 
@@ -100,6 +102,27 @@ int main(int argc,char** argv)
 	return glutmain(argc, argv,640,480,"Static Concave Mesh Demo",concaveDemo);
 }
 
+
+	const int NUM_VERTS_X = 50;
+	const int NUM_VERTS_Y = 50;
+	const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
+
+void	ConcaveDemo::setVertexPositions(float waveheight, float offset)
+{
+	int i;
+	int j;
+
+	for ( i=0;i<NUM_VERTS_X;i++)
+	{
+		for (j=0;j<NUM_VERTS_Y;j++)
+		{
+			gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
+				//0.f,
+				waveheight*sinf((float)i+offset)*cosf((float)j+offset),
+				(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
+		}
+	}
+}
 void	ConcaveDemo::initPhysics()
 {
 	#define TRISIZE 10.f
@@ -107,27 +130,16 @@ void	ConcaveDemo::initPhysics()
 	int vertStride = sizeof(btVector3);
 	int indexStride = 3*sizeof(int);
 
-	const int NUM_VERTS_X = 50;
-	const int NUM_VERTS_Y = 50;
-	const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
 	
 	const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
 
-	btVector3*	gVertices = new btVector3[totalVerts];
+	gVertices = new btVector3[totalVerts];
-	int*	gIndices = new int[totalTriangles*3];
+	gIndices = new int[totalTriangles*3];
 
 	int i;
 
-	for ( i=0;i<NUM_VERTS_X;i++)
+
-	{
+	setVertexPositions(waveheight,0.f);
-		for (int j=0;j<NUM_VERTS_Y;j++)
-		{
-			gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
-				//0.f,
-				2.f*sinf((float)i)*cosf((float)j),
-				(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
-		}
-	}
 
 	int index=0;
 	for ( i=0;i<NUM_VERTS_X-1;i++)
@@ -150,8 +162,7 @@ void	ConcaveDemo::initPhysics()
 		totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
 
 	bool useQuantizedAabbCompression = true;
-	btCollisionShape* trimeshShape  = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
+	trimeshShape  = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
-
 
 
 //	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
@@ -168,7 +179,7 @@ void	ConcaveDemo::initPhysics()
 	startTransform.setIdentity();
 	startTransform.setOrigin(btVector3(0,-2,0));
 
-	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,trimeshShape);
+	staticBody = localCreateRigidBody(mass, startTransform,trimeshShape);
 
 	staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
 
@@ -179,7 +190,7 @@ void	ConcaveDemo::initPhysics()
 		for (int i=0;i<10;i++)
 		{
 			btCollisionShape* boxShape = new btBoxShape(btVector3(1,1,1));
-			startTransform.setOrigin(btVector3(2*i,1,1));
+			startTransform.setOrigin(btVector3(2*i,10,1));
 			localCreateRigidBody(1, startTransform,boxShape);
 		}
 	}
@@ -192,6 +203,20 @@ void ConcaveDemo::clientMoveAndDisplay()
 
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
+
+	
+	
+
+	static float offset=0.f;
+	offset+=0.01f;
+
+	setVertexPositions(waveheight,offset);
+
+	trimeshShape->refitTree();
+
+	//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
+	m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(staticBody->getBroadphaseHandle());
+
 	m_dynamicsWorld->stepSimulation(dt);
 	
 	renderme();

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -33,6 +33,13 @@ btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInte
 
 }
 
+void	btBvhTriangleMeshShape::refitTree()
+{
+	m_bvh->refit( m_meshInterface );
+	
+	recalcLocalAabb();
+}
+
 btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
 {
 	delete m_bvh;

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -44,6 +44,7 @@ public:
 
 	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
+	void	refitTree();
 
 	//debugging
 	virtual char*	getName()const {return "BVHTRIANGLEMESH";}

src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -18,7 +18,7 @@ subject to the following restrictions:
 #include "LinearMath/btAabbUtil2.h"
 
 
-btOptimizedBvh::btOptimizedBvh() : m_contiguousNodes(0), m_quantizedContiguousNodes(0), m_useQuantization(false)
+btOptimizedBvh::btOptimizedBvh() : m_contiguousNodes(0), m_useQuantization(false)
 { 
 
 }
@@ -100,27 +100,7 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 			m_triangleNodes.push_back(node);
 		}
 	};
-	struct	AabbCalculationCallback : public btInternalTriangleIndexCallback
-	{
-		btVector3	m_aabbMin;
-		btVector3	m_aabbMax;
 	
-		AabbCalculationCallback()
-		{
-			m_aabbMin.setValue(1e30,1e30,1e30);
-			m_aabbMax.setValue(-1e30,-1e30,-1e30);
-		}
-
-		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
-		{
-			m_aabbMin.setMin(triangle[0]);
-			m_aabbMax.setMax(triangle[0]);
-			m_aabbMin.setMin(triangle[1]);
-			m_aabbMax.setMax(triangle[1]);
-			m_aabbMin.setMin(triangle[2]);
-			m_aabbMax.setMax(triangle[2]);
-		}
-	};
 
 
 	int numLeafNodes = 0;
@@ -128,28 +108,19 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 	
 	if (m_useQuantization)
 	{
-		//first calculate the total aabb for all triangles
-		AabbCalculationCallback	aabbCallback;
-		btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-		btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-		triangles->InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
 
-		//initialize quantization values
+		initQuantizationValues(triangles);
-		m_bvhAabbMin = aabbCallback.m_aabbMin;
-		m_bvhAabbMax = aabbCallback.m_aabbMax;
-		btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
-		m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize;
 
 		QuantizedNodeTriangleCallback	callback(m_quantizedLeafNodes,this);
 
 	
-		triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+		triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
 
 		//now we have an array of leafnodes in m_leafNodes
 		numLeafNodes = m_quantizedLeafNodes.size();
 
 
-		m_quantizedContiguousNodes = new btQuantizedBvhNode[2*numLeafNodes];
+		m_quantizedContiguousNodes.resize(2*numLeafNodes);
 
 
 	} else
@@ -173,13 +144,151 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 
 }
 
+
+void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface)
+{
+	if (m_useQuantization)
+	{
+		int nodeSubPart=0;
+
+		initQuantizationValues(meshInterface);
+
+		//get access info to trianglemesh data
+		const unsigned char *vertexbase;
+		int numverts;
+		PHY_ScalarType type;
+		int stride;
+		const unsigned char *indexbase;
+		int indexstride;
+		int numfaces;
+		PHY_ScalarType indicestype;
+		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
+
+
+		int numNodes = m_curNodeIndex;
+		int i;
+		for (i=numNodes-1;i>=0;i--)
+		{
+			btVector3	triangleVerts[3];
+
+			btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
+			if (curNode.isLeafNode())
+			{
+				//recalc aabb from triangle data
+				int nodeTriangleIndex = curNode.getTriangleIndex();
+				//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
+
+				int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride);
+				
+				const btVector3& meshScaling = meshInterface->getScaling();
+				for (int j=2;j>=0;j--)
+				{
+					
+					int graphicsindex = gfxbase[j];
+					btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
+
+					triangleVerts[j] = btVector3(
+						graphicsbase[0]*meshScaling.getX(),
+						graphicsbase[1]*meshScaling.getY(),
+						graphicsbase[2]*meshScaling.getZ());
+				}
+
+
+				btVector3	aabbMin,aabbMax;
+				aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+				aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+				aabbMin.setMin(triangleVerts[0]);
+				aabbMax.setMax(triangleVerts[0]);
+				aabbMin.setMin(triangleVerts[1]);
+				aabbMax.setMax(triangleVerts[1]);
+				aabbMin.setMin(triangleVerts[2]);
+				aabbMax.setMax(triangleVerts[2]);
+
+				quantizeWithClamp(&curNode.m_quantizedAabbMin[0],aabbMin);
+				quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax);
+				int k;
+				k=0;
+
+			} else
+			{
+				//combine aabb from both children
+
+				btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
+				
+				btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
+					&m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
+				int k;
+				k=0;
+
+				{
+					for (int i=0;i<3;i++)
+					{
+						curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
+						if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
+							curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
+
+						curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
+						if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
+							curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
+					}
+				}
+			}
+
+		}
+
+		meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+
+	} else
+	{
+
+	}
+
+}
+
+
+void	btOptimizedBvh::initQuantizationValues(btStridingMeshInterface* triangles)
+{
+
+	struct	AabbCalculationCallback : public btInternalTriangleIndexCallback
+	{
+		btVector3	m_aabbMin;
+		btVector3	m_aabbMax;
+
+		AabbCalculationCallback()
+		{
+			m_aabbMin.setValue(1e30,1e30,1e30);
+			m_aabbMax.setValue(-1e30,-1e30,-1e30);
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		{
+			m_aabbMin.setMin(triangle[0]);
+			m_aabbMax.setMax(triangle[0]);
+			m_aabbMin.setMin(triangle[1]);
+			m_aabbMax.setMax(triangle[1]);
+			m_aabbMin.setMin(triangle[2]);
+			m_aabbMax.setMax(triangle[2]);
+		}
+	};
+
+		//first calculate the total aabb for all triangles
+	AabbCalculationCallback	aabbCallback;
+	btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	triangles->InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
+
+	//initialize quantization values
+	m_bvhAabbMin = aabbCallback.m_aabbMin;
+	m_bvhAabbMax = aabbCallback.m_aabbMax;
+	btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+	m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize;
+
+
+}
 btOptimizedBvh::~btOptimizedBvh()
 {
 	if (m_contiguousNodes)
 		delete []m_contiguousNodes;
-
-	if (m_quantizedContiguousNodes)
-		delete []m_quantizedContiguousNodes;
 }
 
 #ifdef DEBUG_TREE_BUILDING
@@ -395,7 +504,7 @@ void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 	quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
 	quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
 	
-	btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
 	int escapeIndex, curIndex = 0;
 	int walkIterations = 0;
 	bool aabbOverlap, isLeafNode;

src/BulletCollision/CollisionShapes/btOptimizedBvh.h

@@ -97,7 +97,8 @@ class btOptimizedBvh
 	btOptimizedBvhNode*	m_contiguousNodes;
 
 	QuantizedNodeArray	m_quantizedLeafNodes;
-	btQuantizedBvhNode*	m_quantizedContiguousNodes;
+	
+	QuantizedNodeArray	m_quantizedContiguousNodes;
 	
 	int					m_curNodeIndex;
 
@@ -152,6 +153,10 @@ class btOptimizedBvh
 		
 	}
 
+	void	initQuantizationValues(btStridingMeshInterface* triangles);
+
+	
+	
 	void	setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
 	{
 		if (m_useQuantization)
@@ -208,7 +213,7 @@ protected:
 
 	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
-	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,unsigned short int* aabbMin2,unsigned short int* aabbMax2) const
+	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{
 		bool overlap = true;
 		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
@@ -233,6 +238,7 @@ public:
 	
 	btVector3	unQuantize(const unsigned short* vecIn) const;
 
+	void	refit(btStridingMeshInterface* triangles);
 };