- Added compressed/quantized AABB tree, 16 bytes per node, while supporting 32-bit (triangle) indices. Should be faster and smaller then original version (quantized aabb check is done in integer space)

Original aabb nodes are 44 bytes (with full floating point precision and additional part index)
- added meter-unit scaling support in ColladaConverter.cpp

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -17,6 +17,7 @@ subject to the following restrictions:
 //#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
 //#define USER_DEFINED_FRICTION_MODEL 1
 #define USE_CUSTOM_NEAR_CALLBACK 1
+//#define CENTER_OF_MASS_SHIFT 1
 
 //following define allows to compare/replace Bullet's constraint solver with ODE quickstep
 //this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files from Extras/quickstep
@@ -46,6 +47,8 @@ float	gCollisionMargin = 0.05f;
 
 #include "GlutStuff.h"
 
+btTransform comOffset;
+btVector3 comOffsetVec(0,2,0);
 
 extern float eye[3];
 extern int glutScreenWidth;
@@ -55,7 +58,7 @@ const int maxProxies = 32766;
 const int maxOverlap = 65535;
 
 bool createConstraint = true;//false;
-bool useCompound = false;//true;//false;
+bool useCompound = false;
 
 
 
@@ -91,6 +94,7 @@ btCollisionShape* shapePtr[numShapes] =
 #endif
 		
 		new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)),
+		//new btSphereShape (CUBE_HALF_EXTENTS),
 		//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin),
 		//new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)),
 		//new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
@@ -245,6 +249,48 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 	
 	renderme(); 
 
+
+	//render the graphics objects, with center of mass shift
+
+		updateCamera();
+
+
+#ifdef CENTER_OF_MASS_SHIFT
+	btScalar m[16];
+
+	if (m_dynamicsWorld)
+	{
+		int numObjects = m_dynamicsWorld->getNumCollisionObjects();
+		btVector3 wireColor(1,0,0);
+		for (int i=0;i<numObjects;i++)
+		{
+			btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
+			btRigidBody* body = btRigidBody::upcast(colObj);
+
+			if (body && body->getMotionState())
+			{
+				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
+				btTransform offsetTrans = myMotionState->m_graphicsWorldTrans;
+
+
+				btVector3 worldShift =  offsetTrans.getBasis() * comOffsetVec;
+				offsetTrans.setOrigin(offsetTrans.getOrigin() + worldShift);
+
+
+				offsetTrans.getOpenGLMatrix(m);
+
+				btVector3 wireColor(1.f,1.0f,1.f); //wants deactivation
+
+				btSphereShape sphereTmp(CUBE_HALF_EXTENTS);
+
+				GL_ShapeDrawer::drawOpenGL(m,&sphereTmp,wireColor,0);
+			}			
+		}
+
+	}
+#endif //CENTER_OF_MASS_SHIFT
+
+
 #ifdef USE_QUICKPROF 
         btProfiler::endBlock("render"); 
 #endif 
@@ -363,13 +409,19 @@ void	CcdPhysicsDemo::initPhysics()
 		btCompoundShape* compoundShape = new btCompoundShape();
 		btCollisionShape* oldShape = shapePtr[1];
 		shapePtr[1] = compoundShape;
+		btVector3 sphereOffset(0,0,2);
 
-		btTransform ident;
+		comOffset.setIdentity();
-		ident.setIdentity();
+
-		ident.setOrigin(btVector3(0,0,0));	
+#ifdef CENTER_OF_MASS_SHIFT
-		compoundShape->addChildShape(ident,oldShape);//
+		comOffset.setOrigin(comOffsetVec);
-		ident.setOrigin(btVector3(0,0,2));	
+		compoundShape->addChildShape(comOffset,oldShape);
-		compoundShape->addChildShape(ident,new btSphereShape(0.9));//
+
+#else
+		compoundShape->addChildShape(tr,oldShape);
+		tr.setOrigin(sphereOffset);
+		compoundShape->addChildShape(tr,new btSphereShape(0.9));
+#endif
 	}
 
 	for (i=0;i<gNumObjects;i++)

Demos/ColladaDemo/ColladaConverter.cpp

@@ -67,9 +67,11 @@ domMatrix_Array emptyMatrixArray;
 domNodeRef	m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
 
 
