From 4dd129f62271d1675b5cdd3baf24214a773016ab Mon Sep 17 00:00:00 2001 From: Jorrit Rouwe Date: Sat, 11 Jan 2025 17:30:14 +0100 Subject: [PATCH] Fixed numerical inaccuracy in penetration depth calculation when CollideShapeSettings::mMaxSeparationDistance was set to a really high value (e.g. 1000). (#1451) * Also fixed missing collision registrations in TriangleShape and SoftBodyShape * Some sonar fixes Fixes #1379 --- Docs/ReleaseNotes.md | 1 + Jolt/Geometry/EPAPenetrationDepth.h | 6 +-- .../Collision/CollideConvexVsTriangles.cpp | 15 ++++-- Jolt/Physics/Collision/Shape/ConvexShape.cpp | 23 +++++--- .../Physics/Collision/Shape/TriangleShape.cpp | 3 ++ Jolt/Physics/SoftBody/SoftBodyShape.cpp | 3 ++ .../ConvexCollision/ConvexHullShrinkTest.cpp | 2 +- Samples/Tests/ConvexCollision/EPATest.cpp | 4 +- .../ConvexCollision/InteractivePairsTest.cpp | 12 ++--- UnitTests/Geometry/EPATests.cpp | 4 +- UnitTests/Physics/CollideShapeTests.cpp | 53 +++++++++++++++++++ 11 files changed, 100 insertions(+), 26 deletions(-) diff --git a/Docs/ReleaseNotes.md b/Docs/ReleaseNotes.md index d88916c61..ef5f85b77 100644 --- a/Docs/ReleaseNotes.md +++ b/Docs/ReleaseNotes.md @@ -34,6 +34,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi * VehicleConstraint would override Body::SetAllowSleeping every frame, making it impossible for client code to configure a vehicle that cannot go to sleep. * Fixed CharacterVirtual::Contact::mIsSensorB not being persisted in SaveState. * Fixed CharacterVirtual::Contact::mHadContact not being true for collisions with sensors. They will still be marked as mWasDiscarded to prevent any further interaction. +* Fixed numerical inaccuracy in penetration depth calculation when CollideShapeSettings::mMaxSeparationDistance was set to a really high value (e.g. 1000). ## v5.2.0 diff --git a/Jolt/Geometry/EPAPenetrationDepth.h b/Jolt/Geometry/EPAPenetrationDepth.h index ffa0c3912..498970193 100644 --- a/Jolt/Geometry/EPAPenetrationDepth.h +++ b/Jolt/Geometry/EPAPenetrationDepth.h @@ -540,9 +540,9 @@ class EPAPenetrationDepth || contact_normal_invalid)) { // If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point - AddConvexRadius add_convex_a(inA, inConvexRadiusA); - AddConvexRadius add_convex_b(inB, inConvexRadiusB); - TransformedConvexObject> transformed_a(inStart, add_convex_a); + AddConvexRadius add_convex_a(inA, inConvexRadiusA); + AddConvexRadius add_convex_b(inB, inConvexRadiusB); + TransformedConvexObject transformed_a(inStart, add_convex_a); if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB)) return false; } diff --git a/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp index f00b0023a..c57e750be 100644 --- a/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +++ b/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp @@ -79,7 +79,8 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1); // Perform GJK step - status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + float max_separation_distance = mCollideShapeSettings.mMaxSeparationDistance; + status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + max_separation_distance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); // Check result of collision detection if (status == EPAPenetrationDepth::EStatus::NotColliding) @@ -88,12 +89,18 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, { // Need to run expensive EPA algorithm + // We know we're overlapping at this point, so we can set the max separation distance to 0. + // Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated. + // In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape, + // but we still inflate it enough to avoid the case where EPA misses the collision. + max_separation_distance = min(max_separation_distance, 1.0f); + // Get the support function if (mShape1IncCvxRadius == nullptr) mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1); // Add convex radius - AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance); + AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, max_separation_distance); // Perform EPA step if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) @@ -101,14 +108,14 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, } // Check if the penetration is bigger than the early out fraction - float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance; + float penetration_depth = (point2 - point1).Length() - max_separation_distance; if (-penetration_depth >= mCollector.GetEarlyOutFraction()) return; // Correct point1 for the added separation distance float penetration_axis_len = penetration_axis.Length(); if (penetration_axis_len > 0.0f) - point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + point1 -= penetration_axis * (max_separation_distance / penetration_axis_len); // Check if we have enabled active edge detection if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) diff --git a/Jolt/Physics/Collision/Shape/ConvexShape.cpp b/Jolt/Physics/Collision/Shape/ConvexShape.cpp index 35e23ee01..e9fa343ab 100644 --- a/Jolt/Physics/Collision/Shape/ConvexShape.cpp +++ b/Jolt/Physics/Collision/Shape/ConvexShape.cpp @@ -57,8 +57,9 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2; // Get bounding boxes + float max_separation_distance = inCollideShapeSettings.mMaxSeparationDistance; AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1); - shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + shape1_bbox.ExpandBy(Vec3::sReplicate(max_separation_distance)); AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2); // Check if they overlap @@ -86,10 +87,10 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2); // Transform shape 2 in the space of shape 1 - TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius); + TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius); // Perform GJK step - status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + max_separation_distance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); } // Check result of collision detection @@ -105,16 +106,22 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS { // Need to run expensive EPA algorithm + // We know we're overlapping at this point, so we can set the max separation distance to 0. + // Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated. + // In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape, + // but we still inflate it enough to avoid the case where EPA misses the collision. + max_separation_distance = min(max_separation_distance, 1.0f); + // Create support function SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius; const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1); const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2); // Add separation distance - AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance); + AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, max_separation_distance); // Transform shape 2 in the space of shape 1 - TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius); + TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius); // Perform EPA step if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) @@ -124,14 +131,14 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS } // Check if the penetration is bigger than the early out fraction - float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance; + float penetration_depth = (point2 - point1).Length() - max_separation_distance; if (-penetration_depth >= ioCollector.GetEarlyOutFraction()) return; // Correct point1 for the added separation distance float penetration_axis_len = penetration_axis.Length(); if (penetration_axis_len > 0.0f) - point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + point1 -= penetration_axis * (max_separation_distance / penetration_axis_len); // Convert to world space point1 = inCenterOfMassTransform1 * point1; @@ -446,7 +453,7 @@ void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg in // Get the support function with convex radius SupportBuffer buffer; const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale); - AddConvexRadius add_convex(*support, support->GetConvexRadius()); + AddConvexRadius add_convex(*support, support->GetConvexRadius()); // Draw the shape DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); }); diff --git a/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/Jolt/Physics/Collision/Shape/TriangleShape.cpp index 5bfa05b91..e7514a77f 100644 --- a/Jolt/Physics/Collision/Shape/TriangleShape.cpp +++ b/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -413,6 +413,9 @@ void TriangleShape::sRegister() { CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle); CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle); + + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape); } // Specialized collision functions diff --git a/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/Jolt/Physics/SoftBody/SoftBodyShape.cpp index 6692b223e..890a0baac 100644 --- a/Jolt/Physics/SoftBody/SoftBodyShape.cpp +++ b/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -328,6 +328,9 @@ void SoftBodyShape::sRegister() { CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody); CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody); + + CollisionDispatch::sRegisterCollideShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCollideShape); + CollisionDispatch::sRegisterCastShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCastShape); } // Specialized collision functions diff --git a/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp b/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp index a5eeab29e..3ca38c3d8 100644 --- a/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp +++ b/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp @@ -141,7 +141,7 @@ void ConvexHullShrinkTest::PrePhysicsUpdate(const PreUpdateParams &inParams) // Get the support function of the shape excluding convex radius and add the convex radius ConvexShape::SupportBuffer buffer; const ConvexShape::Support *support = shape->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); - AddConvexRadius add_cvx(*support, convex_radius); + AddConvexRadius add_cvx(*support, convex_radius); // Calculate the error w.r.t. the original hull float max_error = -FLT_MAX; diff --git a/Samples/Tests/ConvexCollision/EPATest.cpp b/Samples/Tests/ConvexCollision/EPATest.cpp index ddbfc3066..dfbb8d3b1 100644 --- a/Samples/Tests/ConvexCollision/EPATest.cpp +++ b/Samples/Tests/ConvexCollision/EPATest.cpp @@ -33,8 +33,8 @@ bool EPATest::CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphe DrawSphereSP(mDebugRenderer, inMatrix * inSphere.GetCenter(), inSphere.GetRadius(), Color::sGrey); // Transform the box and sphere according to inMatrix - TransformedConvexObject transformed_box(inMatrix, inBox); - TransformedConvexObject transformed_sphere(inMatrix, inSphere); + TransformedConvexObject transformed_box(inMatrix, inBox); + TransformedConvexObject transformed_sphere(inMatrix, inSphere); // Run the EPA algorithm EPAPenetrationDepth epa; diff --git a/Samples/Tests/ConvexCollision/InteractivePairsTest.cpp b/Samples/Tests/ConvexCollision/InteractivePairsTest.cpp index bb91a4ad6..af76d7255 100644 --- a/Samples/Tests/ConvexCollision/InteractivePairsTest.cpp +++ b/Samples/Tests/ConvexCollision/InteractivePairsTest.cpp @@ -110,10 +110,10 @@ void InteractivePairsTest::PrePhysicsUpdate(const PreUpdateParams &inParams) void InteractivePairsTest::TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotationA, float inConvexRadiusA, const AABox &inA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB) { Mat44 mat_a = Mat44::sTranslation(inTranslationA) * Mat44::sRotationX(inRotationA.GetX()) * Mat44::sRotationY(inRotationA.GetY()) * Mat44::sRotationZ(inRotationA.GetZ()); - TransformedConvexObject a(mat_a, inA); + TransformedConvexObject a(mat_a, inA); Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ()); - TransformedConvexObject b(mat_b, inB); + TransformedConvexObject b(mat_b, inB); EPAPenetrationDepth pen_depth; Vec3 v = Vec3::sAxisX(), pa, pb; @@ -130,8 +130,8 @@ void InteractivePairsTest::TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotati if (inConvexRadiusA > 0.0f) DrawWireBoxSP(mDebugRenderer, mat_a, widened_a, Color::sWhite); - AddConvexRadius> a_inc(a, inConvexRadiusA); - AddConvexRadius> b_inc(b, inConvexRadiusB); + AddConvexRadius a_inc(a, inConvexRadiusA); + AddConvexRadius b_inc(b, inConvexRadiusB); if (pen_depth.GetPenetrationDepth(a, a_inc, inConvexRadiusA, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb)) { @@ -154,7 +154,7 @@ void InteractivePairsTest::TestSphereVsBox(Vec3Arg inTranslationA, float inRadiu { Sphere s(inTranslationA, inRadiusA); Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ()); - TransformedConvexObject b(mat_b, inB); + TransformedConvexObject b(mat_b, inB); AABox widened_b = inB; widened_b.ExpandBy(Vec3::sReplicate(inConvexRadiusB)); @@ -164,7 +164,7 @@ void InteractivePairsTest::TestSphereVsBox(Vec3Arg inTranslationA, float inRadiu DrawSphereSP(mDebugRenderer, inTranslationA, inRadiusA, Color::sWhite); - AddConvexRadius> b_inc(b, inConvexRadiusB); + AddConvexRadius b_inc(b, inConvexRadiusB); if (pen_depth.GetPenetrationDepth(s, s, 0.0f, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb)) { diff --git a/UnitTests/Geometry/EPATests.cpp b/UnitTests/Geometry/EPATests.cpp index db966e3d7..b70ec47c0 100644 --- a/UnitTests/Geometry/EPATests.cpp +++ b/UnitTests/Geometry/EPATests.cpp @@ -27,8 +27,8 @@ TEST_SUITE("EPATests") /// @return If a collision was detected static bool CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere) { - TransformedConvexObject transformed_box(inMatrix, inBox); - TransformedConvexObject transformed_sphere(inMatrix, inSphere); + TransformedConvexObject transformed_box(inMatrix, inBox); + TransformedConvexObject transformed_sphere(inMatrix, inSphere); // Use EPA algorithm. Don't use convex radius to avoid EPA being skipped because the inner hulls are not touching. EPAPenetrationDepth epa; diff --git a/UnitTests/Physics/CollideShapeTests.cpp b/UnitTests/Physics/CollideShapeTests.cpp index de0616714..6741ebe89 100644 --- a/UnitTests/Physics/CollideShapeTests.cpp +++ b/UnitTests/Physics/CollideShapeTests.cpp @@ -483,4 +483,57 @@ TEST_SUITE("CollideShapeTests") CHECK(angle >= 2.0f * JPH_PI); } + + // This test checks extreme values of the max separation distance and how it affects ConvexShape::sCollideConvexVsConvex + // See: https://github.com/jrouwe/JoltPhysics/discussions/1379 + TEST_CASE("TestBoxVsSphereLargeSeparationDistance") + { + constexpr float cRadius = 1.0f; + constexpr float cHalfExtent = 10.0f; + RefConst sphere_shape = new SphereShape(cRadius); + RefConst box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent)); + float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f }; + for (float x : distances) + for (float max_separation : distances) + { + CollideShapeSettings collide_settings; + collide_settings.mMaxSeparationDistance = max_separation; + ClosestHitCollisionCollector collector; + CollisionDispatch::sCollideShapeVsShape(box_shape, sphere_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector); + + float expected_penetration = cHalfExtent - (x - cRadius); + if (collector.HadHit()) + CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f); + else + CHECK(expected_penetration < -max_separation); + } + } + + // This test case checks extreme values of the max separation distance and how it affects CollideConvexVsTriangles::Collide + // See: https://github.com/jrouwe/JoltPhysics/discussions/1379 + TEST_CASE("TestTriangleVsBoxLargeSeparationDistance") + { + constexpr float cTriangleX = -0.1f; + constexpr float cHalfExtent = 10.0f; + RefConst triangle_shape = new TriangleShape(Vec3(cTriangleX, -10, 10), Vec3(cTriangleX, -10, -10), Vec3(cTriangleX, 10, 0)); + RefConst box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent)); + float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f }; + for (float x : distances) + for (float max_separation : distances) + { + CollideShapeSettings collide_settings; + collide_settings.mMaxSeparationDistance = max_separation; + ClosestHitCollisionCollector collector; + CollisionDispatch::sCollideShapeVsShape(triangle_shape, box_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector); + + float expected_penetration = cTriangleX - (x - cHalfExtent); + if (collector.HadHit()) + CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f); + else + { + CHECK(expected_penetration < -max_separation); + CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Vec3::sAxisX(), 1.0e-5f); + } + } + } }