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

Add support for generic 2d convex shapes, through wrapper class btConvex2dShape. See Bullet/Demos/Box2dDemo for example wrapping a btCylinderShape and 2d btConvexHullShape.

Browse Source 
Add some extra degeneracy debugging check in btGjkPairDetector
This commit is contained in:
erwin.coumans
2009-09-17 19:45:22 +00:00
parent 3da9c832ae
commit f65e829ca0
14 changed files with 657 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@@ -39,7 +39,8 @@ int gNumGjkChecks = 0;
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_penetrationDepthSolver(penetrationDepthSolver),
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver),
m_minkowskiA(objectA),
m_minkowskiB(objectB),
@@ -53,7 +54,7 @@ m_catchDegeneracies(1)
{
}
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver),
m_minkowskiA(objectA),
@@ -92,6 +93,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset;
bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
btScalar marginA = m_marginA;
btScalar marginB = m_marginB;
@@ -171,12 +173,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
spu_printf("got local supporting vertices\n");
#endif
if (check2d)
{
pWorld[2] = 0.f;
qWorld[2] = 0.f;
}
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex=true;
//checkPenetration = false;
break;
@@ -198,6 +207,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
if (f0 <= btScalar(0.))
{
m_degenerateSimplex = 2;
} else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
@@ -231,6 +243,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if 0
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
@@ -238,6 +252,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
checkSimplex = false;
break;
}
#endif //
m_cachedSeparatingAxis = newCachedSeparatingAxis;
@@ -248,6 +263,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
@@ -278,6 +295,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
//do we need this backup_closest here ?
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
@@ -286,7 +304,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = pointOnA-pointOnB;
btScalar lenSqr = m_cachedSeparatingAxis.length2();
btScalar lenSqr =m_cachedSeparatingAxis.length2();
//valid normal
if (lenSqr < 0.0001)
{
@@ -390,6 +409,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
pointOnA -= m_cachedSeparatingAxis * marginA ;
pointOnB += m_cachedSeparatingAxis * marginB ;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
} else
@@ -404,16 +424,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
}
if (isValid)
if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
{
#ifdef __SPU__
//spu_printf("distance\n");
#endif //__CELLOS_LV2__
#if 0
///some debugging
// if (check2d)
{
printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
}
#endif
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("output 1\n");
#endif
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
@@ -422,10 +445,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
pointOnB+positionOffset,
distance);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("output 2\n");
#endif
//printf("gjk add:%f",distance);
}

58
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
View File

@@ -78,6 +78,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
(void)stackAlloc;
(void)v;
bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
{
@@ -132,7 +133,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
for (i=0;i<numSampleDirections;i++)
{
const btVector3& norm = sPenetrationDirections[i];
btVector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
}
@@ -173,29 +174,44 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
for (i=0;i<numSampleDirections;i++)
{
const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = transA(pInA);
qWorld = transB(qInB);
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
btVector3 norm = sPenetrationDirections[i];
if (check2d)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
norm[2] = 0.f;
}
if (norm.length2()>0.01)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = transA(pInA);
qWorld = transB(qInB);
if (check2d)
{
pWorld[2] = 0.f;
qWorld[2] = 0.f;
}
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
@@ -264,7 +280,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
if (minProj < btScalar(0.))
return false;
minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
btScalar extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
@@ -305,6 +322,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
btIntermediateResult res;
gjkdet.setCachedSeperatingAxis(-minNorm);
gjkdet.getClosestPoints(input,res,debugDraw);
btScalar correctedMinNorm = minProj - res.m_depth;
@@ -313,12 +331,14 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
//the penetration depth is over-estimated, relax it
btScalar penetration_relaxation= btScalar(1.);
minNorm*=penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#ifdef DEBUG_DRAW
if (debugDraw)