From 1f4f8c7e057c0da54b1df9a92fc9e6128c75eb06 Mon Sep 17 00:00:00 2001 From: "erwin.coumans@gmail.com" Date: Tue, 2 Apr 2013 00:32:18 +0000 Subject: [PATCH] Workaround for sticky convex collisions when using GJK/EPA in combination with very small collision margins. In some degenerate cases 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 --- .../btGjkEpaPenetrationDepthSolver.cpp | 2 +- .../btGjkPairDetector.cpp | 27 +++++++++++++++++-- .../NarrowPhaseCollision/btGjkPairDetector.h | 2 +- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index c6dc3f3a6..7087953cb 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -34,7 +34,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim // const btScalar radialmargin(btScalar(0.)); - btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); + btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); btGjkEpaSolver2::sResults results; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 8af16b9cf..2594e8c8d 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -50,7 +50,8 @@ m_marginA(objectA->getMargin()), m_marginB(objectB->getMargin()), m_ignoreMargin(false), m_lastUsedMethod(-1), -m_catchDegeneracies(1) +m_catchDegeneracies(1), +m_fixContactNormalDirection(1) { } btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) @@ -65,7 +66,8 @@ m_marginA(marginA), m_marginB(marginB), m_ignoreMargin(false), m_lastUsedMethod(-1), -m_catchDegeneracies(1) +m_catchDegeneracies(1), +m_fixContactNormalDirection(1) { } @@ -438,6 +440,27 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } #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_cachedSeparatingDistance = distance; diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index f0043b8b9..feeae6862 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -52,7 +52,7 @@ public: int m_curIter; int m_degenerateSimplex; int m_catchDegeneracies; - + int m_fixContactNormalDirection; btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);