+///This code is actually wrong: the order of transformations is lost, so we need to rewrite this!
 btTransform	GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
 														domRotate_Array& rotateArray,
-														domTranslate_Array& translateArray
+														domTranslate_Array& translateArray,
+														float meterScaling
 														)
 
 {
@@ -83,7 +85,7 @@ btTransform	GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
 		domMatrixRef matrixRef = matrixArray[i];
 		domFloat4x4 fl16 = matrixRef->getValue();
 		btVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
-		startTransform.setOrigin(origin);
+		startTransform.setOrigin(origin*meterScaling);
 		btMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
 							fl16.get(4),fl16.get(5),fl16.get(6),
 							fl16.get(8),fl16.get(9),fl16.get(10));
@@ -103,7 +105,8 @@ btTransform	GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
 	{
 		domTranslateRef translateRef = translateArray[i];
 		domFloat3 fl3 = translateRef->getValue();
-		startTransform.getOrigin() += btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
+		btVector3 orgTrans(fl3.get(0),fl3.get(1),fl3.get(2));
+		startTransform.getOrigin() += orgTrans*meterScaling;
 	}
 	return startTransform;
 }
@@ -116,7 +119,8 @@ ColladaConverter::ColladaConverter()
 :m_collada(0),
 m_dom(0),
 m_filename(0),
-m_numObjects(0)
+m_numObjects(0),
+m_unitMeterScaling(1.f)
 {
 	//Collada-m_dom
 	m_collada = new DAE;
@@ -179,6 +183,15 @@ bool ColladaConverter::convert()
 
 //succesfully loaded file, now convert data
 
+			if (m_dom->getAsset() && m_dom->getAsset()->getUnit())
+			{
+				domAsset::domUnitRef unit = m_dom->getAsset()->getUnit();
+				domFloat meter = unit->getMeter();
+				printf("asset unit meter=%f\n",meter);
+				m_unitMeterScaling = meter;
+		
+
+			}
 			if ( m_dom->getAsset() && m_dom->getAsset()->getUp_axis() )
 			{
 				domAsset::domUp_axis * up = m_dom->getAsset()->getUp_axis();
@@ -593,7 +606,8 @@ void	ColladaConverter::prepareConstraints(ConstraintInput& input)
 								(
 								emptyMatrixArray,
 								attachRefBody->getRotate_array(),
-								attachRefBody->getTranslate_array());
+								attachRefBody->getTranslate_array(),
+								m_unitMeterScaling);
 						}
 
 						btTransform attachFrameOther;
@@ -605,7 +619,8 @@ void	ColladaConverter::prepareConstraints(ConstraintInput& input)
 								(
 								emptyMatrixArray,
 								attachBody1->getRotate_array(),
-								attachBody1->getTranslate_array()
+								attachBody1->getTranslate_array(),
+								m_unitMeterScaling
 								);
 						}
 
@@ -718,7 +733,8 @@ void	ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool
 		startTransform = GetbtTransformFromCOLLADA_DOM(
 							node->getMatrix_array(),
 							node->getRotate_array(),
-							node->getTranslate_array()
+							node->getTranslate_array(),
+							m_unitMeterScaling
 							);
 
 		unsigned int i;
@@ -994,7 +1010,7 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 				{
 					const domFloat4 planeEq = planeRef->getEquation()->getValue();
 					btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
-					btScalar planeConstant = planeEq.get(3);
+					btScalar planeConstant = planeEq.get(3)*m_unitMeterScaling;
 					rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
 				}
 
@@ -1005,25 +1021,25 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 				domBoxRef boxRef = shapeRef->getBox();
 				domBox::domHalf_extentsRef	domHalfExtentsRef = boxRef->getHalf_extents();
 				domFloat3& halfExtents = domHalfExtentsRef->getValue();
