Manifold reduction extremity scale shouldn't redundantly include epsilon scale

BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs

@@ -188,7 +188,7 @@ private static void InternalReduce(ref ManifoldCandidate candidates, int maxCand
             //minor todo: don't really need to waste time initializing to an invalid value.
             var bestScore = new Vector<float>(-float.MaxValue);
             //While depth is the dominant heuristic, extremity is used as a bias to keep initial contact selection a little more consistent in near-equal cases.
-            var extremityScale = epsilonScale * 1e-2f;
+            const float extremityScale = 1e-2f;
             for (int i = 0; i < maxCandidateCount; ++i)
             {
                 ref var candidate = ref Unsafe.Add(ref candidates, i);

Demos/Demos/PerBodyGravityDemo.cs

@@ -17,7 +17,7 @@ namespace Demos.Demos;
 /// </summary>
 public class PerBodyGravityDemo : Demo
 {
-    struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks
+    internal struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks
     {
         /// <summary>
         /// Maps body handles to per-body gravity values.
@@ -29,8 +29,13 @@ struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks
         /// <summary>
         /// Used to look up body handles using the callback-provided body indices.
         /// </summary>
-        private Bodies bodies;
-
+        private Bodies bodies;
+
+        public PerBodyGravityDemoCallbacks(CollidableProperty<float> bodyGravities) : this()
+        {
+            BodyGravities = bodyGravities;
+        }
+
         public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
 
         public readonly bool AllowSubstepsForUnconstrainedBodies => false;
@@ -92,7 +97,7 @@ public override void Initialize(ContentArchive content, Camera camera)
 
         //The CollidableProperty is a helper that associates body handles to whatever data you'd like to store. You don't have to use it, but it's fairly convenient.
         var bodyGravities = new CollidableProperty<float>(BufferPool);
-        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PerBodyGravityDemoCallbacks() { BodyGravities = bodyGravities }, new SolveDescription(4, 1));
+        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PerBodyGravityDemoCallbacks(bodyGravities), new SolveDescription(4, 1));
 
         Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(1000, 10, 1000))));
 

Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs

@@ -0,0 +1,75 @@
+using System;
+using System.Linq;
+using System.Numerics;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoRenderer.UI;
+using Demos.Demos;
+using DemoUtilities;
+
+namespace Demos.SpecializedTests;
+
+/// <summary>
+/// Stress tests contact manifold reduction heuristics at a variety of scales using box-box tests.
+/// </summary>
+public class ManifoldReductionScaleTestDemo : Demo
+{
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 0.5f, -3);
+        camera.Yaw = MathF.PI;
+        camera.Pitch = -0.3f;
+
+        var bodyGravities = new CollidableProperty<float>(BufferPool);
+        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(10, 1), float.MaxValue, 0.01f),
+            new PerBodyGravityDemo.PerBodyGravityDemoCallbacks(bodyGravities), new SolveDescription(8, 1));
+
+        var scales = new[] { 0.001f, 0.01f, 0.1f, 1f, 10f, 100f, 1000f };
+        var offsets = new[] { new Vector3(-0.5f, 0, -0.5f), new Vector3(-0.25f, 0, -0.25f), new Vector3(-0.125f, 0, -0.125f), new Vector3(-0.5f, 0, 0), new Vector3(-0.25f, 0, 0), new Vector3(-0.125f, 0, 0), };
+
+        const int rotationSteps = 256;
+        const float maxRotationTop = MathF.PI * 2f;
+        const float maxRotationBottom = MathF.PI * 3f;
+
+        const float pairSpacing = 4f;
+
+        for (int scaleIndex = 0; scaleIndex < scales.Length; scaleIndex++)
+        {
+            var scale = scales[scaleIndex];
+            var scaleGroupY = scaleIndex * pairSpacing * scale;
+            var size = new Vector3(2f, 1f, 2f) * scale;
+            var box = new Box(size.X, size.Y, size.Z);
+            var shapeIndex = Simulation.Shapes.Add(box);
+            var boxInertia = box.ComputeInertia(1f);
+            for (int offsetIndex = 0; offsetIndex < offsets.Length; ++offsetIndex)
+            {
+                var offsetGroupZ = offsetIndex * pairSpacing * scale;
+                var offset = offsets[offsetIndex] * scale;
+
+                for (int rotationIndex = 0; rotationIndex < rotationSteps; rotationIndex++)
+                {
+                    var rotationAngleTop = (float)rotationIndex / (rotationSteps - 1) * maxRotationTop;
+                    var rotationAngleBottom = (float)rotationIndex / (rotationSteps - 1) * maxRotationBottom;
+                    var pairX = (rotationIndex - rotationSteps / 2) * pairSpacing * scale;
+
+                    Simulation.Statics.Add(new StaticDescription(
+                        new RigidPose(new Vector3(pairX, scaleGroupY, offsetGroupZ), Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleBottom)), shapeIndex));
+
+                    var bodyHandle = Simulation.Bodies.Add(BodyDescription.CreateDynamic(
+                        new RigidPose(new Vector3(pairX, size.Y + scaleGroupY, offsetGroupZ) + offset, Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleTop)),
+                        boxInertia, shapeIndex, -0.01f));
+                    bodyGravities.Allocate(bodyHandle) = -10 * scale;
+                }
+            }
+        }
+
+        var groundBox = new Box(100000, 0.1f, 100000);
+        Simulation.Statics.Add(new StaticDescription(
+            new RigidPose(new Vector3(0, -1, 0), Quaternion.Identity),
+            Simulation.Shapes.Add(groundBox)));
+    }
+}