[bug] CapsuleTriangleTester wrong result when capsule axis is closely aligned to triangle plane normal

[nick-dodonov]

It gives strange impulses sometimes - the capsule with a constraint (or an inertia tensor disallowing to rotate) cannot stop jumping. Test to show the bug:

        [Test] //TODO: FIX on 0.000001f offset
        public void TestBepuCapsuleTriangle()
        {
            static Vector<T> Val<T>(T value) where T : struct
            {
                var values = new T[Vector<T>.Count];
                values[0] = value;
                return new Vector<T>(values);
            }

            static void CheckWithPointOffset(float yOffset)
            {
                var capsule = new CapsuleWide {HalfLength = Val(0.5f), Radius = Val(0.5f)};
                var triangle = new TriangleWide {
                    A = new Vector3Wide {X = Val(10f), Y = Val(0f), Z = Val(-5f)},
                    B = new Vector3Wide {X = Val(10f), Y = Val(0f), Z = Val(5f)},
                    C = new Vector3Wide {X = Val(-10f), Y = Val(yOffset), Z = Val(5f)}
                };
                var speculativeMargin = Val(0.1f);
                var offsetTriangle = new Vector3Wide {X = Val(-1f), Y = Val(-0.9f), Z = Val(-1f)};
                var orientationCapsule = new QuaternionWide {W = Val(1f)};
                var orientationTriangle = new QuaternionWide {W = Val(1f)};

                var tester = new CapsuleTriangleTester();
                tester.Test(ref capsule, ref triangle, ref speculativeMargin, ref offsetTriangle, ref orientationCapsule, ref orientationTriangle, 1, out var manifold);

                var depth = manifold.Depth0[0];
                Console.WriteLine($"CapsuleTriangleTester({yOffset}): depth: {depth}{(Mathf.Abs(depth - 0.1f) > 0.01 ? " - ASSERT!": "")}");
                Assert.Less(Mathf.Abs(depth - 0.1f), 1e-1f);
            }

            CheckWithPointOffset(0.0f);
            CheckWithPointOffset(0.01f);
            CheckWithPointOffset(0.001f);
            CheckWithPointOffset(0.0001f);
            CheckWithPointOffset(0.00001f);
            CheckWithPointOffset(0.000001f);
            CheckWithPointOffset(0.0000001f);
            CheckWithPointOffset(0.00000001f);
        }

Test fails with this results:

CapsuleTriangleTester(0): depth: 0.100000024
CapsuleTriangleTester(0.01): depth: 0.10446352
CapsuleTriangleTester(0.001): depth: 0.10064891
CapsuleTriangleTester(0.0001): depth: 0.094688326
CapsuleTriangleTester(1E-05): depth: 0.14237213 - ASSERT!
CapsuleTriangleTester(1E-06): depth: 0.5 - ASSERT!
CapsuleTriangleTester(1E-07): depth: 0.10000008
CapsuleTriangleTester(1E-08): depth: 0.100000024

[RossNordby]

Reproduced; looks like a numerical issue during final contact depth calculation. Looking into it.

[RossNordby]

214a2947e362c942d6e2920673a6c990d7e7287d should fix it. Thanks for the report!

[nick-dodonov]

Thanks! I've just checked in complex test (dynamics on a mesh) - infinite jumps have been gone.