diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 1246a1787..e43dc3426 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -51,6 +51,8 @@ subject to the following restrictions: btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { + m_numPertubationIterations = 3; + m_minimumPointsPertubationThreshold = 3; m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } @@ -59,17 +61,19 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc() { } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPertubationIterations, int minimumPointsPertubationThreshold) : btActivatingCollisionAlgorithm(ci,body0,body1), m_simplexSolver(simplexSolver), m_pdSolver(pdSolver), m_ownManifold (false), m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 ,m_sepDistance((static_cast(body0->getCollisionShape()))->getAngularMotionDisc(), - (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()) + (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()), #endif +m_numPertubationIterations(numPertubationIterations), +m_minimumPointsPertubationThreshold(minimumPointsPertubationThreshold) { (void)body0; (void)body1; @@ -93,8 +97,35 @@ void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) } +struct btPertubedContactResult : public btManifoldResult +{ + btManifoldResult* m_originalManifoldResult; + btTransform m_transformA; + btTransform m_transformB; + btPertubedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB) + :m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB) + { + } + virtual ~ btPertubedContactResult() + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + const btVector3& worldPointB = pointInWorld; + btVector3 worldPointA = worldPointB+normalOnBInWorld*depth; + btVector3 localA = m_transformA.invXform(worldPointA); + btVector3 localB = m_transformB.invXform(pointInWorld); + m_originalManifoldResult->addLocalContactPointInternal( normalOnBInWorld,localA,localB); + } + +}; + +extern btScalar gContactBreakingThreshold; // // Convex-Convex collision algorithm @@ -110,6 +141,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } resultOut->setPersistentManifold(m_manifoldPtr); + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); btConvexShape* min0 = static_cast(body0->getCollisionShape()); @@ -146,9 +179,57 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_transformB = body1->getWorldTransform(); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; + //now pertube directions to get multiple contact points + btVector3 v0,v1; + btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + //now perform 'm_numPertubationIterations' collision queries with the pertubated collision objects + + //perform pertubation when more then 'm_minimumPointsPertubationThreshold' points + if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPertubationThreshold) + { + + int i; + + bool pertubeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar pertubeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) + { + pertubeAngle = gContactBreakingThreshold /radiusA; + pertubeA = true; + } else + { + pertubeAngle = gContactBreakingThreshold / radiusB; + pertubeA = false; + } + if ( pertubeAngle > angleLimit ) + pertubeAngle = angleLimit; + + for ( i=0;igetWorldTransform().getBasis()); + } else + { + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*pertubeRot*rotq)*body1->getWorldTransform().getBasis()); + } + btPertubedContactResult pertubedResultOut(resultOut,input.m_transformA,input.m_transformB); + gjkPairDetector.getClosestPoints(input,pertubedResultOut,dispatchInfo.m_debugDraw); + btScalar curSepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; + } + } + + + #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 14f015ce5..a1fc0b1db 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -33,7 +33,9 @@ class btConvexPenetrationDepthSolver; ///for certain pairs that have a small size ratio ///#define USE_SEPDISTANCE_UTIL2 1 -///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. +///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. +///Multiple contact points are calculated by pertubating the orientation of the smallest object orthogonal to the separating normal. +///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888 class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { #ifdef USE_SEPDISTANCE_UTIL2 @@ -47,12 +49,17 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm btPersistentManifold* m_manifoldPtr; bool m_lowLevelOfDetail; + int m_numPertubationIterations; + int m_minimumPointsPertubationThreshold; + + ///cache separating vector to speedup collision detection public: - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPertubationIterations, int minimumPointsPertubationThreshold); + virtual ~btConvexConvexAlgorithm(); @@ -78,9 +85,12 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { + btConvexPenetrationDepthSolver* m_pdSolver; btSimplexSolverInterface* m_simplexSolver; - + int m_numPertubationIterations; + int m_minimumPointsPertubationThreshold; + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); virtual ~CreateFunc(); @@ -88,7 +98,7 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPertubationIterations,m_minimumPointsPertubationThreshold); } }; diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index f1d4d1af2..7ae4c9da8 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -22,13 +22,13 @@ subject to the following restrictions: //#include -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,btScalar pertubeAngle) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,int minimumPointsPertubationThreshold) : btCollisionAlgorithm(ci), m_ownManifold(false), m_manifoldPtr(mf), m_isSwapped(isSwapped), m_numPertubationIterations(numPertubationIterations), -m_pertubeAngle(pertubeAngle) +m_minimumPointsPertubationThreshold(minimumPointsPertubationThreshold) { btCollisionObject* convexObj = m_isSwapped? col1 : col0; btCollisionObject* planeObj = m_isSwapped? col0 : col1; @@ -89,6 +89,7 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& } } + void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)dispatchInfo; @@ -105,23 +106,33 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - - btVector3 v0,v1; - btPlaneSpace1(planeNormal,v0,v1); //first perform a collision query with the non-pertubated collision objects { btQuaternion rotq(0,0,0,1); collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); } - //now perform 'm_numPertubationIterations' collision queries with the pertubated collision objects - btQuaternion pertubeRot(v0,m_pertubeAngle); - for (int i=0;igetPersistentManifold()->getNumContacts()getAngularMotionDisc(); + pertubeAngle = gContactBreakingThreshold / radius; + if ( pertubeAngle > angleLimit ) + pertubeAngle = angleLimit; + + btQuaternion pertubeRot(v0,pertubeAngle); + for (int i=0;iallocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPertubationIterations,m_pertubeAngle); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPertubationIterations,m_minimumPointsPertubationThreshold); } else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPertubationIterations,m_pertubeAngle); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPertubationIterations,m_minimumPointsPertubationThreshold); } } }; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 3e5fcac77..510995003 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -52,6 +52,14 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b m_rootTransB = body1->getWorldTransform(); } +void btManifoldResult::addLocalContactPointInternal(const btVector3& normalOnBInWorld,const btVector3& localPointA,const btVector3& localPointB) +{ + btVector3 worldPointA = m_rootTransA( localPointA ); + btVector3 worldPointB = m_rootTransB( localPointB ); + btScalar depth = (worldPointA - worldPointB).dot(normalOnBInWorld); + + addContactPoint(normalOnBInWorld,worldPointB,depth); +} void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 5aac9a46f..b905e9a6a 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -45,6 +45,8 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result int m_partId1; int m_index0; int m_index1; + + public: btManifoldResult() @@ -77,8 +79,11 @@ public: m_index1=index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); + virtual void addLocalContactPointInternal(const btVector3& normalOnBInWorld,const btVector3& localPointA,const btVector3& localPointB); + SIMD_FORCE_INLINE void refreshContactPoints() { btAssert(m_manifoldPtr); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 5de85c090..ec570cab8 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -243,7 +243,7 @@ public: return m_totalTorque; }; - const btVector3& getInvInertiaDiagLocal() + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; };