From 3d3d24afa9e8d851d3749e4b7f52d3ab8dd819bd Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 28 Feb 2008 02:55:25 +0000 Subject: [PATCH] refit tree needs an aabbMin/aabbMax improved unquantization for quantized trees reverted heightfield quantize --- .../ConcaveConvexcastDemo.cpp | 5 +- Demos/ConcaveDemo/ConcavePhysicsDemo.cpp | 5 +- .../ConcaveRaycastDemo/ConcaveRaycastDemo.cpp | 5 +- Demos/OpenGL/GL_ShapeDrawer.cpp | 2 +- .../btBvhTriangleMeshShape.cpp | 4 +- .../CollisionShapes/btBvhTriangleMeshShape.h | 2 +- .../btHeightfieldTerrainShape.cpp | 9 +- .../CollisionShapes/btOptimizedBvh.cpp | 23 ++--- .../CollisionShapes/btOptimizedBvh.h | 98 ++++++++++++++++--- 9 files changed, 118 insertions(+), 35 deletions(-) diff --git a/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp b/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp index d776ce11b..4e0115c2d 100644 --- a/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp +++ b/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()); diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index 11425e0f8..d61ddc797 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/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()); diff --git a/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp b/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp index b3d7c7106..467a58394 100644 --- a/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp +++ b/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()); } diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 0524103d4..b44d640d7 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/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" diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index e640df3f4..662830a0b 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/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(); } diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 4f50430cf..0a3d5c88f 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/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); diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 855ed6eeb..5c3e0e228 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/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); - out[2] = (unsigned short)(((unsigned short)v.getZ() & 0xffffffe) | isMax); + + //TODO: optimization: check out how to removed this btFabs + + 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) ); } diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 4f9a22d7c..d20a5de6a 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/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->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax,1); + m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); + 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); - quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax,1); + quantize(&quantizedQueryAabbMin[0],aabbMin,0); + quantize(&quantizedQueryAabbMax[0],aabbMax,1); int i; for (i=0;im_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); - quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax,1); + quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); + 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))); - setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30))); + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin); + setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax); for (i=startIndex;i +#include +#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); - quantizeWithClamp(quantizedAabbMax,newAabbMax,1); + quantize(quantizedAabbMin,newAabbMin,0); + 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); - out[1] = (unsigned short)(((unsigned short)v.getY() & 0xfffe) | isMax); - out[2] = (unsigned short)(((unsigned short)v.getZ() & 0xfffe) | isMax); + + quantize(out,clampedPoint,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);