From b73900bc60ab6aa7079139fb741c6682cb237a69 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Mon, 26 Feb 2007 04:59:05 +0000 Subject: [PATCH] Improved performance of convex collision shapes, cache local AABB instead of recomputation. This fixes issue with very slow performance in larger .bsp levels. Moved some asserts into 'btFullAssert', which is disabled by default (see btScalar.h to enable them). This is to speed-up debugging. --- ChangeLog.txt | 4 ++ Demos/ConcaveDemo/ConcavePhysicsDemo.cpp | 1 + .../UserCollisionAlgorithm.cpp | 4 +- .../CollisionShapes/btConvexHullShape.cpp | 2 + .../CollisionShapes/btConvexHullShape.h | 1 + .../CollisionShapes/btConvexShape.h | 1 + .../btConvexTriangleMeshShape.cpp | 3 ++ .../CollisionShapes/btCylinderShape.cpp | 9 +++- .../CollisionShapes/btCylinderShape.h | 5 +- .../btPolyhedralConvexShape.cpp | 50 ++++++++++++++++++- .../CollisionShapes/btPolyhedralConvexShape.h | 6 +++ .../CollisionShapes/btTetrahedronShape.cpp | 2 + .../CollisionShapes/btTriangleShape.h | 1 + .../btSequentialImpulseConstraintSolver.cpp | 3 +- src/LinearMath/btMatrix3x3.h | 14 +++--- src/LinearMath/btScalar.h | 4 ++ src/LinearMath/btTransformUtil.h | 7 +-- src/LinearMath/btVector3.h | 6 +-- 18 files changed, 102 insertions(+), 21 deletions(-) diff --git a/ChangeLog.txt b/ChangeLog.txt index 83e4b66eb..a16dacb29 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -1,6 +1,10 @@ Bullet Continuous Collision Detection and Physics Library Erwin Coumans + +2007 Feb 25 + - Improved performance of convex collision shapes, cache local AABB instead of recomputation. This fixes issue with very slow performance in larger .bsp levels + 2007 Feb 24 - 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) diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index 80fbaa256..bee786e07 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp @@ -153,6 +153,7 @@ void ConcaveDemo::initPhysics() btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression); + // btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); btVector3 worldMin(-1000,-1000,-1000); diff --git a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp index b883dcb3b..7f17b3e1c 100644 --- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp +++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp @@ -100,7 +100,9 @@ void UserCollisionAlgorithm::initPhysics() delete[] gVertices; - btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(trimesh); + bool useQuantizedBvhTree = false; +// btTriangleMesh uses a subpart for each triangle, this is not compatible with compressed quantized bvh node + btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(trimesh,useQuantizedBvhTree); //ConstraintSolver* solver = new btSequentialImpulseConstraintSolver; diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 23fbcaa1b..9706a340c 100644 --- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -30,6 +30,8 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride); m_points[i] = point[0]; } + + recalcLocalAabb(); } btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 4a5271bb3..f1ac469b5 100644 --- a/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -37,6 +37,7 @@ public: void addPoint(const btPoint3& point) { m_points.push_back(point); + recalcLocalAabb(); } virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.h b/src/BulletCollision/CollisionShapes/btConvexShape.h index e439a33fe..bf640aa37 100644 --- a/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -43,6 +43,7 @@ protected: btScalar m_collisionMargin; + public: btConvexShape(); diff --git a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index b6b68b1db..de4e93d0a 100644 --- a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface) :m_stridingMesh(meshInterface) { + recalcLocalAabb(); } @@ -189,5 +190,7 @@ bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); + + recalcLocalAabb(); } diff --git a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 2e8af6820..f6c77346f 100644 --- a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -18,21 +18,28 @@ subject to the following restrictions: btCylinderShape::btCylinderShape (const btVector3& halfExtents) :btBoxShape(halfExtents) { - + recalcLocalAabb(); } btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + recalcLocalAabb(); } btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + recalcLocalAabb(); } +void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + //skip the box 'getAabb' + btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax); +} inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) diff --git a/src/BulletCollision/CollisionShapes/btCylinderShape.h b/src/BulletCollision/CollisionShapes/btCylinderShape.h index 956788727..0855e86ea 100644 --- a/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -29,10 +29,7 @@ public: btCylinderShape (const btVector3& halfExtents); ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - getAabbSlow(t,aabbMin,aabbMax); - } + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 6aac93509..58306dce4 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -16,7 +16,10 @@ subject to the following restrictions: #include btPolyhedralConvexShape::btPolyhedralConvexShape() -:m_optionalHull(0) +:m_optionalHull(0), +m_localAabbMin(1,1,1), +m_localAabbMax(-1,-1,-1), +m_isLocalAabbValid(false) { } @@ -116,3 +119,48 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine } +void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + + //lazy evaluation of local aabb + btAssert(m_isLocalAabbValid); + + btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX()); + btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY()); + btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ()); + + + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += btVector3(getMargin(),getMargin(),getMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; + + +} + +void btPolyhedralConvexShape::recalcLocalAabb() +{ + m_isLocalAabbValid = true; + + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + btVector3 tmp = localGetSupportingVertex(vec); + m_localAabbMax[i] = tmp[i]+m_collisionMargin; + vec[i] = btScalar(-1.); + tmp = localGetSupportingVertex(vec); + m_localAabbMin[i] = tmp[i]-m_collisionMargin; + } +} + \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 18e1750a2..f1d40f3e2 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -25,6 +25,10 @@ subject to the following restrictions: class btPolyhedralConvexShape : public btConvexShape { + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + public: btPolyhedralConvexShape(); @@ -36,6 +40,8 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void recalcLocalAabb(); virtual int getNumVertices() const = 0 ; virtual int getNumEdges() const = 0; diff --git a/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 7cb40c4fa..e2092416a 100644 --- a/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -58,6 +58,8 @@ btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const void btBU_Simplex1to4::addVertex(const btPoint3& pt) { m_vertices[m_numVertices++] = pt; + + recalcLocalAabb(); } diff --git a/src/BulletCollision/CollisionShapes/btTriangleShape.h b/src/BulletCollision/CollisionShapes/btTriangleShape.h index e7dbae117..14a7c93ab 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -57,6 +57,7 @@ public: getVertex((i+1)%3,pb); } + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const { // btAssert(0); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ea072e131..cab15203a 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -123,7 +123,8 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() /// btSequentialImpulseConstraintSolver Sequentially applies impulses btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - + + btContactSolverInfo info = infoGlobal; int numiter = infoGlobal.m_numIterations; diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 97fc1417e..44899640f 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -45,12 +45,12 @@ class btMatrix3x3 { zx, zy, zz); } - btVector3 getColumn(int i) const + SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); } - const btVector3& getRow(int i) const + SIMD_FORCE_INLINE const btVector3& getRow(int i) const { return m_el[i]; } @@ -58,13 +58,13 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btVector3& operator[](int i) { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } - const btVector3& operator[](int i) const + SIMD_FORCE_INLINE const btVector3& operator[](int i) const { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } @@ -102,7 +102,7 @@ class btMatrix3x3 { void setRotation(const btQuaternion& q) { btScalar d = q.length2(); - assert(d != btScalar(0.0)); + btFullAssert(d != btScalar(0.0)); btScalar s = btScalar(2.0) / d; btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s; btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs; @@ -323,7 +323,7 @@ class btMatrix3x3 { { btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); btScalar det = (*this)[0].dot(co); - assert(det != btScalar(0.0)); + btFullAssert(det != btScalar(0.0)); btScalar s = btScalar(1.0) / det; return btMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index ef99310e8..4e6952860 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -38,6 +38,8 @@ subject to the following restrictions: #include #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert #else //non-windows systems @@ -48,6 +50,8 @@ subject to the following restrictions: #include #endif #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert #endif /// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. diff --git a/src/LinearMath/btTransformUtil.h b/src/LinearMath/btTransformUtil.h index 0b93b3d16..d3b219c3a 100644 --- a/src/LinearMath/btTransformUtil.h +++ b/src/LinearMath/btTransformUtil.h @@ -74,9 +74,9 @@ public: predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); // #define QUATERNION_DERIVATIVE #ifdef QUATERNION_DERIVATIVE - btQuaternion orn = curTrans.getRotation(); - orn += (angvel * orn) * (timeStep * btScalar(0.5)); - orn.normalize(); + btQuaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + predictedOrn.normalize(); #else //exponential map btVector3 axis; @@ -101,6 +101,7 @@ public: btQuaternion orn0 = curTrans.getRotation(); btQuaternion predictedOrn = dorn * orn0; + predictedOrn.normalize(); #endif predictedTransform.setRotation(predictedOrn); } diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 8d8d396ef..4222e713f 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -64,7 +64,7 @@ public: SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } @@ -99,7 +99,7 @@ public: SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } @@ -213,7 +213,7 @@ operator*(const btScalar& s, const btVector3& v) SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return v * (btScalar(1.0) / s); }