diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index fcbd5cf32f..7acfd0033c 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/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(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + { + StartedTouching?.Invoke(); + } + + public void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + { + StoppedTouching?.Invoke(); + } + + public void OnContactAdded(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + { + ContactAdded?.Invoke(); + } + + public void OnContactRemoved(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + { + ContactRemoved?.Invoke(); + } + + public void OnPairCreated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + { + PairCreated?.Invoke(); + } + + public void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) + { + PairEnded?.Invoke(); + } + } } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/Stride.BepuPhysics.Tests.csproj b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/Stride.BepuPhysics.Tests.csproj index 11b308683f..bb8b02b238 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/Stride.BepuPhysics.Tests.csproj +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/Stride.BepuPhysics.Tests.csproj @@ -10,6 +10,7 @@ true true + enable xunit.runner.stride.Program diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs index b8ccba4b24..624891de3a 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs +++ b/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(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, DefaultBatcherCallbacks); } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs index 706b31b316..191a755a63 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs @@ -94,7 +94,7 @@ public InterpolationMode InterpolationMode } /// - /// 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 /// /// /// This property is a shortcut to the . property @@ -139,7 +139,7 @@ public float SleepThreshold } } } - + /// /// 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 } /// - /// 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. /// [DataMemberIgnore] @@ -196,8 +196,8 @@ public Vector3 LinearVelocity /// The rotation velocity in unit per second /// /// - /// 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 /// [DataMemberIgnore] @@ -303,6 +303,16 @@ public ContinuousDetection ContinuousDetection /// public IReadOnlyList Constraints => BoundConstraints ?? (IReadOnlyList)Array.Empty(); + protected internal override CollidableReference? CollidableReference + { + get + { + if (BodyReference is { } bRef) + return bRef.CollidableReference; + return null; + } + } + /// /// Applies an explosive force at a specific offset off of this body which will affect both its angular and linear velocity /// @@ -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; - } - /// /// A special variant taking the center of mass into consideration /// diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs index ad25d766e6..706a20845e 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs +++ b/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); + /// /// The simulation this object belongs to, null when it is not part of a simulation. /// @@ -84,9 +87,9 @@ public required ICollider Collider /// The bounce frequency in hz /// /// - /// 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. /// 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); + } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs index b313ec3239..cc629a2f18 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -2,9 +2,7 @@ // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. using System.Numerics; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuUtilities; @@ -18,470 +16,459 @@ namespace Stride.BepuPhysics.Definitions.Contacts; /// internal class ContactEventsManager : IDisposable { - //To know what events to emit, we have to track the previous state of a collision. We don't need to keep around old positions/offets/normals/depths, so it's quite a bit lighter. - [StructLayout(LayoutKind.Sequential)] - struct PreviousCollision + private readonly Dictionary _trackedCollisions = new(); + private readonly HashSet _outdatedPairs = new(); + private readonly BufferPool _pool; + private readonly BepuSimulation _simulation; + private IndexSet _staticListenerFlags; + private IndexSet _bodyListenerFlags; + + public ContactEventsManager(BufferPool pool, BepuSimulation simulation) { - public CollidableReference Collidable; - public bool Fresh; - public bool WasTouching; - public int ContactCount; - //FeatureIds are identifiers encoding what features on the involved shapes contributed to the contact. We store up to 4 feature ids, one for each potential contact. - //A "feature" is things like a face, vertex, or edge. There is no single interpretation for what a feature is- the mapping is defined on a per collision pair level. - //In this demo, we only care to check whether a given contact in the current frame maps onto a contact from a previous frame. - //We can use this to only emit 'contact added' events when a new contact with an unrecognized id is reported. - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - } - - BepuSimulation bepuSimulation; - Simulation simulation; - IThreadDispatcher? threadDispatcher; - BufferPool? pool; - - //We'll use a handle->index mapping in a CollidableProperty to point at our contiguously stored listeners (in the later listeners array). - //Note that there's also IndexSets for the statics and bodies; those will be checked first before accessing the listenerIndices. - //The CollidableProperty is quite barebones- it doesn't try to stop all invalid accesses, and the backing memory isn't guaranteed to be zero initialized. - //IndexSets are tightly bitpacked and are cheap to access, so they're an easy way to check if a collidable can trigger an event before doing any further processing. - CollidableProperty listenerIndices; - IndexSet staticListenerFlags; - IndexSet bodyListenerFlags; - int listenerCount; - - //For the purpose of this demo, we'll use some regular ol' interfaces rather than using the struct-implementing-interface for specialization. - //This array will be GC tracked as a result, but that should be mostly fine. If you've got hundreds of thousands of event handlers, you may want to consider alternatives. - struct Listener - { - public CollidableReference Source; - public IContactEventHandler Handler; - public QuickList PreviousCollisions; - } - Listener[] listeners; - - //The callbacks are invoked from a multithreaded context, and we don't know how many pairs will exist. - //Rather than attempting to synchronize all accesses, every worker thread spits out the results into a worker-local list to be processed later by the main thread flush. - struct PendingWorkerAdd - { - public int ListenerIndex; - public PreviousCollision Collision; + _pool = pool; + _simulation = simulation; } - QuickList[] pendingWorkerAdds; - /// - /// Creates a new contact events stream. - /// - /// Thread dispatcher to pull per-thread buffer pools from, if any. - /// Buffer pool used to manage resources internally. If null, the simulation's pool will be used. - /// Number of listeners to allocate space for initially. -#pragma warning disable CS8618 // Unassigned null fields, will be initialized through Initialize() below - public ContactEventsManager(IThreadDispatcher? threadDispatcher = null, BufferPool? pool = null, int initialListenerCapacity = 64) -#pragma warning restore CS8618 + public void Initialize() { - this.threadDispatcher = threadDispatcher; - this.pool = pool; - listeners = new Listener[initialListenerCapacity]; + _simulation.Simulation.Timestepper.BeforeCollisionDetection += TrackActivePairs; } - BufferPool? GetPoolForWorker(int workerIndex) - { - return threadDispatcher == null ? pool : threadDispatcher.WorkerPools[workerIndex]; - } - - /// - /// Initializes the contact events system with a simulation. - /// - /// Simulation to use with the contact events demo. - /// The constructor and initialization are split because of how this class is expected to be used. - /// It will be passed into a simulation's constructor as a part of its contact callbacks, so there is no simulation available at the time of construction. - public void Initialize(BepuSimulation simulation) + public void Dispose() { - this.bepuSimulation = simulation; - this.simulation = simulation.Simulation; - pool ??= simulation.BufferPool; - this.simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; - listenerIndices = new CollidableProperty(this.simulation, pool); - pendingWorkerAdds = new QuickList[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount]; + _simulation.Simulation.Timestepper.BeforeCollisionDetection -= TrackActivePairs; + if (_bodyListenerFlags.Flags.Allocated) + _bodyListenerFlags.Dispose(_pool); + if (_staticListenerFlags.Flags.Allocated) + _staticListenerFlags.Dispose(_pool); } /// /// Begins listening for events related to the given collidable. /// - /// Collidable to monitor for events. - /// Handlers to use for the collidable. - public void Register(CollidableReference collidable, IContactEventHandler handler) + public void Register(CollidableComponent collidable) { - if (collidable.Mobility == CollidableMobility.Static) - staticListenerFlags.Add(collidable.RawHandleValue, pool); + var reference = collidable.CollidableReference ?? throw new InvalidOperationException($"This Collidable's {nameof(CollidableReference)} should exist"); + if (reference.Mobility == CollidableMobility.Static) + _staticListenerFlags.Add(reference.RawHandleValue, _pool); else - bodyListenerFlags.Add(collidable.RawHandleValue, pool); - if (listenerCount >= listeners.Length) - { - Array.Resize(ref listeners, listeners.Length * 2); - } - //Note that allocations for the previous collision list are deferred until they actually exist. - listeners[listenerCount] = new Listener { Handler = handler, Source = collidable }; - listenerIndices[collidable] = listenerCount; - ++listenerCount; - } - - /// - /// Begins listening for events related to the given body. - /// - /// Body to monitor for events. - /// Handlers to use for the body. - public void Register(BodyHandle body, IContactEventHandler handler) - { - Register(simulation.Bodies[body].CollidableReference, handler); - } - - /// - /// Begins listening for events related to the given static. - /// - /// Static to monitor for events. - /// Handlers to use for the static. - public void Register(StaticHandle staticHandle, IContactEventHandler handler) - { - Register(new CollidableReference(staticHandle), handler); + _bodyListenerFlags.Add(reference.RawHandleValue, _pool); } /// /// Stops listening for events related to the given collidable. /// - /// Collidable to stop listening for. - public void Unregister(CollidableReference collidable) + public void Unregister(CollidableComponent collidable) { - if (collidable.Mobility == CollidableMobility.Static) - { - staticListenerFlags.Remove(collidable.RawHandleValue); - } + var reference = collidable.CollidableReference ?? throw new InvalidOperationException($"This Collidable's {nameof(CollidableReference)} should exist"); + if (reference.Mobility == CollidableMobility.Static) + _staticListenerFlags.Remove(reference.RawHandleValue); else - { - bodyListenerFlags.Remove(collidable.RawHandleValue); - } - var index = listenerIndices[collidable]; - --listenerCount; - ref var removedSlot = ref listeners[index]; - if (removedSlot.PreviousCollisions.Span.Allocated) - removedSlot.PreviousCollisions.Dispose(pool); - ref var lastSlot = ref listeners[listenerCount]; - if (index < listenerCount) - { - listenerIndices[lastSlot.Source] = index; - removedSlot = lastSlot; - } - lastSlot = default; - } + _bodyListenerFlags.Remove(reference.RawHandleValue); - /// - /// Stops listening for events related to the given body. - /// - /// Body to stop listening for. - public void Unregister(BodyHandle body) - { - Unregister(simulation.Bodies[body].CollidableReference); + ClearCollisionsOf(collidable); } /// - /// Stops listening for events related to the given static. + /// Checks if a collidable is registered as a listener. /// - /// Static to stop listening for. - public void Unregister(StaticHandle staticHandle) + public bool IsRegistered(CollidableComponent collidable) { - Unregister(new CollidableReference(staticHandle)); + if (collidable.CollidableReference is { } reference) + return IsRegistered(reference); + + return false; } /// /// Checks if a collidable is registered as a listener. /// - /// Collidable to check. - /// True if the collidable has been registered as a listener, false otherwise. - public bool IsListener(CollidableReference collidable) + private bool IsRegistered(CollidableReference reference) { - if (collidable.Mobility == CollidableMobility.Static) - { - return staticListenerFlags.Contains(collidable.RawHandleValue); - } + if (reference.Mobility == CollidableMobility.Static) + return _staticListenerFlags.Contains(reference.RawHandleValue); else - { - return bodyListenerFlags.Contains(collidable.RawHandleValue); - } + return _bodyListenerFlags.Contains(reference.RawHandleValue); } - /// - /// Checks if a collidable is registered as a listener. - /// - public bool IsListener(BodyHandle body) - { - return IsListener(simulation.Bodies[body].CollidableReference); - } - /// - /// Checks if a collidable is registered as a listener. - /// - public bool IsListener(StaticHandle staticHandle) + public void ClearCollisionsOf(CollidableComponent collidable) { - return IsListener(simulation.Statics[staticHandle].CollidableReference); - } - /// - /// Callback attached to the simulation's ITimestepper which executes just prior to collision detection to take a snapshot of activity states to determine which pairs we should expect updates in. - /// - void SetFreshnessForCurrentActivityStatus(float dt, IThreadDispatcher threadDispatcher) - { - //Every single pair tracked by the contact events has a 'freshness' flag. If the final flush sees a pair that is stale, it'll remove it - //and any necessary events to represent the end of that pair are reported. - //HandleManifoldForCollidable sets 'Fresh' to true for any processed pair, but pairs between sleeping or static bodies will not show up in HandleManifoldForCollidable since they're not active. - //We don't want Flush to report that sleeping pairs have stopped colliding, so we pre-initialize any such sleeping/static pair as 'fresh'. - - //This could be multithreaded reasonably easily if there are a ton of listeners or collisions, but that would be a pretty high bar. - //For simplicity, the demo will keep it single threaded. - var bodyHandleToLocation = simulation.Bodies.HandleToLocation; - for (int listenerIndex = 0; listenerIndex < listenerCount; ++listenerIndex) + // Really slow, but improving performance has a huge amount of gotchas since user code + // may cause this method to be re-entrant through handler calls. + // Something to investigate later + + var manifold = new EmptyManifold(); + foreach (var (pair, state) in _trackedCollisions) { - ref var listener = ref listeners[listenerIndex]; - var source = listener.Source; - //If it's a body, and it's in the active set (index 0), then every pair associated with the listener should expect updates. - var sourceExpectsUpdates = source.Mobility != CollidableMobility.Static && bodyHandleToLocation[source.BodyHandle.Value].SetIndex == 0; - if (sourceExpectsUpdates) - { - var previousCollisions = listeners[listenerIndex].PreviousCollisions; - for (int j = 0; j < previousCollisions.Count; ++j) - { - //Pair updates will set the 'freshness' to true when they happen, so that they won't be considered 'stale' in the flush and removed. - previousCollisions[j].Fresh = false; - } - } - else - { - //The listener is either static or sleeping. We should only expect updates if the other collidable is awake. - var previousCollisions = listeners[listenerIndex].PreviousCollisions; - for (int j = 0; j < previousCollisions.Count; ++j) - { - ref var previousCollision = ref previousCollisions[j]; - previousCollision.Fresh = previousCollision.Collidable.Mobility == CollidableMobility.Static || bodyHandleToLocation[previousCollision.Collidable.BodyHandle.Value].SetIndex > 0; - } - } + if (!ReferenceEquals(pair.A, collidable) && !ReferenceEquals(pair.B, collidable)) + continue; + + ClearCollision(pair, ref manifold, 0); } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void UpdatePreviousCollision(ref PreviousCollision collision, ref TManifold manifold, bool isTouching) where TManifold : unmanaged, IContactManifold + private unsafe void ClearCollision(OrderedPair pair, ref EmptyManifold manifold, int workerIndex) { - //If the above assert gets hit because of a change to nonconvex manifold capacities, the packed feature id representation this uses will need to be updated. - //I very much doubt the nonconvex manifold will ever use more than 8 contacts, so addressing this wouldn't require much of a change. - for (int j = 0; j < manifold.Count; ++j) - { - Unsafe.Add(ref collision.FeatureId0, j) = manifold.GetFeatureId(j); - } - collision.ContactCount = manifold.Count; - collision.Fresh = true; - collision.WasTouching = isTouching; + const bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one +#if DEBUG + ref var stateRef = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, pair, out _); + _trackedCollisions.Remove(pair, out var state); + System.Diagnostics.Debug.Assert(stateRef.Alive == false); // Notify HandleManifoldInner higher up the call stack that the manifold they are processing is dead +#else + _trackedCollisions.Remove(pair, out var state); +#endif + + for (int i = 0; i < state.ACount; i++) + state.HandlerA?.OnContactRemoved(pair.A, pair.B, ref manifold, flippedManifold, state.FeatureIdA[i], workerIndex, _simulation); + for (int i = 0; i < state.BCount; i++) + state.HandlerB?.OnContactRemoved(pair.B, pair.A, ref manifold, flippedManifold, state.FeatureIdB[i], workerIndex, _simulation); + + if (state.TryClear(Events.TouchingA)) + state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); + if (state.TryClear(Events.TouchingB)) + state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, workerIndex, _simulation); + + if (state.TryClear(Events.CreatedA)) + state.HandlerA?.OnPairEnded(pair.A, pair.B, _simulation); + if (state.TryClear(Events.CreatedB)) + state.HandlerB?.OnPairEnded(pair.B, pair.A, _simulation); + + _outdatedPairs.Remove(pair); } public void HandleManifold(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { - bool aListener = IsListener(pair.A); - bool bListener = IsListener(pair.B); + bool aListener = IsRegistered(pair.A); + bool bListener = IsRegistered(pair.B); if (aListener == false && bListener == false) return; - var collidableA = bepuSimulation.GetComponent(pair.A); - var collidableB = bepuSimulation.GetComponent(pair.B); - - if (aListener) - HandleManifoldInner(workerIndex, pair.A, pair.B, collidableA, collidableB, false, ref manifold); - if (bListener) - HandleManifoldInner(workerIndex, pair.B, pair.A, collidableB, collidableA, true, ref manifold); + HandleManifoldInner(workerIndex, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.B), ref manifold); } - void HandleManifoldInner(int workerIndex, CollidableReference source, CollidableReference other, CollidableComponent sourceCollidable, CollidableComponent otherCollidable, bool flippedManifold, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + private unsafe void HandleManifoldInner(int workerIndex, CollidableComponent a, CollidableComponent b, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { - var listenerIndex = listenerIndices[source]; - //This collidable is registered. Is the opposing collidable present? - ref var listener = ref listeners[listenerIndex]; + System.Diagnostics.Debug.Assert(manifold.Count <= LastCollisionState.FeatureCount, "This was built on the assumption that nonconvex manifolds will have a maximum of 4 contacts, but that might have changed."); + //If the above assert gets hit because of a change to nonconvex manifold capacities, the packed feature id representation this uses will need to be updated. + //I very much doubt the nonconvex manifold will ever use more than 8 contacts, so addressing this wouldn't require much of a change. - int previousCollisionIndex = -1; - bool isTouching = false; - for (int i = 0; i < listener.PreviousCollisions.Count; ++i) - { - ref var collision = ref listener.PreviousCollisions[i]; - //Since the 'Packed' field contains both the handle type (dynamic, kinematic, or static) and the handle index packed into a single bitfield, an equal value guarantees we are dealing with the same collidable. - if (collision.Collidable.Packed != other.Packed) - continue; + // We must first sort the collidables to ensure calls happen in a deterministic order, and to mimic `ClearCollision`'s order + var orderedPair = new OrderedPair(a, b); - previousCollisionIndex = i; - //This manifold is associated with an existing collision. - //For every contact in the old collsion still present (by feature id), set a flag in this bitmask so we can know when a contact is removed. - int previousContactsStillExist = 0; + bool aFlipped = ReferenceEquals(a, orderedPair.B); // Whether the manifold is flipped from a's point of view + bool bFlipped = !aFlipped; + + (a, b) = (orderedPair.A, orderedPair.B); + + IContactEventHandler? handlerA; + IContactEventHandler? handlerB; + ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, orderedPair, out bool alreadyExisted); + if (alreadyExisted) + { + handlerA = collisionState.HandlerA; + handlerB = collisionState.HandlerB; + bool touching = false; for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex) { - //We can check if each contact was already present in the previous frame by looking at contact feature ids. See the 'PreviousCollision' type for a little more info on FeatureIds. - var featureId = manifold.GetFeatureId(contactIndex); - var featureIdWasInPreviousCollision = false; - for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex) + if (manifold.GetDepth(contactIndex) < 0) + continue; + + touching = true; + if (handlerA is not null && collisionState.TrySet(Events.TouchingA)) { - if (featureId == Unsafe.Add(ref collision.FeatureId0, previousContactIndex)) - { - featureIdWasInPreviousCollision = true; - previousContactsStillExist |= 1 << previousContactIndex; - break; - } + handlerA.OnStartedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - if (!featureIdWasInPreviousCollision) + if (handlerB is not null && collisionState.TrySet(Events.TouchingB)) { - listener.Handler.OnContactAdded(sourceCollidable, otherCollidable, ref manifold, flippedManifold, contactIndex, workerIndex, bepuSimulation); + handlerB.OnStartedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - if (manifold.GetDepth(contactIndex) >= 0) - isTouching = true; + + handlerA?.OnTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + break; } - if (previousContactsStillExist != (1 << collision.ContactCount) - 1) + + if (touching == false && handlerA is not null && collisionState.TryClear(Events.TouchingA)) { - //At least one contact that used to exist no longer does. - for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex) - { - if ((previousContactsStillExist & 1 << previousContactIndex) == 0) - { - listener.Handler.OnContactRemoved(sourceCollidable, otherCollidable, ref manifold, flippedManifold, Unsafe.Add(ref collision.FeatureId0, previousContactIndex), workerIndex, bepuSimulation); - } - } + handlerA.OnStoppedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (touching == false && handlerB is not null && collisionState.TryClear(Events.TouchingB)) + { + handlerB.OnStoppedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - if (!collision.WasTouching && isTouching) + + uint toRemove = (1u << collisionState.ACount) - 1u; // Bitmask to mark contacts we have to change + uint toAdd = (1u << manifold.Count) - 1u; + + for (int i = 0; i < manifold.Count; ++i) // Check if any of our previous contact still exist { - listener.Handler.OnStartedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + int featureId = manifold.GetFeatureId(i); + for (int j = 0; j < collisionState.ACount; ++j) + { + if (featureId != collisionState.FeatureIdA[j]) + continue; + + toAdd ^= 1u << i; + toRemove ^= 1u << j; + break; + } } - else if (collision.WasTouching && !isTouching) + + while (toRemove != 0) { - listener.Handler.OnStoppedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + int index = 31 - BitOperations.LeadingZeroCount(toRemove); // LeadingZeroCount to remove from the end to the start + toRemove ^= 1u << index; + + int id = collisionState.FeatureIdA[index]; + + collisionState.ACount--; + if (index != collisionState.ACount) + collisionState.FeatureIdA[index] = collisionState.FeatureIdA[collisionState.ACount]; // Remove this index by swapping with last one + + handlerA?.OnContactRemoved(a, b, ref manifold, aFlipped, id, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + collisionState.BCount--; + if (index != collisionState.BCount) + collisionState.FeatureIdB[index] = collisionState.FeatureIdB[collisionState.BCount]; + + handlerB?.OnContactRemoved(b, a, ref manifold, bFlipped, id, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - if (isTouching) + + while (toAdd != 0) { - listener.Handler.OnTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + int index = BitOperations.TrailingZeroCount(toAdd); // We can add from the start to the end here + toAdd ^= 1u << index; + + int featureId = manifold.GetFeatureId(index); + + collisionState.FeatureIdA[collisionState.ACount++] = featureId; + handlerA?.OnContactAdded(a, b, ref manifold, aFlipped, index, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + collisionState.FeatureIdB[collisionState.BCount++] = featureId; + handlerB?.OnContactAdded(b, a, ref manifold, bFlipped, index, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - UpdatePreviousCollision(ref collision, ref manifold, isTouching); - break; } - if (previousCollisionIndex < 0) + else { - //There was no collision previously. - ref var addsForWorker = ref pendingWorkerAdds[workerIndex]; - //EnsureCapacity will create the list if it doesn't already exist. - addsForWorker.EnsureCapacity(Math.Max(addsForWorker.Count + 1, 64), GetPoolForWorker(workerIndex)); - ref var pendingAdd = ref addsForWorker.AllocateUnsafely(); - pendingAdd.ListenerIndex = listenerIndex; - pendingAdd.Collision.Collidable = other; - listener.Handler.OnPairCreated(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); - //Dispatch events for all contacts in this new manifold. + collisionState.Alive = true; // This is set as a flag to check for removal events + handlerA = collisionState.HandlerA = a.ContactEventHandler; + handlerB = collisionState.HandlerB = b.ContactEventHandler; + + if (handlerA is not null && collisionState.TrySet(Events.CreatedA)) + { + handlerA.OnPairCreated(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (handlerB is not null && collisionState.TrySet(Events.CreatedB)) + { + handlerB.OnPairCreated(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + for (int i = 0; i < manifold.Count; ++i) { - listener.Handler.OnContactAdded(sourceCollidable, otherCollidable, ref manifold, flippedManifold, i, workerIndex, bepuSimulation); - if (manifold.GetDepth(i) >= 0) - isTouching = true; + if (manifold.GetDepth(i) < 0) + continue; + + if (handlerA is not null && collisionState.TrySet(Events.TouchingA)) + { + handlerA.OnStartedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (handlerB is not null && collisionState.TrySet(Events.TouchingB)) + { + handlerB.OnStartedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (handlerA is not null) + { + handlerA.OnTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (handlerB is not null) + { + handlerB.OnTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + break; } - if (isTouching) + + for (int i = 0; i < manifold.Count; ++i) { - listener.Handler.OnStartedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); - listener.Handler.OnTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + int featureId = manifold.GetFeatureId(i); + + collisionState.FeatureIdA[collisionState.ACount++] = featureId; + handlerA?.OnContactAdded(a, b, ref manifold, aFlipped, i, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + collisionState.FeatureIdB[collisionState.BCount++] = featureId; + handlerB?.OnContactAdded(b, a, ref manifold, bFlipped, i, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - UpdatePreviousCollision(ref pendingAdd.Collision, ref manifold, isTouching); } - listener.Handler.OnPairUpdated(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); - } - //For final events fired by the flush that still expect a manifold, we'll provide a special empty type. - struct EmptyManifold : IContactManifold - { - public int Count => 0; - public bool Convex => true; - //This type never has any contacts, so there's no need for any property grabbers. - public Contact this[int contactIndex] { get => throw new NotImplementedException(); set => throw new NotImplementedException(); } - public static ref ConvexContact GetConvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public static ref float GetDepthReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public static ref int GetFeatureIdReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public static ref Contact GetNonconvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public static ref Vector3 GetNormalReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public static ref Vector3 GetOffsetReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); - public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) => throw new NotImplementedException(); - public void GetContact(int contactIndex, out Contact contactData) => throw new NotImplementedException(); - public float GetDepth(int contactIndex) => throw new NotImplementedException(); - public int GetFeatureId(int contactIndex) => throw new NotImplementedException(); - public Vector3 GetNormal(int contactIndex) => throw new NotImplementedException(); - public Vector3 GetOffset(int contactIndex) => throw new NotImplementedException(); + if (handlerA is not null) + { + handlerA.OnPairUpdated(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + if (handlerB is not null) + { + handlerB.OnPairUpdated(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } + + _outdatedPairs.Remove(orderedPair); } public void Flush() { - //For simplicity, this is completely sequential. Note that it's technically possible to extract more parallelism, but the complexity cost is high and you would need - //very large numbers of events being processed to make it worth it. + var manifold = new EmptyManifold(); //Remove any stale collisions. Stale collisions are those which should have received a new manifold update but did not because the manifold is no longer active. - for (int i = 0; i < listenerCount; ++i) + foreach (var pair in _outdatedPairs) + ClearCollision(pair, ref manifold, 0); + } + + /// + /// Callback attached to the simulation's ITimestepper which executes just prior to collision detection to take a snapshot of activity states to determine which pairs we should expect updates in. + /// + private void TrackActivePairs(float dt, IThreadDispatcher threadDispatcher) + { + // We need to be notified when two collidables are too far apart to have a manifold between them, + // We'll track any collision were one of the pair is active, manifolds we receive will filter out those that are still in contact + // leaving us to Flush() only those that are not + + var bodyHandleToLocation = _simulation.Simulation.Bodies.HandleToLocation; + foreach (var trackedCollision in _trackedCollisions) { - ref var listener = ref listeners[i]; - var sourceComponent = bepuSimulation.GetComponent(listener.Source); - //Note reverse order. We remove during iteration. - for (int j = listener.PreviousCollisions.Count - 1; j >= 0; --j) + var aRef = trackedCollision.Key.A.CollidableReference ?? throw new InvalidOperationException(); + var bRef = trackedCollision.Key.B.CollidableReference ?? throw new InvalidOperationException(); + if ((aRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[aRef.BodyHandle.Value].SetIndex == 0) + || (bRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[bRef.BodyHandle.Value].SetIndex == 0)) { - ref var collision = ref listener.PreviousCollisions[j]; - if (collision.Fresh) - { - collision.Fresh = false; - continue; - } + _outdatedPairs.Add(trackedCollision.Key); // It's active, if manifolds did not signal that they touched we should discard this one + } + } + } - var otherComponent = bepuSimulation.GetComponent(collision.Collidable); - //Sort the references to be consistent with the direct narrow phase results. - CollidablePair pair; - NarrowPhase.SortCollidableReferencesForPair(listener.Source, collision.Collidable, out _, out _, out pair.A, out pair.B); - if (collision.ContactCount > 0) - { - var emptyManifold = new EmptyManifold(); - for (int previousContactCount = 0; previousContactCount < collision.ContactCount; ++previousContactCount) - { - listener.Handler.OnContactRemoved(sourceComponent, otherComponent, ref emptyManifold, false, Unsafe.Add(ref collision.FeatureId0, previousContactCount), 0, bepuSimulation); - } - - if (collision.WasTouching) - { - listener.Handler.OnStoppedTouching(sourceComponent, otherComponent, ref emptyManifold, false, 0, bepuSimulation); - } - } + private unsafe struct LastCollisionState + { + public const int FeatureCount = 4; - listener.Handler.OnPairEnded(sourceComponent, otherComponent, bepuSimulation); - //This collision was not updated since the last flush despite being active. It should be removed. - listener.PreviousCollisions.FastRemoveAt(j); - if (listener.PreviousCollisions.Count == 0) - { - listener.PreviousCollisions.Dispose(pool); - listener.PreviousCollisions = default; - } + public IContactEventHandler? HandlerA, HandlerB; + public bool Alive; + public Events EventsTriggered; + public int ACount; + public int BCount; + //FeatureIds are identifiers encoding what features on the involved shapes contributed to the contact. We store up to 4 feature ids, one for each potential contact. + //A "feature" is things like a face, vertex, or edge. There is no single interpretation for what a feature is- the mapping is defined on a per collision pair level. + //In this demo, we only care to check whether a given contact in the current frame maps onto a contact from a previous frame. + //We can use this to only emit 'contact added' events when a new contact with an unrecognized id is reported. + public fixed int FeatureIdA[FeatureCount]; + public fixed int FeatureIdB[FeatureCount]; + + public bool TrySet(Events e) + { + if ((e & EventsTriggered) == 0) + { + EventsTriggered |= e; + return true; } + + return false; } - for (int i = 0; i < pendingWorkerAdds.Length; ++i) + public bool TryClear(Events e) { - ref var pendingAdds = ref pendingWorkerAdds[i]; - for (int j = 0; j < pendingAdds.Count; ++j) + if ((e & EventsTriggered) == e) { - ref var add = ref pendingAdds[j]; - ref var collisions = ref listeners[add.ListenerIndex].PreviousCollisions; - //Ensure capacity will initialize the slot if necessary. - collisions.EnsureCapacity(Math.Max(8, collisions.Count + 1), pool); - collisions.AllocateUnsafely() = pendingAdds[j].Collision; + EventsTriggered ^= e; + return true; } - if (pendingAdds.Span.Allocated) - pendingAdds.Dispose(GetPoolForWorker(i)); - //We rely on zeroing out the count for lazy initialization. - pendingAdds = default; + + return false; } } - public void Dispose() + [Flags] + private enum Events + { + CreatedA = 0b0001, + CreatedB = 0b0010, + TouchingA = 0b0100, + TouchingB = 0b1000, + } + + private readonly record struct OrderedPair { - if (bodyListenerFlags.Flags.Allocated) - bodyListenerFlags.Dispose(pool); - if (staticListenerFlags.Flags.Allocated) - staticListenerFlags.Dispose(pool); - listenerIndices.Dispose(); - simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + public readonly CollidableComponent A, B; + public OrderedPair(CollidableComponent a, CollidableComponent b) + { + if (a.InstanceIndex != b.InstanceIndex) + (A, B) = a.InstanceIndex > b.InstanceIndex ? (a, b) : (b, a); + else if (a.GetHashCode() != b.GetHashCode()) + (A, B) = a.GetHashCode() > b.GetHashCode() ? (a, b) : (b, a); + else if (ReferenceEquals(a, b)) + (A, B) = (a, b); + else + throw new InvalidOperationException("Could not order this pair of collidable, incredibly unlikely event"); + } + } + + + private struct EmptyManifold : IContactManifold + { + public int Count => 0; + public bool Convex => true; + public Contact this[int contactIndex] { get => throw new IndexOutOfRangeException("This manifold is empty"); set => throw new IndexOutOfRangeException("This manifold is empty"); } + public static ref ConvexContact GetConvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public static ref float GetDepthReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public static ref int GetFeatureIdReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public static ref Contact GetNonconvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public static ref Vector3 GetNormalReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public static ref Vector3 GetOffsetReference(ref EmptyManifold manifold, int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) => throw new IndexOutOfRangeException("This manifold is empty"); + public void GetContact(int contactIndex, out Contact contactData) => throw new IndexOutOfRangeException("This manifold is empty"); + public float GetDepth(int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public int GetFeatureId(int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public Vector3 GetNormal(int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); + public Vector3 GetOffset(int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs index bb10c56d5e..5768ddd0eb 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs @@ -18,6 +18,10 @@ public interface IContactEventHandler /// /// Fires when a contact is added. /// + /// + /// This may be called before , + /// contacts are registered when two collidables are close enough, not necessarily when actually touching. + /// /// Type of the contact manifold detected. /// Collidable that the event was attached to. /// Other collider collided with. @@ -33,6 +37,11 @@ void OnContactAdded(CollidableComponent eventSource, CollidableCompon /// /// Fires when a contact is removed. /// + /// + /// This may be called without a corresponding call to , + /// contacts are registered when two collidables are close enough, not necessarily when actually touching. + /// If the two collidables grazed each other, none of the touching methods will be called. + /// /// Type of the contact manifold detected. /// Collidable that the event was attached to. /// Other collider collided with. diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs index ef7438feee..24fec9c155 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs @@ -58,6 +58,16 @@ public ContinuousDetection ContinuousDetection protected override ref MaterialProperties MaterialProperties => ref Simulation!.CollidableMaterials[StaticReference!.Value.Handle]; protected internal override NRigidPose? Pose => StaticReference?.Pose; + protected internal override CollidableReference? CollidableReference + { + get + { + if (StaticReference is { } sRef) + return sRef.CollidableReference; + return null; + } + } + protected override void AttachInner(NRigidPose pose, BodyInertia shapeInertia, TypedIndex shapeIndex) { Debug.Assert(Processor is not null); @@ -104,23 +114,4 @@ protected override int GetHandleValue() throw new InvalidOperationException(); } - - protected override void RegisterContactHandler() - { - if (ContactEventHandler is not null && Simulation is not null && StaticReference is { } sRef) - Simulation.ContactEvents.Register(sRef.Handle, ContactEventHandler); - } - - protected override void UnregisterContactHandler() - { - if (Simulation is not null && StaticReference is { } sRef) - Simulation.ContactEvents.Unregister(sRef.Handle); - } - - protected override bool IsContactHandlerRegistered() - { - if (Simulation is not null && StaticReference is { } sRef) - return Simulation.ContactEvents.IsListener(sRef.Handle); - return false; - } }