-				float x = halfExtents.get(0);
+				float x = halfExtents.get(0)*m_unitMeterScaling;
-				float y = halfExtents.get(1);
+				float y = halfExtents.get(1)*m_unitMeterScaling;
-				float z = halfExtents.get(2);
+				float z = halfExtents.get(2)*m_unitMeterScaling;
 				rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
 			}
 			if (shapeRef->getSphere())
 			{
 				domSphereRef sphereRef = shapeRef->getSphere();
 				domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
-				domFloat radius = radiusRef->getValue();
+				domFloat radius = radiusRef->getValue()*m_unitMeterScaling;
 				rbOutput.m_colShape = new btSphereShape(radius);
 			}
 
 			if (shapeRef->getCylinder())
 			{
 				domCylinderRef cylinderRef = shapeRef->getCylinder();
-				domFloat height = cylinderRef->getHeight()->getValue();
+				domFloat height = cylinderRef->getHeight()->getValue()*m_unitMeterScaling;
 				domFloat2 radius2 = cylinderRef->getRadius()->getValue();
-				domFloat radius0 = radius2.get(0);
+				domFloat radius0 = radius2.get(0)*m_unitMeterScaling;
 
 				//Cylinder around the local Y axis
 				rbOutput.m_colShape = new btCylinderShape(btVector3(radius0,height,radius0));
@@ -1117,7 +1133,7 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 												domFloat fl1 = listFloats.get(index0+1);
 												domFloat fl2 = listFloats.get(index0+2);
 												k+=meshPart.m_triangleIndexStride;
-												verts[i].setValue(fl0,fl1,fl2);
+												verts[i].setValue(fl0*m_unitMeterScaling,fl1*m_unitMeterScaling,fl2*m_unitMeterScaling);
 											}
 											trimesh->addTriangle(verts[0],verts[1],verts[2]);
 										}
@@ -1143,7 +1159,8 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 							} else
 							{
 								printf("static concave triangle <mesh> added\n");
-								rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
+								bool useQuantizedAabbCompression = false;
+								rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh,useQuantizedAabbCompression);
 								//rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
 								//rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
 								
@@ -1182,7 +1199,7 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 											domFloat fl0 = listFloats.get(vertIndex);
 											domFloat fl1 = listFloats.get(vertIndex+1);
 											domFloat fl2 = listFloats.get(vertIndex+2);
-											convexHull->addPoint(btPoint3(fl0,fl1,fl2));
+											convexHull->addPoint(btPoint3(fl0,fl1,fl2) * m_unitMeterScaling);
 										}
 									}
 								}
@@ -1280,7 +1297,7 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 														domFloat fl2 = listFloats.get(k+2);
 														//printf("float %f %f %f\n",fl0,fl1,fl2);
 
-														convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
+														convexHullShape->addPoint(btPoint3(fl0,fl1,fl2) * m_unitMeterScaling);
 													}
 
 												}
@@ -1328,7 +1345,7 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 										domFloat fl2 = listFloats.get(k+2);
 										//printf("float %f %f %f\n",fl0,fl1,fl2);
 
-										convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
+										convexHullShape->addPoint(btPoint3(fl0,fl1,fl2)*m_unitMeterScaling);
 									}
 
 								}
@@ -1383,7 +1400,8 @@ void	ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
 					localTransform = GetbtTransformFromCOLLADA_DOM(
 						emptyMatrixArray,
 						shapeRef->getRotate_array(),
-						shapeRef->getTranslate_array()
+						shapeRef->getTranslate_array(),
+						m_unitMeterScaling
 						);
 					}
 

Demos/ColladaDemo/ColladaConverter.h

@@ -41,6 +41,7 @@ protected:
 	class DAE* m_collada;
 	class domCOLLADA* m_dom;
 	const char* m_filename;
+	float	m_unitMeterScaling;
 	
 	int	m_numObjects;
 	btRigidBody* m_rigidBodies[COLLADA_CONVERTER_MAX_NUM_OBJECTS];

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -149,7 +149,8 @@ void	ConcaveDemo::initPhysics()
 		indexStride,
 		totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
 
-	btCollisionShape* trimeshShape  = new btBvhTriangleMeshShape(indexVertexArrays);
+	bool useQuantizedAabbCompression = true;
+	btCollisionShape* trimeshShape  = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
 
 
 //	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
@@ -159,6 +160,7 @@ void	ConcaveDemo::initPhysics()
 	btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
 	btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
 	
 	float mass = 0.f;
 	btTransform	startTransform;

Demos/VehicleDemo/VehicleDemo.cpp

@@ -185,7 +185,8 @@ const float TRIANGLE_SIZE=20.f;
 		indexStride,
 		totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
 
