fix: bepu, removal of collidables while running a contact event leaves contact tracking in an unexpected state

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

@@ -4,9 +4,11 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using BepuPhysics.CollisionDetection;
 using Stride.BepuPhysics.Constraints;
 using Stride.BepuPhysics.Definitions;
 using Stride.BepuPhysics.Definitions.Colliders;
+using Stride.BepuPhysics.Definitions.Contacts;
 using Stride.Core.Mathematics;
 using Xunit;
 using Stride.Engine;
@@ -132,12 +134,12 @@ public static void OnContactRemovalTest()
                 var c1 = new CharacterComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } };
                 var c2 = new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } };
 
-                var e1 = new Entity{ c1 };
-                var e2 = new Entity{ c2 };
+                var e1 = new Entity { c1 };
+                var e2 = new Entity { c2 };
 
                 e1.Transform.Position.Y = 3;
 
-                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new []{ e1, e2 });
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
 
                 var simulation = e1.GetSimulation();
 
@@ -154,6 +156,91 @@ public static void OnContactRemovalTest()
             RunGameTest(game);
         }
 
+        [Fact]
+        public static void OnTriggerRemovalTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0;
+                var trigger = new Trigger();
+                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++;
+
+                // Remove the component as soon as it enters the trigger to test if the system handles that case properly
+                trigger.PairCreated += () => e1.Scene = null;
+
+                e1.Transform.Position.Y = 3;
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
+
+                var simulation = e1.GetSimulation();
+
+                while (pairEnded == 0)
+                    await simulation.AfterUpdate();
+
+                Assert.Equal(1, pairCreated);
+                Assert.Equal(0, contactAdded);
+                Assert.Equal(0, startedTouching);
+
+                Assert.Equal(pairCreated, pairEnded);
+                Assert.Equal(contactAdded, contactRemoved);
+                Assert.Equal(startedTouching, stoppedTouching);
+
+                game.Exit();
+            });
+            RunGameTest(game);
+        }
+
+        [Fact]
+        public static void OnTriggerTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0;
+                var trigger = new Trigger();
+                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++;
+
+                e1.Transform.Position.Y = 3;
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 });
+
+                var simulation = e1.GetSimulation();
+
+                while (pairEnded == 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();
+            });
+            RunGameTest(game);
+        }
+
         [Fact]
         public static void OnRaycastRemovalTest()
         {
@@ -165,8 +252,8 @@ public static void OnRaycastRemovalTest()
                 var c1 = new CharacterComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } };
                 var c2 = new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } };
 
-                var e1 = new Entity{ c1 };
-                var e2 = new Entity{ c2 };
+                var e1 = new Entity { c1 };
+                var e2 = new Entity { c2 };
 
                 e1.Transform.Position.Y = 3;
 
@@ -193,5 +280,42 @@ int TestRemovalUnsafe(BepuSimulation simulation)
                 return hits.Span.Length;
             }
         }
+
+        private class Trigger : IContactEventHandler
+        {
+            public bool NoContactResponse => true;
+
+            public event Action? ContactAdded, ContactRemoved, StartedTouching, StoppedTouching, PairCreated, PairEnded;
+
+            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 void OnContactRemoved<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
+            {
+                ContactRemoved?.Invoke();
+            }
+
+            public void OnPairCreated<TManifold>(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold<TManifold>
+            {
+                PairCreated?.Invoke();
+            }
+
+            public void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation)
+            {
+                PairEnded?.Invoke();
+            }
+        }
     }
 }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/Stride.BepuPhysics.Tests.csproj

@@ -10,6 +10,7 @@
     <StrideGraphicsApiDependent>true</StrideGraphicsApiDependent>
     <!-- Force msbuild to check to rebuild this assembly instead of letting VS IDE guess -->
     <DisableFastUpToDateCheck>true</DisableFastUpToDateCheck>
+    <Nullable>enable</Nullable>
   </PropertyGroup>
   <PropertyGroup>
     <StartupObject>xunit.runner.stride.Program</StartupObject>

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

@@ -292,7 +292,7 @@ public BepuSimulation()
         #warning Consider wrapping stride's threadpool/dispatcher into an IThreadDispatcher and passing that over to bepu instead of using their dispatcher
         _threadDispatcher = new ThreadDispatcher(targetThreadCount);
         BufferPool = new BufferPool();
-        ContactEvents = new ContactEventsManager(_threadDispatcher, BufferPool);
+        ContactEvents = new ContactEventsManager(BufferPool, this);
 
         var strideNarrowPhaseCallbacks = new StrideNarrowPhaseCallbacks(this, ContactEvents, CollidableMaterials);
         var stridePoseIntegratorCallbacks = new StridePoseIntegratorCallbacks(CollidableMaterials);
@@ -303,7 +303,7 @@ public BepuSimulation()
         Simulation.Solver.SubstepCount = 1;
 
         CollidableMaterials.Initialize(Simulation);
-        ContactEvents.Initialize(this);
+        ContactEvents.Initialize();
         //CollisionBatcher = new CollisionBatcher<BatcherCallbacks>(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, DefaultBatcherCallbacks);
     }
 

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

@@ -94,7 +94,7 @@ public InterpolationMode InterpolationMode
     }
 
     /// <summary>
-    /// Whether the object's path or only its destination is checked for collision when moving, prevents objects from passing through each other at higher speed 
+    /// Whether the object's path or only its destination is checked for collision when moving, prevents objects from passing through each other at higher speed
     /// </summary>
     /// <remarks>
     /// This property is a shortcut to the <see cref="ContinuousDetection"/>.<see cref="ContinuousDetection.Mode"/> property
