Fixed tolerance in assert in GetPenetrationDepthStepEPA (#1061)

Fixes #1037

Jolt/Geometry/EPAPenetrationDepth.h

@@ -47,6 +47,11 @@ private:
 	/// The GJK algorithm, used to start the EPA algorithm
 	GJKClosestPoint		mGJK;
 
+#ifdef JPH_ENABLE_ASSERTS
+	/// Tolerance as passed to the GJK algorithm, used for asserting.
+	float				mGJKTolerance = 0.0f;
+#endif // JPH_ENABLE_ASSERTS
+
 	/// A list of support points for the EPA algorithm
 	class SupportPoints
 	{
@@ -100,6 +105,8 @@ public:
 	{
 		JPH_PROFILE_FUNCTION();
 
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;)
+
 		// Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points
 		JPH_ASSERT(!ioV.IsNearZero());
 
@@ -154,7 +161,7 @@ public:
 		case 1:
 			{
 				// 1 vertex, which must be at the origin, which is useless for our purpose
-				JPH_ASSERT(support_points.mY[0].IsNearZero(1.0e-8f));
+				JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance)));
 				support_points.mY.pop_back();
 
 				// Add support points in 4 directions to form a tetrahedron around the origin
@@ -514,6 +521,8 @@ public:
 	template <typename A, typename B>
 	bool				CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal)
 	{
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;)
+
 		// First determine if there's a collision at all
 		if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal))
 			return false;