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