Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Calculate multiple contact points (for convex-convex and convex-plane) when less then 3 points exist in the persistent manifold.

Browse Source 
Uses the normal pertubation method, described by Gino van den Bergen:  http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
Made btRigidBody::getInvInertiaDiagLocal const, thanks to abhikp (http://code.google.com/p/bullet/issues/detail?id=183 )
This commit is contained in:
erwin.coumans
2009-02-03 00:54:01 +00:00
parent bcbe730471
commit 0754876d77
7 changed files with 145 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -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<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
(static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
(static_cast<btConvexShape*>(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<btConvexShape*>(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;i<m_numPertubationIterations;i++)
{
btQuaternion pertubeRot(v0,pertubeAngle);
btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPertubationIterations));
btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
if (pertubeA)
{
input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*pertubeRot*rotq)*body0->getWorldTransform().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)
{

16
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
View File

@@ -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,8 +85,11 @@ public:
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
btConvexPenetrationDepthSolver* m_pdSolver;
btSimplexSolverInterface* m_simplexSolver;
int m_numPertubationIterations;
int m_minimumPointsPertubationThreshold;
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
@@ -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);
}
};

23
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
View File

@@ -22,13 +22,13 @@ subject to the following restrictions:
//#include <stdio.h>
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);
}
if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPertubationThreshold)
{
btVector3 v0,v1;
btPlaneSpace1(planeNormal,v0,v1);
//now perform 'm_numPertubationIterations' collision queries with the pertubated collision objects
btQuaternion pertubeRot(v0,m_pertubeAngle);
const btScalar angleLimit = 0.125f * SIMD_PI;
btScalar pertubeAngle;
btScalar radius = convexShape->getAngularMotionDisc();
pertubeAngle = gContactBreakingThreshold / radius;
if ( pertubeAngle > angleLimit )
pertubeAngle = angleLimit;
btQuaternion pertubeRot(v0,pertubeAngle);
for (int i=0;i<m_numPertubationIterations;i++)
{
btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPertubationIterations));
btQuaternion rotq(planeNormal,iterationAngle);
collideSingleContact(rotq.inverse()*pertubeRot*rotq,body0,body1,dispatchInfo,resultOut);
}
}
if (m_ownManifold)
{

14
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
View File

@@ -32,11 +32,11 @@ class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
int m_numPertubationIterations;
btScalar m_pertubeAngle;
int m_minimumPointsPertubationThreshold;
public:
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations, btScalar pertubeAngle);
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,int minimumPointsPertubationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm();
@@ -57,11 +57,11 @@ public:
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
int m_numPertubationIterations;
btScalar m_pertubeAngle;
int m_minimumPointsPertubationThreshold;
CreateFunc()
: m_numPertubationIterations(10),
m_pertubeAngle(0.05f)
: m_numPertubationIterations(3),
m_minimumPointsPertubationThreshold(3)
{
}
@@ -70,10 +70,10 @@ public:
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(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);
}
}
};

8
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -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)
{

5
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@@ -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);

2
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -243,7 +243,7 @@ public:
return m_totalTorque;
};
const btVector3& getInvInertiaDiagLocal()
const btVector3& getInvInertiaDiagLocal() const
{
return m_invInertiaLocal;
};