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

pybullet getClosestPoints

Browse Source
This commit is contained in:
erwincoumans
2016-11-09 21:01:04 -08:00
parent 2bb53b311d
commit 0d47d61007
15 changed files with 1373 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1252,6 +1252,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
if (algorithm)
{
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
//discrete collision detection query
algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);

6
src/BulletCollision/CollisionDispatch/btCollisionWorld.h
View File

@@ -412,10 +412,12 @@ public:
{
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
btScalar m_closestDistanceThreshold;
ContactResultCallback()
:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter)
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
m_closestDistanceThreshold(0)
{
}

24
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -161,6 +161,13 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMin1 -= thresholdVec;
aabbMax0 += thresholdVec;
aabbMax1 += thresholdVec;
if (gCompoundCompoundChildShapePairCallback)
{
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
@@ -217,10 +224,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btTransform& xform)
const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
{
btVector3 newmin,newmax;
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
return Intersect(a,newb);
}
@@ -229,7 +238,7 @@ static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
static inline void MycollideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
const btTransform& xform,
btCompoundCompoundLeafCallback* callback)
btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
{
if(root0&&root1)
@@ -241,7 +250,7 @@ static inline void MycollideTT( const btDbvtNode* root0,
stkStack[0]=btDbvt::sStkNN(root0,root1);
do {
btDbvt::sStkNN p=stkStack[--depth];
if(MyIntersect(p.a->volume,p.b->volume,xform))
if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
{
if(depth>treshold)
{
@@ -343,7 +352,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
//printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
@@ -383,7 +392,9 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
newChildWorldTrans0 = orgTrans0*childTrans0 ;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
}
btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMax0 += thresholdVec;
{
btTransform orgInterpolationTrans1;
const btCollisionShape* childShape1 = 0;
@@ -398,7 +409,8 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
}
aabbMin1 -= thresholdVec;
aabbMax1 += thresholdVec;
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
{

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

@@ -365,7 +365,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
// input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold;
// }
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;

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

@@ -49,17 +49,19 @@ protected:
int m_index0;
int m_index1;
public:
btManifoldResult()
#ifdef DEBUG_PART_INDEX
:
#ifdef DEBUG_PART_INDEX
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
m_closestPointDistanceThreshold(0)
{
}
@@ -142,6 +144,8 @@ public:
return m_body1Wrap->getCollisionObject();
}
btScalar m_closestPointDistanceThreshold;
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);

2
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@@ -62,7 +62,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
{
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();