-	groundShape = new btBvhTriangleMeshShape(indexVertexArrays);
+	bool useQuantizedAabbCompression = true;
+	groundShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
 	
 #endif //
 

src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -110,14 +110,7 @@ btAxisSweep3::~btAxisSweep3()
 void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const
 {
 	btPoint3 clampedPoint(point);
-	/*
+	
-	if (isMax)
-		clampedPoint += btVector3(10,10,10);
-	else
-	{
-		clampedPoint -= btVector3(10,10,10);
-	}
-	*/
 
 
 	clampedPoint.setMax(m_worldAabbMin);

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -20,14 +20,14 @@ subject to the following restrictions:
 
 ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
 ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
-btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface)
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression)
-:btTriangleMeshShape(meshInterface)
+:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression)
 {
 	//construct bvh from meshInterface
 #ifndef DISABLE_BVH
 
 	m_bvh = new btOptimizedBvh();
-	m_bvh->build(meshInterface);
+	m_bvh->build(meshInterface,m_useQuantizedAabbCompression);
 
 #endif //DISABLE_BVH
 
@@ -63,7 +63,7 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 		{
 		}
 				
-		virtual void processNode(const btOptimizedBvhNode* node)
+		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
 			const unsigned char *vertexbase;
 			int numverts;
@@ -84,9 +84,9 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 				indexstride,
 				numfaces,
 				indicestype,
-				node->m_subPart);
+				nodeSubPart);
 
-			int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride);
+			int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride);
 			
 			const btVector3& meshScaling = m_meshInterface->getScaling();
 			for (int j=2;j>=0;j--)
@@ -107,8 +107,8 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 #endif //DEBUG_TRIANGLE_MESH
 			}
 
-			m_callback->processTriangle(m_triangle,node->m_subPart,node->m_triangleIndex);
+			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
-			m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart);
+			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 		}
 
 	};
@@ -131,7 +131,7 @@ void	btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 		btTriangleMeshShape::setLocalScaling(scaling);
 		delete m_bvh;
 		m_bvh = new btOptimizedBvh();
-		m_bvh->build(m_meshInterface);
+		m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression);
 		//rebuild the bvh...
 	}
 }

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -25,10 +25,10 @@ class btBvhTriangleMeshShape : public btTriangleMeshShape
 {
 
 	btOptimizedBvh*	m_bvh;
-	
+	bool m_useQuantizedAabbCompression;
 
 public:
-	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface);
+	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression);
 
 	virtual ~btBvhTriangleMeshShape();
 

src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -18,16 +18,15 @@ subject to the following restrictions:
 #include "LinearMath/btAabbUtil2.h"
 
 
-btOptimizedBvh::btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) 
+btOptimizedBvh::btOptimizedBvh() : m_contiguousNodes(0), m_quantizedContiguousNodes(0), m_useQuantization(false)
 { 
 
 }
 
 
-void btOptimizedBvh::build(btStridingMeshInterface* triangles)
+void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression)
 {
-	//int countTriangles = 0;
+	m_useQuantization = useQuantizedAabbCompression;
-
 
 
 	// NodeArray	triangleNodes;
@@ -39,38 +38,122 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles)
 		NodeTriangleCallback(NodeArray&	triangleNodes)
 			:m_triangleNodes(triangleNodes)
 		{
-
 		}
 
 		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
 		{
-
 			btOptimizedBvhNode node;
-			node.m_aabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); 
+			btVector3	aabbMin,aabbMax;
-			node.m_aabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+			aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-			node.m_aabbMin.setMin(triangle[0]);
+			aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
-			node.m_aabbMax.setMax(triangle[0]);
+			aabbMin.setMin(triangle[0]);
-			node.m_aabbMin.setMin(triangle[1]);
+			aabbMax.setMax(triangle[0]);
-			node.m_aabbMax.setMax(triangle[1]);
+			aabbMin.setMin(triangle[1]);
-			node.m_aabbMin.setMin(triangle[2]);
+			aabbMax.setMax(triangle[1]);
-			node.m_aabbMax.setMax(triangle[2]);
+			aabbMin.setMin(triangle[2]);
+			aabbMax.setMax(triangle[2]);
+
+			//with quantization?
+			node.m_aabbMinOrg = aabbMin;
+			node.m_aabbMaxOrg = aabbMax;
 
 			node.m_escapeIndex = -1;
