fix: Refactor bepu contact management, support compound shape child index lookup and provide Impact Force evaluation to users

samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs

@@ -41,17 +41,17 @@ public override void Update()
         }
     }
 
-    public class MyCustomContactEventHandler : IContactEventHandler
+    public class MyCustomContactEventHandler : IContactHandler
     {
         public bool Contact { get; private set; } = false;
         public bool NoContactResponse => false;
 
-        void IContactEventHandler.OnStartedTouching<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation)
+        void IContactHandler.OnStartedTouching<TManifold>(Contacts<TManifold> contacts)
         {
             Contact = true;
         }
 
-        void IContactEventHandler.OnStoppedTouching<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation)
+        void IContactHandler.OnStoppedTouching<TManifold>(Contacts<TManifold> contacts)
         {
             Contact = false;
         }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs

@@ -4,6 +4,7 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using BepuPhysics.Collidables;
 using BepuPhysics.CollisionDetection;
 using Stride.BepuPhysics.Constraints;
 using Stride.BepuPhysics.Definitions;
@@ -156,6 +157,44 @@ public static void OnContactRemovalTest()
             RunGameTest(game);
         }
 
+        [Fact]
+        public static void OnContactRollTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                int contactStarted = 0, contactStopped = 0, passedGoal = 0;
+                var killTrigger = new ContactEvents { NoContactResponse = true };
+                var contacts = new ContactEvents { NoContactResponse = false };
+                var sphere = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new SphereCollider() } } } };
+                var slope = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(2, 0.1f, 2) } } }, ContactEventHandler = contacts } };
+                var goal = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(10, 0.1f, 10) } } }, ContactEventHandler = killTrigger } };
+                contacts.StartedTouching += (_, _) => contactStarted++;
+                contacts.StoppedTouching += (_, _) => contactStopped++;
+                killTrigger.StoppedTouching += (_, _) => passedGoal++;
+
+                sphere.Transform.Position.Y = 3;
+                slope.Transform.Rotation = Quaternion.RotationZ(10);
+                goal.Transform.Position.Y = -10;
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { sphere, slope, goal });
+
+                var simulation = sphere.GetSimulation();
+
+                while (passedGoal == 0)
+                    await simulation.AfterUpdate();
+
+                Assert.Equal(1, contactStarted);
+
+                Assert.Equal(contactStarted, contactStopped);
+
+                game.Exit();
+            });
+            RunGameTest(game);
+        }
+
         [Fact]
         public static void OnTriggerRemovalTest()
         {
@@ -164,42 +203,90 @@ public static void OnTriggerRemovalTest()
             {
                 game.ScreenShotAutomationEnabled = false;
 
-                int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0;
-                var trigger = new Trigger();
+                int startedTouching = 0, stoppedTouching = 0;
+                var trigger = new ContactEvents { NoContactResponse = true };
                 var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } };
                 var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } };
-                trigger.PairCreated += () => pairCreated++;
-                trigger.PairEnded += () => pairEnded++;
-                trigger.ContactAdded += () => contactAdded++;
-                trigger.ContactRemoved += () => contactRemoved++;
-                trigger.StartedTouching += () => startedTouching++;
-                trigger.StoppedTouching += () => stoppedTouching++;
+                trigger.StartedTouching += (_, _) => startedTouching++;
+                trigger.StoppedTouching += (_, _) => stoppedTouching++;
 
                 // Remove the component as soon as it enters the trigger to test if the system handles that case properly
-                trigger.PairCreated += () => e1.Scene = null;
+                trigger.StartedTouching += (_, _) => e1.Scene = null;
 
                 e1.Transform.Position.Y = 3;
 
                 game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
 
                 var simulation = e1.GetSimulation();
 
-                while (pairEnded == 0)
+                while (stoppedTouching == 0)
                     await simulation.AfterUpdate();
 
-                Assert.Equal(1, pairCreated);
-                Assert.Equal(0, contactAdded);
-                Assert.Equal(0, startedTouching);
+                Assert.Equal(1, startedTouching);
 
-                Assert.Equal(pairCreated, pairEnded);
-                Assert.Equal(contactAdded, contactRemoved);
                 Assert.Equal(startedTouching, stoppedTouching);
 
                 game.Exit();
             });
             RunGameTest(game);
         }
 
