diff --git a/Demos/ConcaveDemo/ConcaveDemo.h b/Demos/ConcaveDemo/ConcaveDemo.h index ca866dea0..e5b06f14d 100644 --- a/Demos/ConcaveDemo/ConcaveDemo.h +++ b/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); }; diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index 8fced2c13..2318bbe4f 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/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]; -int gIndices[NUM_TRIANGLES*3]; -const float TRIANGLE_SIZE=80.f; +btVector3* gVertices=0; +int* gIndices=0; +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;isetCollisionFlags(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(); diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index bf78fe63b..be36183f5 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/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; diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index b797b19c8..5048e5b8e 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/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";} diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 67f945fd4..133ee2b35 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/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 - 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; + initQuantizationValues(triangles); 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; diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index aba5b3bba..b0621e9d3 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -97,8 +97,9 @@ class btOptimizedBvh btOptimizedBvhNode* m_contiguousNodes; QuantizedNodeArray m_quantizedLeafNodes; - btQuantizedBvhNode* m_quantizedContiguousNodes; - + + QuantizedNodeArray m_quantizedContiguousNodes; + int m_curNodeIndex; @@ -151,6 +152,10 @@ class btOptimizedBvh return m_leafNodes[nodeIndex].m_aabbMaxOrg; } + + void initQuantizationValues(btStridingMeshInterface* triangles); + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) { @@ -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); };