-			node.m_leftChild = 0;
-			node.m_rightChild = 0;
-
 	
 			//for child nodes
 			node.m_subPart = partId;
 			node.m_triangleIndex = triangleIndex;
+			m_triangleNodes.push_back(node);
+		}
+	};
+	struct	QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
+	{
+		QuantizedNodeArray&	m_triangleNodes;
+		const btOptimizedBvh* m_optimizedTree; // for quantization
 
+		QuantizedNodeTriangleCallback(QuantizedNodeArray&	triangleNodes,const btOptimizedBvh* tree)
+			:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		{
+			btAssert(partId==0);
+			//negative indices are reserved for escapeIndex
+			btAssert(triangleIndex>=0);
+
+			btQuantizedBvhNode node;
+			btVector3	aabbMin,aabbMax;
+			aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+			aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+			aabbMin.setMin(triangle[0]);
+			aabbMax.setMax(triangle[0]);
+			aabbMin.setMin(triangle[1]);
+			aabbMax.setMax(triangle[1]);
+			aabbMin.setMin(triangle[2]);
+			aabbMax.setMax(triangle[2]);
+
+			m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin);
+			m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax);
+
+			node.m_escapeIndexOrTriangleIndex = triangleIndex;
 
 			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;
 
+	
+	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
+		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);
+
+		//now we have an array of leafnodes in m_leafNodes
+		numLeafNodes = m_quantizedLeafNodes.size();
+
+
+		m_quantizedContiguousNodes = new btQuantizedBvhNode[2*numLeafNodes];
+
+
+	} else
+	{
 		NodeTriangleCallback	callback(m_leafNodes);
 
 		btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
@@ -79,21 +162,24 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles)
 		triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
 
 		//now we have an array of leafnodes in m_leafNodes
+		numLeafNodes = m_leafNodes.size();
+
+		m_contiguousNodes = new btOptimizedBvhNode[2*numLeafNodes];
+	}
 
-	m_contiguousNodes = new btOptimizedBvhNode[2*m_leafNodes.size()];
 	m_curNodeIndex = 0;
 
-	m_rootNode1 = buildTree(m_leafNodes,0,m_leafNodes.size());
+	buildTree(0,numLeafNodes);
 
-
-	///create the leafnodes first
-//	btOptimizedBvhNode* leafNodes = new btOptimizedBvhNode;
 }
 
 btOptimizedBvh::~btOptimizedBvh()
 {
 	if (m_contiguousNodes)
 		delete []m_contiguousNodes;
+
+	if (m_quantizedContiguousNodes)
+		delete []m_quantizedContiguousNodes;
 }
 
 #ifdef DEBUG_TREE_BUILDING
@@ -101,7 +187,7 @@ int gStackDepth = 0;
 int gMaxStackDepth = 0;
 #endif //DEBUG_TREE_BUILDING
 
-btOptimizedBvhNode*	btOptimizedBvh::buildTree	(NodeArray&	leafNodes,int startIndex,int endIndex)
+void	btOptimizedBvh::buildTree	(int startIndex,int endIndex)
 {
 #ifdef DEBUG_TREE_BUILDING
 	gStackDepth++;
@@ -109,7 +195,6 @@ btOptimizedBvhNode*	btOptimizedBvh::buildTree	(NodeArray&	leafNodes,int startInd
 		gMaxStackDepth = gStackDepth;
 #endif //DEBUG_TREE_BUILDING
 
-	btOptimizedBvhNode* internalNode;
 
 	int splitAxis, splitIndex, i;
 	int numIndices =endIndex-startIndex;
@@ -122,38 +207,48 @@ btOptimizedBvhNode*	btOptimizedBvh::buildTree	(NodeArray&	leafNodes,int startInd
 #ifdef DEBUG_TREE_BUILDING
 		gStackDepth--;
 #endif //DEBUG_TREE_BUILDING
-		return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);
+		
+		assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
+
+		m_curNodeIndex++;
+		return;	
 	}
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 	
-	splitAxis = calcSplittingAxis(leafNodes,startIndex,endIndex);
+	splitAxis = calcSplittingAxis(startIndex,endIndex);
 
-	splitIndex = sortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis);
+	splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
 
