refit tree needs an aabbMin/aabbMax

improved unquantization for quantized trees
reverted heightfield quantize

Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp

@@ -400,7 +400,10 @@ void ConcaveConvexcastDemo::clientMoveAndDisplay()
 
 		setVertexPositions(waveheight,offset);
 
-		trimeshShape->refitTree();
+		btVector3 worldMin(-1000,-1000,-1000);
+		btVector3 worldMax(1000,1000,1000);
+
+		trimeshShape->refitTree(worldMin,worldMax);
 
 		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
 		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -317,7 +317,10 @@ void ConcaveDemo::clientMoveAndDisplay()
 
 		setVertexPositions(waveheight,offset);
 
-		trimeshShape->refitTree();
+		btVector3 worldMin(-1000,-1000,-1000);
+		btVector3 worldMax(1000,1000,1000);
+
+		trimeshShape->refitTree(worldMin,worldMax);
 
 		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
 		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());

Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp

@@ -401,8 +401,11 @@ void ConcaveRaycastDemo::clientMoveAndDisplay()
 
 		setVertexPositions(waveheight,offset);
 
-		trimeshShape->refitTree();
+		btVector3 worldMin(-1000,-1000,-1000);
+		btVector3 worldMax(1000,1000,1000);
 
+		trimeshShape->refitTree(worldMin,worldMax);
+		
 		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
 		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 	}

Demos/OpenGL/GL_ShapeDrawer.cpp

@@ -41,7 +41,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btUniformScalingShape.h"
 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 ///
-#include "ConvexHull/btShapeHull.h"
+#include "btShapeHull.h"
 
 #include "LinearMath/btTransformUtil.h"
 

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -75,9 +75,9 @@ void	btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btV
 }
 
 
-void	btBvhTriangleMeshShape::refitTree()
+void	btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax)
 {
-	m_bvh->refit( m_meshInterface );
+	m_bvh->refit( m_meshInterface, aabbMin,aabbMax );
 	
 	recalcLocalAabb();
 }

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -53,7 +53,7 @@ public:
 
 	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 
-	void	refitTree();
+	void	refitTree(const btVector3& aabbMin,const btVector3& aabbMax);
 
 	///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
 	void	partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -189,9 +189,12 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi
 	clampedPoint.setMin(m_localAabbMax);
 
 	btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization;
-	out[0] = (unsigned short)(((unsigned short)v.getX() & 0xffffffe) | isMax);
+
-	out[1] = (unsigned short)(((unsigned short)v.getY() & 0xffffffe) | isMax);
+	//TODO: optimization: check out how to removed this btFabs
-	out[2] = (unsigned short)(((unsigned short)v.getZ() & 0xffffffe) | isMax);
+		
+	out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) );
+	out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) );
+	out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) );
 	
 }
 

src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -132,8 +132,8 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 				aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
 			}
 
-			m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin,0);
+			m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
-			m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax,1);
+			m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
 
 			node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 
@@ -221,8 +221,8 @@ void	btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b
 	unsigned short	quantizedQueryAabbMin[3];
 	unsigned short	quantizedQueryAabbMax[3];
 
-	quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin,0);
+	quantize(&quantizedQueryAabbMin[0],aabbMin,0);
-	quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax,1);
+	quantize(&quantizedQueryAabbMax[0],aabbMax,1);
 
 	int i;
 	for (i=0;i<this->m_SubtreeHeaders.size();i++)
@@ -328,8 +328,8 @@ void	btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f
 				aabbMin.setMin(triangleVerts[2]);
 				aabbMax.setMax(triangleVerts[2]);
 
-				quantizeWithClamp(&curNode.m_quantizedAabbMin[0],aabbMin,0);
+				quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
-				quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax,1);
+				quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
 				
 			} else
 			{
@@ -370,17 +370,14 @@ void	btOptimizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV
 	m_bvhAabbMin = bvhAabbMin - clampValue;
 	m_bvhAabbMax = bvhAabbMax + clampValue;
 	btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
-	m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize;
+	m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
 }
 
 