@@ -139,7 +139,7 @@ public float SleepThreshold
             }
         }
     }
-    
+
     /// <summary>
     /// The number of time steps that the body must be under the sleep threshold before the body becomes a sleeping candidate.
     /// Note that the body is not guaranteed to go to sleep immediately after meeting this minimum.
@@ -164,7 +164,7 @@ public byte MinimumTimestepCountUnderThreshold
     }
 
     /// <summary>
-    /// Whether the body is being actively simulated. 
+    /// Whether the body is being actively simulated.
     /// Setting this to true will attempt to wake the body; setting it to false will force the body and any constraint-connected bodies asleep.
     /// </summary>
     [DataMemberIgnore]
@@ -196,8 +196,8 @@ public Vector3 LinearVelocity
     /// The rotation velocity in unit per second
     /// </summary>
     /// <remarks>
-    /// The rotation format is in axis-angle, 
-    /// meaning that AngularVelocity.Normalized is the axis of rotation, 
+    /// 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]
@@ -303,6 +303,16 @@ public ContinuousDetection ContinuousDetection
     /// </summary>
     public IReadOnlyList<ConstraintComponentBase> Constraints => BoundConstraints ?? (IReadOnlyList<ConstraintComponentBase>)Array.Empty<ConstraintComponentBase>();
 
+    protected internal override CollidableReference? CollidableReference
+    {
+        get
+        {
+            if (BodyReference is { } bRef)
+                return bRef.CollidableReference;
+            return null;
+        }
+    }
+
     /// <summary>
     /// Applies an explosive force at a specific offset off of this body which will affect both its angular and linear velocity
     /// </summary>
@@ -418,25 +428,6 @@ protected override int GetHandleValue()
         throw new InvalidOperationException();
     }
 
-    protected override void RegisterContactHandler()
-    {
-        if (ContactEventHandler is not null && Simulation is not null && BodyReference is { } bRef)
-            Simulation.ContactEvents.Register(bRef.Handle, ContactEventHandler);
-    }
-
-    protected override void UnregisterContactHandler()
-    {
-        if (Simulation is not null && BodyReference is { } bRef)
-            Simulation.ContactEvents.Unregister(bRef.Handle);
-    }
-
-    protected override bool IsContactHandlerRegistered()
-    {
-        if (Simulation is not null && BodyReference is { } bRef)
-            return Simulation.ContactEvents.IsListener(bRef.Handle);
-        return false;
-    }
-
     /// <summary>
     /// A special variant taking the center of mass into consideration
     /// </summary>

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs

@@ -22,6 +22,7 @@ namespace Stride.BepuPhysics;
 [DefaultEntityComponentProcessor(typeof(CollidableProcessor), ExecutionMode = ExecutionMode.Runtime)]
 public abstract class CollidableComponent : EntityComponent
 {
+    private static uint IdCounter;
     private static uint VersioningCounter;
 
     private float _springFrequency = 30;
@@ -47,6 +48,8 @@ public abstract class CollidableComponent : EntityComponent
     [DataMemberIgnore]
     internal uint Versioning { get; private set; }
 
+    internal uint InstanceIndex { get; } = Interlocked.Increment(ref IdCounter);
+
     /// <summary>
     /// The simulation this object belongs to, null when it is not part of a simulation.
     /// </summary>
@@ -84,9 +87,9 @@ public required ICollider Collider
     /// The bounce frequency in hz
     /// </summary>
     /// <remarks>
-    /// Must be low enough that the simulation can actually represent it. 
-    /// If the contact is trying to make a bounce happen at 240hz, 
-    /// but the integrator timestep is only 60hz, 
+    /// Must be low enough that the simulation can actually represent it.
+    /// If the contact is trying to make a bounce happen at 240hz,
+    /// but the integrator timestep is only 60hz,
     /// the unrepresentable motion will get damped out and the body won't bounce as much.
     /// </remarks>
     public float SpringFrequency
@@ -216,6 +219,8 @@ public IContactEventHandler? ContactEventHandler
     [DataMemberIgnore]
     public Vector3 CenterOfMass { get; private set; }
 
+    protected internal abstract CollidableReference? CollidableReference { get; }
+
     public CollidableComponent()
     {
         _collider = new CompoundCollider();
@@ -291,7 +296,7 @@ internal void Detach(bool reAttaching)
         if (reAttaching == false)
         {
             Simulation.TemporaryDetachedLookup = (getHandleValue, this);
-            Simulation.ContactEvents.Flush(); // Ensure that removing this collidable sends the appropriate contact events to listeners
+            Simulation.ContactEvents.ClearCollisionsOf(this); // Ensure that removing this collidable sends the appropriate contact events to listeners
             Simulation.TemporaryDetachedLookup = (-1, null);
         }
 
@@ -338,7 +343,21 @@ protected void TryUpdateMaterialProperties()
 
     protected abstract int GetHandleValue();
 
-    protected abstract void RegisterContactHandler();
-    protected abstract void UnregisterContactHandler();
-    protected abstract bool IsContactHandlerRegistered();
+
+    protected void RegisterContactHandler()
+    {
+        if (ContactEventHandler is not null && Simulation is not null)
+            Simulation.ContactEvents.Register(this);
+    }
+
+    protected void UnregisterContactHandler()
+    {
+        if (Simulation is not null)
+            Simulation.ContactEvents.Unregister(this);
+    }
+
+    protected bool IsContactHandlerRegistered()
+    {
+        return Simulation is not null && Simulation.ContactEvents.IsRegistered(this);
+    }
 }