+        [Fact]
+        public void ContactImpulseTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                var contactE = new ContactSampleForces();
+                var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = contactE } };
+                var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } };
+
+                var source = e1.Get<BodyComponent>()!;
+                source.ContinuousDetectionMode = ContinuousDetectionMode.Continuous;
+                e1.Transform.Position.Y = 3;
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
+                source.LinearVelocity = new Vector3(0, -100, 0);
+
+                var simulation = e1.GetSimulation();
+
+                while (contactE.Exit == false)
+                    await simulation.AfterUpdate();
+
+                Assert.NotEmpty(contactE.ImpactForces.Where(x => x.Length() > 100));
+
+                game.Exit();
+            });
+            RunGameTest(game);
+        }
+
+        private class ContactSampleForces : IContactHandler
+        {
+            public bool NoContactResponse => false;
+
+            public List<Vector3> ImpactForces = new();
+            public bool Exit;
+
+            public void OnStartedTouching<TManifold>(Contacts<TManifold> contacts) where TManifold : unmanaged, IContactManifold<TManifold>
+            {
+                foreach (var contact in contacts)
+                {
+                    ImpactForces.Add(contacts.ComputeImpactForce(contact));
+                }
+            }
+
+            public void OnTouching<TManifold>(Contacts<TManifold> manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+            {
+            }
+
+            public void OnStoppedTouching<TManifold>(Contacts<TManifold> manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+            {
+                Exit = true;
+            }
+        }
+
         [Fact]
         public static void OnTriggerTest()
         {
@@ -208,32 +295,24 @@ public static void OnTriggerTest()
             {
                 game.ScreenShotAutomationEnabled = false;
 
-                int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0;
-                var trigger = new Trigger();
+                int startedTouching = 0, stoppedTouching = 0;
+                var trigger = new ContactEvents { NoContactResponse = true };
                 var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } };
                 var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } };
-                trigger.PairCreated += () => pairCreated++;
-                trigger.PairEnded += () => pairEnded++;
-                trigger.ContactAdded += () => contactAdded++;
-                trigger.ContactRemoved += () => contactRemoved++;
-                trigger.StartedTouching += () => startedTouching++;
-                trigger.StoppedTouching += () => stoppedTouching++;
+                trigger.StartedTouching += (_, _) => startedTouching++;
+                trigger.StoppedTouching += (_, _) => stoppedTouching++;
 
                 e1.Transform.Position.Y = 3;
 
                 game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
 
                 var simulation = e1.GetSimulation();
 
-                while (pairEnded == 0)
+                while (stoppedTouching == 0)
                     await simulation.AfterUpdate();
 
-                Assert.Equal(1, pairCreated);
-                Assert.NotEqual(0, contactAdded);
                 Assert.Equal(1, startedTouching);
 
-                Assert.Equal(pairCreated, pairEnded);
-                Assert.Equal(contactAdded, contactRemoved);
                 Assert.Equal(startedTouching, stoppedTouching);
 
                 game.Exit();
@@ -281,40 +360,25 @@ int TestRemovalUnsafe(BepuSimulation simulation)
             }
         }
 
-        private class Trigger : IContactEventHandler
+        private class ContactEvents : IContactHandler
         {
-            public bool NoContactResponse => true;
-
-            public event Action? ContactAdded, ContactRemoved, StartedTouching, StoppedTouching, PairCreated, PairEnded;
+            public required bool NoContactResponse { get; init; }
 
-            public void OnStartedTouching<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                StartedTouching?.Invoke();
-            }
-
-            public void OnStoppedTouching<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                StoppedTouching?.Invoke();
-            }
-
-            public void OnContactAdded<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                ContactAdded?.Invoke();
-            }
+            public event Action<CollidableComponent, CollidableComponent>? StartedTouching, Touching, StoppedTouching;
 
-            public void OnContactRemoved<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
+            public void OnStartedTouching<TManifold>(Contacts<TManifold> manifold) where TManifold : unmanaged, IContactManifold<TManifold>
             {
-                ContactRemoved?.Invoke();
+                StartedTouching?.Invoke(manifold.EventSource, manifold.Other);
             }
 
-            public void OnPairCreated<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
+            public void OnTouching<TManifold>(Contacts<TManifold> manifold) where TManifold : unmanaged, IContactManifold<TManifold>
             {
-                PairCreated?.Invoke();
+                Touching?.Invoke(manifold.EventSource, manifold.Other);
             }
 
-            public void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation)
+            public void OnStoppedTouching<TManifold>(Contacts<TManifold> manifold) where TManifold : unmanaged, IContactManifold<TManifold>
             {
-                PairEnded?.Invoke();
+                StoppedTouching?.Invoke(manifold.EventSource, manifold.Other);
             }
         }
     }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs

