From a7e04dbdc2abdb0bb8bdc9f4b08b1e86f4e1020e Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Tue, 11 Dec 2007 23:13:29 +0000 Subject: [PATCH] Added faster and more robust support for btStaticPlaneShape --- Demos/BasicDemo/BasicDemo.cpp | 11 +- Demos/BasicDemo/main.cpp | 5 +- .../btConvexPlaneCollisionAlgorithm.cpp | 109 ++++++++++++++++++ .../btConvexPlaneCollisionAlgorithm.h | 63 ++++++++++ .../btDefaultCollisionConfiguration.cpp | 29 ++++- .../btDefaultCollisionConfiguration.h | 4 +- .../CollisionShapes/btStaticPlaneShape.h | 9 ++ 7 files changed, 220 insertions(+), 10 deletions(-) create mode 100644 src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp create mode 100644 src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index fcf8b5ff7..eb857f186 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -96,15 +96,16 @@ void BasicDemo::initPhysics() ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; m_solver = sol; - + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); ///create a few basic rigid bodies - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); - +// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -135,8 +136,8 @@ void BasicDemo::initPhysics() //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance - //btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1)); - btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1)); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects diff --git a/Demos/BasicDemo/main.cpp b/Demos/BasicDemo/main.cpp index 09c575885..f85e7dfb6 100644 --- a/Demos/BasicDemo/main.cpp +++ b/Demos/BasicDemo/main.cpp @@ -15,14 +15,17 @@ subject to the following restrictions: #include "BasicDemo.h" #include "GlutStuff.h" +#include "GLDebugDrawer.h" +#include "btBulletDynamicsCommon.h" -//#define CHECK_MEMORY_LEAKS 1 int main(int argc,char** argv) { + GLDebugDrawer gDebugDrawer; BasicDemo ccdDemo; ccdDemo.initPhysics(); + ccdDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); #ifdef CHECK_MEMORY_LEAKS diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp new file mode 100644 index 000000000..9b39433dd --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,109 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +//#include + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + btCollisionObject* convexObj = m_isSwapped? col1 : col0; + btCollisionObject* planeObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + //btVector3 vtx = convexShape->localGetSupportingVertexWithoutMargin(planeNormal); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + //btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant) - convexShape->getMargin(); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h new file mode 100644 index 000000000..b503ba40c --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -0,0 +1,63 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H +#define CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 661270b9b..83286c37a 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" + #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" @@ -58,7 +60,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* s m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); @@ -72,7 +74,13 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* s m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; - + //convex versus plane + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + m_planeConvexCF->m_swapped = true; + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); @@ -167,6 +175,11 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_triangleSphereCF); + m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexPlaneCF); + m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_planeConvexCF); + m_simplexSolver->~btVoronoiSimplexSolver(); btAlignedFree(m_simplexSolver); m_pdSolver->~btGjkEpaPenetrationDepthSolver(); @@ -178,7 +191,8 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { - + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) { @@ -205,6 +219,15 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_triangleSphereCF; } + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2e99f1db1..14475ad14 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -54,7 +54,9 @@ class btDefaultCollisionConfiguration : public btCollisionConfiguration btCollisionAlgorithmCreateFunc* m_boxSphereCF; btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; btCollisionAlgorithmCreateFunc* m_triangleSphereCF; - + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + public: btDefaultCollisionConfiguration(btStackAlloc* stackAlloc=0,btPoolAllocator* persistentManifoldPool=0,btPoolAllocator* collisionAlgorithmPool=0); diff --git a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index 0cbce3abd..42b8cf113 100644 --- a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -51,6 +51,15 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + const btVector3& getPlaneNormal() const + { + return m_planeNormal; + } + + const btScalar& getPlaneConstant() const + { + return m_planeConstant; + } //debugging virtual const char* getName()const {return "STATICPLANE";}