Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

Show All 20 Lines
#if defined(DEBUG) || defined (_DEBUG) #if defined(DEBUG) || defined (_DEBUG)
//#define TEST_NON_VIRTUAL 1 //#define TEST_NON_VIRTUAL 1
#include <stdio.h> //for debug printf #include <stdio.h> //for debug printf
#ifdef __SPU__ #ifdef __SPU__
#include <spu_printf.h> #include <spu_printf.h>
#define printf spu_printf #define printf spu_printf
//#define DEBUG_SPU_COLLISION_DETECTION 1
#endif //__SPU__ #endif //__SPU__
#endif #endif
//must be above the machine epsilon //must be above the machine epsilon
#define REL_ERROR2 btScalar(1.0e-6) #define REL_ERROR2 btScalar(1.0e-6)
//temp globals, to improve GJK/EPA/penetration calculations //temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0; int gNumDeepPenetrationChecks = 0;
Show All 38 Linesvoid btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
(void)swapResults; (void)swapResults;
getClosestPointsNonVirtual(input,output,debugDraw); getClosestPointsNonVirtual(input,output,debugDraw);
} }
#ifdef __SPU__ #ifdef __SPU__
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
#else #else
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
#endif #endif
{ {
m_cachedSeparatingDistance = 0.f; m_cachedSeparatingDistance = 0.f;
btScalar distance=btScalar(0.); btScalar distance=btScalar(0.);
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 pointOnA,pointOnB; btVector3 pointOnA,pointOnB;
btTransform localTransA = input.m_transformA; btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB; btTransform localTransB = input.m_transformB;
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
localTransA.getOrigin() -= positionOffset; localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset;
bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
btScalar marginA = m_marginA; btScalar marginA = m_marginA;
btScalar marginB = m_marginB; btScalar marginB = m_marginB;
gNumGjkChecks++; gNumGjkChecks++;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins //for CCD we don't use margins
if (m_ignoreMargin) if (m_ignoreMargin)
{ {
marginA = btScalar(0.); marginA = btScalar(0.);
marginB = btScalar(0.); marginB = btScalar(0.);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
} }
m_curIter = 0; m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue(0,1,0); m_cachedSeparatingAxis.setValue(0,1,0);
bool isValid = false; bool isValid = false;
bool checkSimplex = false; bool checkSimplex = false;
Show All 14 Lines m_lastUsedMethod = -1;
for ( ; ; ) for ( ; ; )
//while (true) //while (true)
{ {
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
#if 1
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
#else
#ifdef __SPU__
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#else
btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
btAssert((pInAv-pInA).length() < 0.0001);
btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
#endif
btVector3 pWorld = localTransA(pInA); btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB); btVector3 qWorld = localTransB(qInB);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("got local supporting vertices\n");
#endif
if (check2d) if (check2d)
{ {
pWorld[2] = 0.f; pWorld[2] = 0.f;
qWorld[2] = 0.f; qWorld[2] = 0.f;
} }
btVector3 w = pWorld - qWorld; btVector3 w = pWorld - qWorld;
Show All 27 Lines //while (true)
} else } else
{ {
m_degenerateSimplex = 11; m_degenerateSimplex = 11;
} }
checkSimplex = true; checkSimplex = true;
break; break;
} }
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 1\n");
#endif
//add current vertex to simplex //add current vertex to simplex
m_simplexSolver->addVertex(w, pWorld, qWorld); m_simplexSolver->addVertex(w, pWorld, qWorld);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 2\n");
#endif
btVector3 newCachedSeparatingAxis; btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v) //calculate the closest point to the origin (update vector v)
if (!m_simplexSolver->closest(newCachedSeparatingAxis)) if (!m_simplexSolver->closest(newCachedSeparatingAxis))
{ {
m_degenerateSimplex = 3; m_degenerateSimplex = 3;
checkSimplex = true; checkSimplex = true;
break; break;
Show All 33 Lines// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
break; break;
} }
m_cachedSeparatingAxis = newCachedSeparatingAxis; m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter) if (m_curIter++ > gGjkMaxIter)
{ {
#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) #if defined(DEBUG) || defined (_DEBUG)
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(), m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(), m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(), m_cachedSeparatingAxis.getZ(),
squaredDistance, squaredDistance,
m_minkowskiA->getShapeType(), m_minkowskiA->getShapeType(),
Show All 16 Lines// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
break; break;
} }
} }
if (checkSimplex) if (checkSimplex)
{ {
m_simplexSolver->compute_points(pointOnA, pointOnB); m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = m_cachedSeparatingAxis; normalInB = m_cachedSeparatingAxis;
btScalar lenSqr =m_cachedSeparatingAxis.length2(); btScalar lenSqr =m_cachedSeparatingAxis.length2();
//valid normal //valid normal
if (lenSqr < 0.0001) if (lenSqr < 0.0001)
{ {
m_degenerateSimplex = 5; m_degenerateSimplex = 5;
} }
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
normalInB *= rlen; //normalize normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance); btScalar s = btSqrt(squaredDistance);
btAssert(s > btScalar(0.0)); btAssert(s > btScalar(0.0));
pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s); pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((btScalar(1.)/rlen) - margin); distance = ((btScalar(1.)/rlen) - margin);
isValid = true; isValid = true;
Show All 39 Lines if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
tmpNormalInB = m_cachedSeparatingAxis; tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2(); lenSqr = m_cachedSeparatingAxis.length2();
} }
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
{ {
tmpNormalInB /= btSqrt(lenSqr); tmpNormalInB /= btSqrt(lenSqr);
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
m_lastUsedMethod = 3;
//only replace valid penetrations when the result is deeper (check) //only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance)) if (!isValid || (distance2 < distance))
{ {
distance = distance2; distance = distance2;
pointOnA = tmpPointOnA; pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB; pointOnB = tmpPointOnB;
normalInB = tmpNormalInB; normalInB = tmpNormalInB;
///todo: need to track down this EPA penetration solver degeneracy
///the penetration solver reports penetration but the contact normal
///connecting the contact points is pointing in the opposite direction
///until then, detect the issue and revert the normal
{
btScalar d1=0;
{
btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
d1 = (-normalInB).dot(w);
}
btScalar d0 = 0.f;
{
btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
d0 = normalInB.dot(w);
}
if (d1>d0)
{
m_lastUsedMethod = 10;
normalInB*=-1;
}
}
isValid = true; isValid = true;
m_lastUsedMethod = 3;
} else } else
{ {
m_lastUsedMethod = 8; m_lastUsedMethod = 8;
} }
} else } else
{ {
m_lastUsedMethod = 9; m_lastUsedMethod = 9;
} }
Show All 15 Lines if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
{ {
distance = distance2; distance = distance2;
pointOnA = tmpPointOnA; pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB; pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA ; pointOnA -= m_cachedSeparatingAxis * marginA ;
pointOnB += m_cachedSeparatingAxis * marginB ; pointOnB += m_cachedSeparatingAxis * marginB ;
normalInB = m_cachedSeparatingAxis; normalInB = m_cachedSeparatingAxis;
normalInB.normalize(); normalInB.normalize();
isValid = true; isValid = true;
m_lastUsedMethod = 6; m_lastUsedMethod = 6;
} else } else
{ {
m_lastUsedMethod = 5; m_lastUsedMethod = 5;
} }
} }
} }
} }
} }
} }
if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
{ {
#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
if (m_fixContactNormalDirection)
{
///@workaround for sticky convex collisions
//in some degenerate cases (usually when the use uses very small margins)
//the contact normal is pointing the wrong direction
//so fix it now (until we can deal with all degenerate cases in GJK and EPA)
//contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
//We like to use a dot product of the normal against the difference of the centroids,
//once the centroid is available in the API
//until then we use the center of the aabb to approximate the centroid
btVector3 aabbMin,aabbMax;
m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
btVector3 posA = (aabbMax+aabbMin)*btScalar(0.5);
m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5);
btVector3 diff = posA-posB;
if (diff.dot(normalInB) < 0.f)
normalInB *= -1.f;
}
m_cachedSeparatingAxis = normalInB; m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance; m_cachedSeparatingDistance = distance;
output.addContactPoint( output.addContactPoint(
normalInB, normalInB,
pointOnB+positionOffset, pointOnB+positionOffset,
distance); distance);
Show All 9 Lines