-	internalNode = &m_contiguousNodes[m_curNodeIndex++];
+	int internalNodeIndex = m_curNodeIndex;
 	
-	internalNode->m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)));
-	internalNode->m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)));
 	
 	for (i=startIndex;i<endIndex;i++)
 	{
-		internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax);
+		mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
-		internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin);
 	}
 
+	m_curNodeIndex++;
 	
 
 	//internalNode->m_escapeIndex;
-	internalNode->m_leftChild = buildTree(leafNodes,startIndex,splitIndex);
+	
-	internalNode->m_rightChild = buildTree(leafNodes,splitIndex,endIndex);
+	//build left child tree
+	buildTree(startIndex,splitIndex);
+
+	//build right child tree
+	buildTree(splitIndex,endIndex);
+
 #ifdef DEBUG_TREE_BUILDING
 	gStackDepth--;
 #endif //DEBUG_TREE_BUILDING
-	internalNode->m_escapeIndex  = m_curNodeIndex - curIndex;
+	
-	return internalNode;
+	setInternalNodeEscapeIndex(internalNodeIndex,m_curNodeIndex - curIndex);
+
 }
 
-int	btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray&	leafNodes,int startIndex,int endIndex,int splitAxis)
+int	btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
 {
 	int i;
 	int splitIndex =startIndex;
@@ -163,7 +258,7 @@ int	btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray&	leafNodes,int startInde
 	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
 	for (i=startIndex;i<endIndex;i++)
 	{
-		btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
 		means+=center;
 	}
 	means *= (btScalar(1.)/(btScalar)numIndices);
@@ -173,13 +268,11 @@ int	btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray&	leafNodes,int startInde
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
 	for (i=startIndex;i<endIndex;i++)
 	{
-		btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
 		if (center[splitAxis] > splitValue)
 		{
 			//swap
-			btOptimizedBvhNode tmp = leafNodes[i];
+			swapLeafNodes(i,splitIndex);
-			leafNodes[i] = leafNodes[splitIndex];
-			leafNodes[splitIndex] = tmp;
 			splitIndex++;
 		}
 	}
@@ -208,7 +301,7 @@ int	btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray&	leafNodes,int startInde
 }
 
 
