diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 05573c7cf..fcbba5484 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -36,6 +36,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btGjkEpaSolver2::sResults results; + + if(btGjkEpaSolver2::Penetration(pConvexA,transformA, pConvexB,transformB, guessVector,results)) @@ -46,7 +48,15 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; return true; + } else + { + if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + return false; } + } return false; } diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 774fde26c..bcc2f6105 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -324,6 +324,32 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } } else { + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + btVector3 tmpNormalInB = tmpPointOnA - tmpPointOnB; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = (tmpPointOnA-tmpPointOnB).length(); + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + m_lastUsedMethod = 6; + } else + { + + } + } m_lastUsedMethod = 5; }