diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 28cc841c6..6026c3cf4 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/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;igetCollisionObjectArray()[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; - ident.setIdentity(); - ident.setOrigin(btVector3(0,0,0)); - compoundShape->addChildShape(ident,oldShape);// - ident.setOrigin(btVector3(0,0,2)); - compoundShape->addChildShape(ident,new btSphereShape(0.9));// + comOffset.setIdentity(); + +#ifdef CENTER_OF_MASS_SHIFT + comOffset.setOrigin(comOffsetVec); + compoundShape->addChildShape(comOffset,oldShape); + +#else + compoundShape->addChildShape(tr,oldShape); + tr.setOrigin(sphereOffset); + compoundShape->addChildShape(tr,new btSphereShape(0.9)); +#endif } for (i=0;igetValue(); 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 y = halfExtents.get(1); - float z = halfExtents.get(2); + float x = halfExtents.get(0)*m_unitMeterScaling; + float y = halfExtents.get(1)*m_unitMeterScaling; + 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 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 ); } diff --git a/Demos/ColladaDemo/ColladaConverter.h b/Demos/ColladaDemo/ColladaConverter.h index dd8c2af44..cfec57bff 100644 --- a/Demos/ColladaDemo/ColladaConverter.h +++ b/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]; diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index 24b689c9c..80fbaa256 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/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; diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp index 1863a3a70..4c7782bc8 100644 --- a/Demos/VehicleDemo/VehicleDemo.cpp +++ b/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 // diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index 9480d9206..63e6069d7 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/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); diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 8334a549e..bf78fe63b 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/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) -:btTriangleMeshShape(meshInterface) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression) +: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_meshInterface->unLockReadOnlyVertexBase(node->m_subPart); + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + 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... } } diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 65fa9705c..b797b19c8 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/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(); diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index c37770270..67f945fd4 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -18,17 +18,16 @@ 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,61 +38,148 @@ 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)); - node.m_aabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - node.m_aabbMin.setMin(triangle[0]); - node.m_aabbMax.setMax(triangle[0]); - node.m_aabbMin.setMin(triangle[1]); - node.m_aabbMax.setMax(triangle[1]); - node.m_aabbMin.setMin(triangle[2]); - node.m_aabbMax.setMax(triangle[2]); + 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]); + + //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); - NodeTriangleCallback callback(m_leafNodes); + //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; - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + + triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); - //now we have an array of leafnodes in m_leafNodes + //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)); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + + 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)); - internalNode->m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30))); + setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30))); for (i=startIndex;im_aabbMax.setMax(leafNodes[i].m_aabbMax); - internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin); + mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); } + 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 splitValue) { //swap - btOptimizedBvhNode tmp = leafNodes[i]; - leafNodes[i] = leafNodes[splitIndex]; - leafNodes[splitIndex] = tmp; + swapLeafNodes(i,splitIndex); 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;im_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]; + } +} diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 67031f2e5..aba5b3bba 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -23,95 +23,215 @@ subject to the following restrictions: //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp -#include - 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: -/// 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 +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode) { + //32 bytes + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; - btVector3 m_aabbMin; - btVector3 m_aabbMax; - -//these 2 pointers are obsolete, the stackless traversal just uses the escape index - btOptimizedBvhNode* m_leftChild; - btOptimizedBvhNode* m_rightChild; - + //4 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 NodeArray; typedef btAlignedObjectArray NodeArray; +typedef btAlignedObjectArray 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: btOptimizedBvh(); virtual ~btOptimizedBvh(); - - void build(btStridingMeshInterface* triangles); - 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 build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression); 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; };