@@ -54,9 +54,6 @@ public sealed class BepuSimulation : IDisposable
     internal List<BodyComponent?> Bodies { get; } = new();
     internal List<StaticComponent?> Statics { get; } = new();
 
-    /// <summary> Required when a component is removed from the simulation and must have its contacts flushed </summary>
-    internal (int value, CollidableComponent? component) TemporaryDetachedLookup { get; set; }
-
     /// <inheritdoc cref="Stride.BepuPhysics.Definitions.CollisionMatrix"/>
     [DataMemberIgnore]
     public CollisionMatrix CollisionMatrix = CollisionMatrix.All; // Keep this as a field, user need ref access for writes
@@ -322,19 +319,13 @@ public CollidableComponent GetComponent(CollidableReference collidable)
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public BodyComponent GetComponent(BodyHandle handle)
     {
-        if (TemporaryDetachedLookup.component is BodyComponent detachedBody && handle.Value == TemporaryDetachedLookup.value)
-            return detachedBody;
-
         var body = Bodies[handle.Value];
         Debug.Assert(body is not null, "Handle is invalid, Bepu's array indexing strategy might have changed under us");
         return body;
     }
 
     public StaticComponent GetComponent(StaticHandle handle)
     {
-        if (TemporaryDetachedLookup.component is StaticComponent detachedStatic && handle.Value == TemporaryDetachedLookup.value)
-            return detachedStatic;
-
         var statics = Statics[handle.Value];
         Debug.Assert(statics is not null, "Handle is invalid, Bepu's array indexing strategy might have changed under us");
         return statics;
@@ -726,6 +717,8 @@ internal void Update(TimeSpan elapsed)
 
             Elider.SimulationUpdate(_simulationUpdateComponents, this, simTimeStepInSec);
 
+            Dispatcher.ForBatched(Bodies.Count, new UpdatePreviousVelocities { Bodies = Bodies });
+
             Simulation.Timestep(simTimeStepInSec, _threadDispatcher); //perform physic simulation using SimulationFixedStep
             ContactEvents.Flush(); //Fire event handler stuff.
 
@@ -979,4 +972,22 @@ public void GetResult() { }
 
         public TickAwaiter GetAwaiter() => this;
     }
+
+    private readonly struct UpdatePreviousVelocities : Dispatcher.IBatchJob
+    {
+        public required List<BodyComponent?> Bodies { get; init; }
+
+        public void Process(int start, int endExclusive)
+        {
+            for (; start < endExclusive; start++)
+            {
+                var body = Bodies[start];
+                if (body is not null)
+                {
+                    body.PreviousAngularVelocity = body.AngularVelocity;
+                    body.PreviousLinearVelocity = body.LinearVelocity;
+                }
+            }
+        }
+    }
 }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs

@@ -220,6 +220,23 @@ public Vector3 AngularVelocity
         }
     }
 
+    /// <summary>
+    /// The translation velocity in unit per second during the previous physics tick
+    /// </summary>
+    [DataMemberIgnore]
+    public Vector3 PreviousLinearVelocity { get; internal set; }
+
+    /// <summary>
+    /// The rotation velocity in unit per second during the previous physics tick
+    /// </summary>
+    /// <remarks>
+    /// The rotation format is in axis-angle,
+    /// meaning that AngularVelocity.Normalized is the axis of rotation,
+    /// while AngularVelocity.Length is the amount of rotation around that axis in radians per second
+    /// </remarks>
+    [DataMemberIgnore]
+    public Vector3 PreviousAngularVelocity { get; internal set; }
+
     /// <summary>
     /// The position of this body in the physics scene, setting it will teleport this object to the position provided.
     /// </summary>
@@ -437,6 +454,8 @@ protected override void AttachInner(NRigidPose pose, BodyInertia shapeInertia, T
         }
         else
         {
+            LinearVelocity = AngularVelocity = default;
+
             var bHandle = Simulation.Simulation.Bodies.Add(bDescription);
             BodyReference = Simulation.Simulation.Bodies[bHandle];
             BodyReference.Value.Collidable.Continuity = ContinuousDetection;
@@ -488,14 +507,6 @@ protected override void DetachInner()
         BodyReference = null;
     }
 
-    protected override int GetHandleValue()
-    {
-        if (BodyReference is { } bRef)
-            return bRef.Handle.Value;
-
-        throw new InvalidOperationException();
-    }
-
     /// <summary>
     /// A special variant taking the center of mass into consideration
     /// </summary>