diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index fd09db27..c82256f3 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -39,6 +39,9 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Static variables initialization +const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); + // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { @@ -353,7 +356,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // We have found a separating axis return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { isMinPenetrationFaceNormal = true; minPenetrationDepth = penetrationDepth; minFaceIndex = faceIndex; @@ -367,7 +370,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // We have found a separating axis return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { isMinPenetrationFaceNormal = true; minPenetrationDepth = penetrationDepth; minFaceIndex = faceIndex; @@ -409,7 +412,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + minPenetrationDepth = penetrationDepth; isMinPenetrationFaceNormalPolyhedron1 = false; isMinPenetrationFaceNormal = false; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index ae5da16e..a95abe31 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -41,6 +41,11 @@ class SATAlgorithm { // -------------------- Attributes -------------------- // + /// Bias used to make sure the SAT algorithm returns the same penetration axis between frames + /// when there are multiple separating axis with the same penetration depth. The goal is to + /// make sure the contact manifold does not change too much between frames. + static const decimal SAME_SEPARATING_AXIS_BIAS; + // -------------------- Methods -------------------- // /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)