-int	btOptimizedBvh::calcSplittingAxis(NodeArray&	leafNodes,int startIndex,int endIndex)
+int	btOptimizedBvh::calcSplittingAxis(int startIndex,int endIndex)
 {
 	int i;
 
@@ -218,15 +311,14 @@ int	btOptimizedBvh::calcSplittingAxis(NodeArray&	leafNodes,int startIndex,int en
 
 	for (i=startIndex;i<endIndex;i++)
 	{
-		btOptimizedBvhNode& node = leafNodes[i];
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
-		btVector3 center = btScalar(0.5)*(node.m_aabbMax+node.m_aabbMin);
 		means+=center;
 	}
 	means *= (btScalar(1.)/(btScalar)numIndices);
 		
 	for (i=startIndex;i<endIndex;i++)
 	{
-		btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
 		btVector3 diff2 = center-means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
@@ -242,11 +334,104 @@ void	btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
 {
 	//either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
 
-	//walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
 
-	walkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+	if (m_useQuantization)
+	{
+		walkStacklessQuantizedTree(nodeCallback,aabbMin,aabbMax);
+	} else
+	{
+		walkStacklessTree(nodeCallback,aabbMin,aabbMax);
+	}
 }
 
+
+int maxIterations = 0;
+
+void	btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	btAssert(!m_useQuantization);
+
+	btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool aabbOverlap, isLeafNode;
+
+	while (curIndex < m_curNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		isLeafNode = rootNode->m_escapeIndex == -1;
+		
+		if (isLeafNode && aabbOverlap)
+		{
+			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+		} 
+		
+		if (aabbOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->m_escapeIndex;
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	btAssert(m_useQuantization);
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
+	quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
+	
+	btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool aabbOverlap, isLeafNode;
+
+	while (curIndex < m_curNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		isLeafNode = rootNode->isLeafNode();
+		
+		if (isLeafNode && aabbOverlap)
+		{
+			nodeCallback->processNode(0,rootNode->getTriangleIndex());
+		} 
+		
+		if (aabbOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+/*
+///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);
@@ -264,46 +449,8 @@ void	btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback
 	}
 
 }
+*/
 
-int maxIterations = 0;
-
-void	btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
-{
-	int escapeIndex, curIndex = 0;
-	int walkIterations = 0;
-	bool aabbOverlap, isLeafNode;
-
-	while (curIndex < m_curNodeIndex)
-	{
-		//catch bugs in tree data
-		assert (walkIterations < m_curNodeIndex);
-
-		walkIterations++;
-		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
-		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
-		
-		if (isLeafNode && aabbOverlap)
-		{
-			nodeCallback->processNode(rootNode);
-		} 
-		
-		if (aabbOverlap || isLeafNode)
-		{
-			rootNode++;
-			curIndex++;
-		} else
-		{
-			escapeIndex = rootNode->m_escapeIndex;
-			rootNode += escapeIndex;
-			curIndex += escapeIndex;
-		}
-		
-	}
-
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
-
-}
 
 
 void	btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
@@ -311,3 +458,56 @@ void	btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCal
 
 }
 
+
+void btOptimizedBvh::quantizeWithClamp(unsigned short* out, const btVector3& point) const
+{
+
+	btAssert(m_useQuantization);
+
+	btVector3 clampedPoint(point);
+	clampedPoint.setMax(m_bvhAabbMin);
+	clampedPoint.setMin(m_bvhAabbMax);
+
+	btVector3 v = (clampedPoint - m_bvhAabbMin) * m_bvhQuantization;
+	out[0] = (unsigned short)(v.getX()+0.5f);
+	out[1] = (unsigned short)(v.getY()+0.5f);
+	out[2] = (unsigned short)(v.getZ()+0.5f);		
+}
+
+btVector3	btOptimizedBvh::unQuantize(const unsigned short* vecIn) const
+{
+	btVector3	vecOut;
+	vecOut.setValue(
+		(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
+		(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
+		(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
+	vecOut += m_bvhAabbMin;
+	return vecOut;
+}
+
+
+void	btOptimizedBvh::swapLeafNodes(int i,int splitIndex)
+{
+	if (m_useQuantization)
+	{
+			btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
+			m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
+			m_quantizedLeafNodes[splitIndex] = tmp;
+	} else
+	{
+			btOptimizedBvhNode tmp = m_leafNodes[i];
+			m_leafNodes[i] = m_leafNodes[splitIndex];
+			m_leafNodes[splitIndex] = tmp;
+	}
+}
+
+void	btOptimizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
+{
+	if (m_useQuantization)
+	{
+		m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
+	} else
+	{
+		m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
+	}
+}

src/BulletCollision/CollisionShapes/btOptimizedBvh.h

@@ -23,67 +23,199 @@ subject to the following restrictions:
 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
 
 
-#include <vector>
-
 
 class btStridingMeshInterface;
 
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///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];
+	//4 bytes
+	int	m_escapeIndexOrTriangleIndex;
+
+	bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrTriangleIndex >= 0);
+	}
+	int getEscapeIndex() const
+	{
+		btAssert(!isLeafNode());
+		return -m_escapeIndexOrTriangleIndex;
+	}
+	int	getTriangleIndex() const
+	{
+		btAssert(isLeafNode());
+		return m_escapeIndexOrTriangleIndex;
+	}
+};
+
 /// btOptimizedBvhNode contains both internal and leaf node information.
-/// It hasn't been optimized yet for storage. Some obvious optimizations are:
+/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
-/// Removal of the pointers (can already be done, they are not used for traversal)
-/// and storing aabbmin/max as quantized integers.
-/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
-/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
 ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode)
 {
+	//32 bytes
+	btVector3	m_aabbMinOrg;
+	btVector3	m_aabbMaxOrg;
 
-	btVector3	m_aabbMin;
+	//4
-	btVector3	m_aabbMax;
-
-//these 2 pointers are obsolete, the stackless traversal just uses the escape index
-	btOptimizedBvhNode*	m_leftChild;
-	btOptimizedBvhNode*	m_rightChild;
-
 	int	m_escapeIndex;
 
+	//8
 	//for child nodes
 	int	m_subPart;
 	int	m_triangleIndex;
 
 };
 
+
+
 class btNodeOverlapCallback
 {
 public:
 	virtual ~btNodeOverlapCallback() {};
 
-	virtual void processNode(const btOptimizedBvhNode* node) = 0;
+	virtual void processNode(int subPart, int triangleIndex) = 0;
 };
 
 #include "../../LinearMath/btAlignedAllocator.h"
 #include "../../LinearMath/btAlignedObjectArray.h"
 
-//typedef std::vector< unsigned , allocator_type >     container_type;
-const unsigned size = (1 << 20);
-typedef btAlignedAllocator< btOptimizedBvhNode , size >  allocator_type;
-
-//typedef btAlignedObjectArray<btOptimizedBvhNode, allocator_type>	NodeArray;
 
 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
 {
 	NodeArray			m_leafNodes;
-
-	btOptimizedBvhNode*	m_rootNode1;
-	
 	btOptimizedBvhNode*	m_contiguousNodes;
+
+	QuantizedNodeArray	m_quantizedLeafNodes;
+	btQuantizedBvhNode*	m_quantizedContiguousNodes;
+
 	int					m_curNodeIndex;
 
-	int					m_numNodes;
 
+	//quantization data
+	bool				m_useQuantization;
+	btVector3			m_bvhAabbMin;
+	btVector3			m_bvhAabbMax;
+	btVector3			m_bvhQuantization;
+
+	//two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+	void	setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
+	{
+		if (m_useQuantization)
+		{
+			quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin);
+		} else
+		{
+			m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
+
+		}
+	}
+	void	setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
+	{
+		if (m_useQuantization)
+		{
+			quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax);
+		} else
+		{
+			m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
+		}
+	}
+
+	btVector3 getAabbMin(int nodeIndex) const
+	{
+		if (m_useQuantization)
+		{
+			return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
+		}
+		//non-quantized
+		return m_leafNodes[nodeIndex].m_aabbMinOrg;
+
+	}
+	btVector3 getAabbMax(int nodeIndex) const
+	{
+		if (m_useQuantization)
+		{
+			return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
+		} 
+		//non-quantized
+		return m_leafNodes[nodeIndex].m_aabbMaxOrg;
+		
+	}
+	
+	void	setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
+	{
+		if (m_useQuantization)
+		{
+			m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
+		} 
+		else
+		{
+			m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
+		}
+
+	}
+
+	void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) 
+	{
+		if (m_useQuantization)
+		{
+			unsigned short int quantizedAabbMin[3];
+			unsigned short int quantizedAabbMax[3];
+			quantizeWithClamp(quantizedAabbMin,newAabbMin);
+			quantizeWithClamp(quantizedAabbMax,newAabbMax);
+			for (int i=0;i<3;i++)
+			{
+				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
+					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
+
+				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
+					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
+
+			}
+		} else
+		{
+			//non-quantized
+			m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
+			m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);		
+		}
+	}
+
+	void	swapLeafNodes(int firstIndex,int secondIndex);
+
+	void	assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
+
+protected:
+
+	
+
+	void	buildTree	(int startIndex,int endIndex);
+
+	int	calcSplittingAxis(int startIndex,int endIndex);
+
+	int	sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
+	
+	void	walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	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
+	{
+		bool overlap = true;
+		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+		return overlap;
+	}
 
 
 public:
@@ -91,27 +223,15 @@ public:
 
 	virtual ~btOptimizedBvh();
 
-	void	build(btStridingMeshInterface* triangles);
+	void	build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression);
-
-	btOptimizedBvhNode*	buildTree	(NodeArray&	leafNodes,int startIndex,int endIndex);
-
-	int	calcSplittingAxis(NodeArray&	leafNodes,int startIndex,int endIndex);
-
-	int	sortAndCalcSplittingIndex(NodeArray&	leafNodes,int startIndex,int endIndex,int splitAxis);
-	
-	void	walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-	
-	void	walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-	
-
-	//OptimizedBvhNode*	GetRootNode() { return m_rootNode1;}
-
-	int					getNumNodes() { return m_numNodes;}
 
 	void	reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
 	void	reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
+	void quantizeWithClamp(unsigned short* out, const btVector3& point) const;
+	
+	btVector3	unQuantize(const unsigned short* vecIn) const;
 
 };