-void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface)
+void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
 {
 	if (m_useQuantization)
 	{
-		//calculate new aabb
-		btVector3 aabbMin,aabbMax;
-		meshInterface->calculateAabbBruteForce(aabbMin,aabbMax);
 
 		setQuantizationValues(aabbMin,aabbMax);
 
@@ -446,8 +443,8 @@ void	btOptimizedBvh::buildTree	(int startIndex,int endIndex)
 
 	int internalNodeIndex = m_curNodeIndex;
 	
-	setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)));
+	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
-	setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)));
+	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
 	
 	for (i=startIndex;i<endIndex;i++)
 	{

src/BulletCollision/CollisionShapes/btOptimizedBvh.h

@@ -16,7 +16,15 @@ subject to the following restrictions:
 #ifndef OPTIMIZED_BVH_H
 #define OPTIMIZED_BVH_H
 
+//#define DEBUG_CHECK_DEQUANTIZATION 1
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+#ifdef __SPU__
+#define printf spu_printf
+#endif //__SPU__
 
+#include <stdio.h>
+#include <stdlib.h>
+#endif //DEBUG_CHECK_DEQUANTIZATION
 
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedAllocator.h"
@@ -191,7 +199,7 @@ protected:
 	{
 		if (m_useQuantization)
 		{
-			quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
 		} else
 		{
 			m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
@@ -202,7 +210,7 @@ protected:
 	{
 		if (m_useQuantization)
 		{
-			quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
 		} else
 		{
 			m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
@@ -251,8 +259,8 @@ protected:
 		{
 			unsigned short int quantizedAabbMin[3];
 			unsigned short int quantizedAabbMax[3];
-			quantizeWithClamp(quantizedAabbMin,newAabbMin,0);
+			quantize(quantizedAabbMin,newAabbMin,0);
-			quantizeWithClamp(quantizedAabbMax,newAabbMax,1);
+			quantize(quantizedAabbMax,newAabbMax,1);
 			for (int i=0;i<3;i++)
 			{
 				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
@@ -332,19 +340,85 @@ public:
 	void	reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 	void	reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
 	void	reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
-	
+
-	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point,int isMax) const
+		SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
 	{
 
 		btAssert(m_useQuantization);
 
-		btVector3 clampedPoint(point);
+		btAssert(point.getX() <= m_bvhAabbMax.getX());
+		btAssert(point.getY() <= m_bvhAabbMax.getY());
+		btAssert(point.getZ() <= m_bvhAabbMax.getZ());
+
+		btAssert(point.getX() >= m_bvhAabbMin.getX());
+		btAssert(point.getY() >= m_bvhAabbMin.getY());
+		btAssert(point.getZ() >= m_bvhAabbMin.getZ());
+
+		btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
+		///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
+		///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
+		///todo: double-check this
+		if (isMax)
+		{
+			out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
+			out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
+			out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
+		} else
+		{
+			out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
+			out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
+			out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
+		}
+
+
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+		btVector3 newPoint = unQuantize(out);
+		if (isMax)
+		{
+			if (newPoint.getX() < point.getX())
+			{
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+			}
+			if (newPoint.getY() < point.getY())
+			{
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+			}
+			if (newPoint.getZ() < point.getZ())
+			{
+
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+			}
+		} else
+		{
+			if (newPoint.getX() > point.getX())
+			{
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+			}
+			if (newPoint.getY() > point.getY())
+			{
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+			}
+			if (newPoint.getZ() > point.getZ())
+			{
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+			}
+		}
+#endif //DEBUG_CHECK_DEQUANTIZATION
+
+	}
+
+
+	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
+	{
+
+		btAssert(m_useQuantization);
+
+		btVector3 clampedPoint(point2);
 		clampedPoint.setMax(m_bvhAabbMin);
 		clampedPoint.setMin(m_bvhAabbMax);
-		btVector3 v = (clampedPoint - m_bvhAabbMin) * m_bvhQuantization;
+
-		out[0] = (unsigned short)(((unsigned short)v.getX() & 0xfffe) | isMax);
+		quantize(out,clampedPoint,isMax);
-		out[1] = (unsigned short)(((unsigned short)v.getY() & 0xfffe) | isMax);
+
-		out[2] = (unsigned short)(((unsigned short)v.getZ() & 0xfffe) | isMax);
 	}
 	
 	SIMD_FORCE_INLINE btVector3	unQuantize(const unsigned short* vecIn) const
@@ -364,7 +438,7 @@ public:
 		m_traversalMode = traversalMode;
 	}
 
-	void	refit(btStridingMeshInterface* triangles);
+	void	refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax);
 
 	void	refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax);