From 16c782c16a50646240d4e6ca08baf287dc2a4df9 Mon Sep 17 00:00:00 2001 From: Eideren Date: Sat, 25 Jan 2025 23:26:11 +0100 Subject: [PATCH 1/3] fix: Re-entry/removal of collidables in contact events --- .../Stride.BepuPhysics/BepuSimulation.cs | 4 +- .../Stride.BepuPhysics/BodyComponent.cs | 14 +- .../Stride.BepuPhysics/CollidableComponent.cs | 11 +- .../Contacts/ContactEventsManager.cs | 635 +++++++++--------- .../Stride.BepuPhysics/StaticComponent.cs | 4 +- 5 files changed, 328 insertions(+), 340 deletions(-) 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..19e04718d9 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] @@ -421,7 +421,7 @@ protected override int GetHandleValue() protected override void RegisterContactHandler() { if (ContactEventHandler is not null && Simulation is not null && BodyReference is { } bRef) - Simulation.ContactEvents.Register(bRef.Handle, ContactEventHandler); + Simulation.ContactEvents.Register(bRef.Handle); } protected override void UnregisterContactHandler() @@ -433,7 +433,7 @@ protected override void UnregisterContactHandler() protected override bool IsContactHandlerRegistered() { if (Simulation is not null && BodyReference is { } bRef) - return Simulation.ContactEvents.IsListener(bRef.Handle); + return Simulation.ContactEvents.IsRegistered(bRef.Handle); return false; } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs index ad25d766e6..47f8e12d3c 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 StableId { 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 @@ -291,7 +294,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); } 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..5238f4ca67 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -18,160 +18,60 @@ 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 BufferPool _pool; + private readonly BepuSimulation _simulation; + private IndexSet _staticListenerFlags; + private IndexSet _bodyListenerFlags; + private int _flushes; + + 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; + _pool = pool; + _simulation = simulation; } - 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; - } - 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 += SetFreshnessForCurrentActivityStatus; } - 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]; - } - - /// - /// 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) - { - if (collidable.Mobility == CollidableMobility.Static) - staticListenerFlags.Add(collidable.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; + //_simulation.Simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + if (_bodyListenerFlags.Flags.Allocated) + _bodyListenerFlags.Dispose(_pool); + if (_staticListenerFlags.Flags.Allocated) + _staticListenerFlags.Dispose(_pool); } /// /// 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) + public void Register(BodyHandle body) { - Register(simulation.Bodies[body].CollidableReference, handler); + Register(_simulation.Simulation.Bodies[body].CollidableReference); } /// /// 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) + public void Register(StaticHandle staticHandle) { - Register(new CollidableReference(staticHandle), handler); + Register(new CollidableReference(staticHandle)); } /// - /// Stops listening for events related to the given collidable. + /// Begins listening for events related to the given collidable. /// - /// Collidable to stop listening for. - public void Unregister(CollidableReference collidable) + public void Register(CollidableReference collidable) { if (collidable.Mobility == CollidableMobility.Static) - { - staticListenerFlags.Remove(collidable.RawHandleValue); - } + _staticListenerFlags.Add(collidable.RawHandleValue, _pool); 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.Add(collidable.RawHandleValue, _pool); } /// @@ -180,7 +80,7 @@ public void Unregister(CollidableReference collidable) /// Body to stop listening for. public void Unregister(BodyHandle body) { - Unregister(simulation.Bodies[body].CollidableReference); + Unregister(_simulation.Simulation.Bodies[body].CollidableReference); } /// @@ -193,295 +93,380 @@ public void Unregister(StaticHandle staticHandle) } /// - /// Checks if a collidable is registered as a listener. + /// Stops listening for events related to the given collidable. /// - /// Collidable to check. - /// True if the collidable has been registered as a listener, false otherwise. - public bool IsListener(CollidableReference collidable) + public void Unregister(CollidableReference collidable) { if (collidable.Mobility == CollidableMobility.Static) - { - return staticListenerFlags.Contains(collidable.RawHandleValue); - } + _staticListenerFlags.Remove(collidable.RawHandleValue); else - { - return bodyListenerFlags.Contains(collidable.RawHandleValue); - } + _bodyListenerFlags.Remove(collidable.RawHandleValue); + + ClearCollisionsOf(_simulation.GetComponent(collidable)); } /// /// Checks if a collidable is registered as a listener. /// - public bool IsListener(BodyHandle body) + public bool IsRegistered(BodyHandle body) { - return IsListener(simulation.Bodies[body].CollidableReference); + return IsRegistered(_simulation.Simulation.Bodies[body].CollidableReference); } /// /// Checks if a collidable is registered as a listener. /// - public bool IsListener(StaticHandle staticHandle) + public bool IsRegistered(StaticHandle staticHandle) { - return IsListener(simulation.Statics[staticHandle].CollidableReference); + return IsRegistered(_simulation.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. + /// Checks if a collidable is registered as a listener. /// - void SetFreshnessForCurrentActivityStatus(float dt, IThreadDispatcher threadDispatcher) + public bool IsRegistered(CollidableReference collidable) { - //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'. + if (collidable.Mobility == CollidableMobility.Static) + return _staticListenerFlags.Contains(collidable.RawHandleValue); + else + return _bodyListenerFlags.Contains(collidable.RawHandleValue); + } - //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) + public unsafe void ClearCollisionsOf(CollidableComponent collidable) + { +#error Handle handler exceptions + // 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 + + int workerIndex = 0; + var manifold = new EmptyManifold(); + bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one + + 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) + if (!ReferenceEquals(pair.A, collidable) && !ReferenceEquals(pair.B, collidable)) + continue; + +#if DEBUG + ref var stateRef = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, pair, out _); + _trackedCollisions.Remove(pair); + 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); +#endif + + for (int i = 0; i < state.ContactCount; i++) { - 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; - } + state.HandlerA?.OnContactRemoved(pair.A, pair.B, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); + state.HandlerB?.OnContactRemoved(pair.B, pair.A, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); } - else + + if (state.Touching) { - //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; - } + state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); + state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, workerIndex, _simulation); } - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void UpdatePreviousCollision(ref PreviousCollision collision, ref TManifold manifold, bool isTouching) where TManifold : unmanaged, IContactManifold - { - //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); + state.HandlerA?.OnPairEnded(pair.A, pair.B, _simulation); + state.HandlerB?.OnPairEnded(pair.B, pair.A, _simulation); } - collision.ContactCount = manifold.Count; - collision.Fresh = true; - collision.WasTouching = isTouching; } 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.A), 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]; +#error Handle handler exceptions + ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, new OrderedPair(a, b), out bool alreadyExisted); + + var handlerA = a.ContactEventHandler; + var handlerB = b.ContactEventHandler; - int previousCollisionIndex = -1; - bool isTouching = false; - for (int i = 0; i < listener.PreviousCollisions.Count; ++i) + if (alreadyExisted) { - 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; + bool previouslyTouching = collisionState.Touching; + for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex) + { + if (manifold.GetDepth(contactIndex) < 0) + continue; + + if (collisionState.Touching == false) + { + collisionState.Touching = true; + handlerA?.OnStartedTouching(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnStartedTouching(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } - 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; + handlerA?.OnTouching(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnTouching(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + break; + } + + int previousContactsStillExist = 0; // Bitmask to mark contacts that are both inside the previous and current manifold 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) + for (int previousContactIndex = 0; previousContactIndex < collisionState.ContactCount; ++previousContactIndex) { - if (featureId == Unsafe.Add(ref collision.FeatureId0, previousContactIndex)) + if (featureId == collisionState.FeatureId[previousContactIndex]) { featureIdWasInPreviousCollision = true; previousContactsStillExist |= 1 << previousContactIndex; break; } } + if (!featureIdWasInPreviousCollision) { - listener.Handler.OnContactAdded(sourceCollidable, otherCollidable, ref manifold, flippedManifold, contactIndex, workerIndex, bepuSimulation); + handlerA?.OnContactAdded(a, b, ref manifold, false, contactIndex, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnContactAdded(b, a, ref manifold, true, contactIndex, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - if (manifold.GetDepth(contactIndex) >= 0) - isTouching = true; } - if (previousContactsStillExist != (1 << collision.ContactCount) - 1) + + if (previousContactsStillExist != (1 << collisionState.ContactCount) - 1) //At least one contact that used to exist no longer does. { - //At least one contact that used to exist no longer does. - for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex) + for (int previousContactIndex = 0; previousContactIndex < collisionState.ContactCount; ++previousContactIndex) { - if ((previousContactsStillExist & 1 << previousContactIndex) == 0) - { - listener.Handler.OnContactRemoved(sourceCollidable, otherCollidable, ref manifold, flippedManifold, Unsafe.Add(ref collision.FeatureId0, previousContactIndex), workerIndex, bepuSimulation); - } + if ((previousContactsStillExist & (1 << previousContactIndex)) != 0) + continue; + + int id = collisionState.FeatureId[previousContactIndex]; + handlerA?.OnContactRemoved(a, b, ref manifold, false, id, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnContactRemoved(b, a, ref manifold, true, id, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } } - if (!collision.WasTouching && isTouching) + + if (previouslyTouching && collisionState.Touching == false) { - listener.Handler.OnStartedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + handlerA?.OnStoppedTouching(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + handlerB?.OnStoppedTouching(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - else if (collision.WasTouching && !isTouching) + } + else + { + collisionState.Alive = true; // This is set as a flag to check for removal events + collisionState.HandlerA = handlerA; + collisionState.HandlerB = handlerB; + + + handlerA?.OnPairCreated(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnPairCreated(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + for (int i = 0; i < manifold.Count; ++i) { - listener.Handler.OnStoppedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + if (manifold.GetDepth(i) < 0) + continue; + + collisionState.Touching = true; + + handlerA?.OnStartedTouching(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnStartedTouching(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + handlerA?.OnTouching(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnTouching(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + break; } - if (isTouching) + + for (int i = 0; i < manifold.Count; ++i) { - listener.Handler.OnTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + handlerA?.OnContactAdded(a, b, ref manifold, false, i, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnContactAdded(b, a, ref manifold, true, i, workerIndex, _simulation); + if (collisionState.Alive == false) + return; } - UpdatePreviousCollision(ref collision, ref manifold, isTouching); - break; } - if (previousCollisionIndex < 0) + + handlerA?.OnPairUpdated(a, b, ref manifold, false, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + handlerB?.OnPairUpdated(b, a, ref manifold, true, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + + 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. + for (int j = 0; j < manifold.Count; ++j) + collisionState.FeatureId[j] = manifold.GetFeatureId(j); + + collisionState.ContactCount = manifold.Count; + collisionState.Flushes = _flushes; + } + + public unsafe void Flush() + { +#error Handle handler exceptions + return; + int workerIndex = 0; + var manifold = new EmptyManifold(); + bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one + + //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. + foreach (var (pair, state) in _trackedCollisions) { - //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. - for (int i = 0; i < manifold.Count; ++i) + if (state.Flushes == _flushes) + continue; + + if ((state.Flushes & 1) != (_flushes & 1)) + continue; + + // Two flushes ago, remove the collision + +#if DEBUG + ref var stateRef = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, pair, out _); + _trackedCollisions.Remove(pair); + 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); +#endif + + for (int i = 0; i < state.ContactCount; i++) { - listener.Handler.OnContactAdded(sourceCollidable, otherCollidable, ref manifold, flippedManifold, i, workerIndex, bepuSimulation); - if (manifold.GetDepth(i) >= 0) - isTouching = true; + state.HandlerA?.OnContactRemoved(pair.A, pair.B, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); + state.HandlerB?.OnContactRemoved(pair.B, pair.A, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); } - if (isTouching) + + if (state.Touching) { - listener.Handler.OnStartedTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); - listener.Handler.OnTouching(sourceCollidable, otherCollidable, ref manifold, flippedManifold, workerIndex, bepuSimulation); + state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); + state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, workerIndex, _simulation); } - UpdatePreviousCollision(ref pendingAdd.Collision, ref manifold, isTouching); + + state.HandlerA?.OnPairEnded(pair.A, pair.B, _simulation); + state.HandlerB?.OnPairEnded(pair.B, pair.A, _simulation); } - 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(); + _flushes++; } - public void Flush() + /// + /// 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) { - //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. + //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'. - //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) + //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.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) - { - ref var collision = ref listener.PreviousCollisions[j]; - if (collision.Fresh) - { - collision.Fresh = false; - continue; - } + var source = trackedCollision.Key.A.ContactEventHandler; - 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) + //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) { - 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); - } + //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; } - - 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) + } + 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) { - listener.PreviousCollisions.Dispose(pool); - listener.PreviousCollisions = default; + ref var previousCollision = ref previousCollisions[j]; + previousCollision.Fresh = previousCollision.Collidable.Mobility == CollidableMobility.Static || bodyHandleToLocation[previousCollision.Collidable.BodyHandle.Value].SetIndex > 0; } } } + }*/ + + private unsafe struct LastCollisionState + { + public const int FeatureCount = 4; + + public IContactEventHandler? HandlerA, HandlerB; + public bool Alive; + public bool Touching; + public int Flushes; + 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 fixed int FeatureId[FeatureCount]; + } - for (int i = 0; i < pendingWorkerAdds.Length; ++i) + private readonly record struct OrderedPair + { + public readonly CollidableComponent A, B; + public OrderedPair(CollidableComponent a, CollidableComponent b) { - ref var pendingAdds = ref pendingWorkerAdds[i]; - for (int j = 0; j < pendingAdds.Count; ++j) - { - 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; - } - if (pendingAdds.Span.Allocated) - pendingAdds.Dispose(GetPoolForWorker(i)); - //We rely on zeroing out the count for lazy initialization. - pendingAdds = default; + (A, B) = a.StableId > b.StableId ? (a, b) : (b, a); } } - public void Dispose() + + private struct EmptyManifold : IContactManifold { - if (bodyListenerFlags.Flags.Allocated) - bodyListenerFlags.Dispose(pool); - if (staticListenerFlags.Flags.Allocated) - staticListenerFlags.Dispose(pool); - listenerIndices.Dispose(); - simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + 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/StaticComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs index ef7438feee..800a272ad0 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs @@ -108,7 +108,7 @@ protected override int GetHandleValue() protected override void RegisterContactHandler() { if (ContactEventHandler is not null && Simulation is not null && StaticReference is { } sRef) - Simulation.ContactEvents.Register(sRef.Handle, ContactEventHandler); + Simulation.ContactEvents.Register(sRef.Handle); } protected override void UnregisterContactHandler() @@ -120,7 +120,7 @@ protected override void UnregisterContactHandler() protected override bool IsContactHandlerRegistered() { if (Simulation is not null && StaticReference is { } sRef) - return Simulation.ContactEvents.IsListener(sRef.Handle); + return Simulation.ContactEvents.IsRegistered(sRef.Handle); return false; } } From abee704099738b268006a74363bc15e093d387e9 Mon Sep 17 00:00:00 2001 From: Eideren Date: Mon, 27 Jan 2025 20:55:18 +0100 Subject: [PATCH 2/3] Finish Flush implementation, add tests --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 134 ++++- .../Stride.BepuPhysics.Tests.csproj | 1 + .../Stride.BepuPhysics/BodyComponent.cs | 29 +- .../Stride.BepuPhysics/CollidableComponent.cs | 24 +- .../Contacts/ContactEventsManager.cs | 475 +++++++++--------- .../Contacts/IContactEventHandler.cs | 9 + .../Stride.BepuPhysics/StaticComponent.cs | 29 +- 7 files changed, 419 insertions(+), 282 deletions(-) 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/BodyComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs index 19e04718d9..191a755a63 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs @@ -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); - } - - 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.IsRegistered(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 47f8e12d3c..706a20845e 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs @@ -48,7 +48,7 @@ public abstract class CollidableComponent : EntityComponent [DataMemberIgnore] internal uint Versioning { get; private set; } - internal uint StableId { get; } = Interlocked.Increment(ref IdCounter); + internal uint InstanceIndex { get; } = Interlocked.Increment(ref IdCounter); /// /// The simulation this object belongs to, null when it is not part of a simulation. @@ -219,6 +219,8 @@ public IContactEventHandler? ContactEventHandler [DataMemberIgnore] public Vector3 CenterOfMass { get; private set; } + protected internal abstract CollidableReference? CollidableReference { get; } + public CollidableComponent() { _collider = new CompoundCollider(); @@ -341,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 5238f4ca67..e020559805 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; @@ -19,11 +17,11 @@ namespace Stride.BepuPhysics.Definitions.Contacts; internal class ContactEventsManager : IDisposable { private readonly Dictionary _trackedCollisions = new(); + private readonly HashSet _outdatedPairs = new(); private readonly BufferPool _pool; private readonly BepuSimulation _simulation; private IndexSet _staticListenerFlags; private IndexSet _bodyListenerFlags; - private int _flushes; public ContactEventsManager(BufferPool pool, BepuSimulation simulation) { @@ -33,143 +31,109 @@ public ContactEventsManager(BufferPool pool, BepuSimulation simulation) public void Initialize() { - //simulation.Simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; + _simulation.Simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; } public void Dispose() { - //_simulation.Simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + _simulation.Simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; if (_bodyListenerFlags.Flags.Allocated) _bodyListenerFlags.Dispose(_pool); if (_staticListenerFlags.Flags.Allocated) _staticListenerFlags.Dispose(_pool); } - /// - /// Begins listening for events related to the given body. - /// - /// Body to monitor for events. - public void Register(BodyHandle body) - { - Register(_simulation.Simulation.Bodies[body].CollidableReference); - } - - /// - /// Begins listening for events related to the given static. - /// - /// Static to monitor for events. - public void Register(StaticHandle staticHandle) - { - Register(new CollidableReference(staticHandle)); - } - /// /// Begins listening for events related to the given collidable. /// - public void Register(CollidableReference collidable) + 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); - } - - /// - /// Stops listening for events related to the given body. - /// - /// Body to stop listening for. - public void Unregister(BodyHandle body) - { - Unregister(_simulation.Simulation.Bodies[body].CollidableReference); - } - - /// - /// Stops listening for events related to the given static. - /// - /// Static to stop listening for. - public void Unregister(StaticHandle staticHandle) - { - Unregister(new CollidableReference(staticHandle)); + _bodyListenerFlags.Add(reference.RawHandleValue, _pool); } /// /// Stops listening for events related to the given collidable. /// - 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); + _bodyListenerFlags.Remove(reference.RawHandleValue); - ClearCollisionsOf(_simulation.GetComponent(collidable)); + ClearCollisionsOf(collidable); } /// /// Checks if a collidable is registered as a listener. /// - public bool IsRegistered(BodyHandle body) - { - return IsRegistered(_simulation.Simulation.Bodies[body].CollidableReference); - } - /// - /// Checks if a collidable is registered as a listener. - /// - public bool IsRegistered(StaticHandle staticHandle) + public bool IsRegistered(CollidableComponent collidable) { - return IsRegistered(_simulation.Simulation.Statics[staticHandle].CollidableReference); + if (collidable.CollidableReference is { } reference) + return IsRegistered(reference); + + return false; } /// /// Checks if a collidable is registered as a listener. /// - public bool IsRegistered(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); } - public unsafe void ClearCollisionsOf(CollidableComponent collidable) + public void ClearCollisionsOf(CollidableComponent collidable) { -#error Handle handler exceptions // 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 - int workerIndex = 0; var manifold = new EmptyManifold(); - bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one - foreach (var (pair, state) in _trackedCollisions) { if (!ReferenceEquals(pair.A, collidable) && !ReferenceEquals(pair.B, collidable)) continue; + ClearCollision(pair, ref manifold, 0); + } + } + + private unsafe void ClearCollision(OrderedPair pair, ref EmptyManifold manifold, int workerIndex) + { + 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); - System.Diagnostics.Debug.Assert(stateRef.Alive == false); // Notify HandleManifoldInner higher up the call stack that the manifold they are processing is dead + 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); + _trackedCollisions.Remove(pair, out var state); #endif - for (int i = 0; i < state.ContactCount; i++) - { - state.HandlerA?.OnContactRemoved(pair.A, pair.B, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); - state.HandlerB?.OnContactRemoved(pair.B, pair.A, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); - } + 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.Touching) - { - state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); - state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, 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 @@ -179,96 +143,128 @@ public void HandleManifold(int workerIndex, CollidablePair pair, ref if (aListener == false && bListener == false) return; - HandleManifoldInner(workerIndex, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.A), ref manifold); + HandleManifoldInner(workerIndex, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.B), ref manifold); } private unsafe void HandleManifoldInner(int workerIndex, CollidableComponent a, CollidableComponent b, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { -#error Handle handler exceptions - ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, new OrderedPair(a, b), out bool alreadyExisted); + 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. + + // 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); + + bool aFlipped = ReferenceEquals(a, orderedPair.B); // Whether the manifold is flipped from a's point of view + bool bFlipped = !aFlipped; - var handlerA = a.ContactEventHandler; - var handlerB = b.ContactEventHandler; + (a, b) = (orderedPair.A, orderedPair.B); + IContactEventHandler? handlerA; + IContactEventHandler? handlerB; + ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, orderedPair, out bool alreadyExisted); if (alreadyExisted) { - bool previouslyTouching = collisionState.Touching; + handlerA = collisionState.HandlerA; + handlerB = collisionState.HandlerB; + bool touching = false; for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex) { if (manifold.GetDepth(contactIndex) < 0) continue; - if (collisionState.Touching == false) + touching = true; + if (handlerA is not null && collisionState.TrySet(Events.TouchingA)) { - collisionState.Touching = true; - handlerA?.OnStartedTouching(a, b, ref manifold, false, workerIndex, _simulation); + handlerA.OnStartedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); if (collisionState.Alive == false) return; - handlerB?.OnStartedTouching(b, a, ref manifold, true, workerIndex, _simulation); + } + if (handlerB is not null && collisionState.TrySet(Events.TouchingB)) + { + handlerB.OnStartedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); if (collisionState.Alive == false) return; } - handlerA?.OnTouching(a, b, ref manifold, false, workerIndex, _simulation); + handlerA?.OnTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); if (collisionState.Alive == false) return; - handlerB?.OnTouching(b, a, ref manifold, true, workerIndex, _simulation); + handlerB?.OnTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); if (collisionState.Alive == false) return; break; } - int previousContactsStillExist = 0; // Bitmask to mark contacts that are both inside the previous and current manifold - for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex) + if (touching == false && handlerA is not null && collisionState.TryClear(Events.TouchingA)) { - //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 < collisionState.ContactCount; ++previousContactIndex) - { - if (featureId == collisionState.FeatureId[previousContactIndex]) - { - featureIdWasInPreviousCollision = true; - previousContactsStillExist |= 1 << previousContactIndex; - break; - } - } + handlerA.OnStoppedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } - if (!featureIdWasInPreviousCollision) - { - handlerA?.OnContactAdded(a, b, ref manifold, false, contactIndex, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnContactAdded(b, a, ref manifold, true, contactIndex, 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 (previousContactsStillExist != (1 << collisionState.ContactCount) - 1) //At least one contact that used to exist no longer does. + 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 { - for (int previousContactIndex = 0; previousContactIndex < collisionState.ContactCount; ++previousContactIndex) + int featureId = manifold.GetFeatureId(i); + for (int j = 0; j < collisionState.ACount; ++j) { - if ((previousContactsStillExist & (1 << previousContactIndex)) != 0) + if (featureId != collisionState.FeatureIdA[j]) continue; - int id = collisionState.FeatureId[previousContactIndex]; - handlerA?.OnContactRemoved(a, b, ref manifold, false, id, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnContactRemoved(b, a, ref manifold, true, id, workerIndex, _simulation); - if (collisionState.Alive == false) - return; + toAdd ^= 1u << i; + toRemove ^= 1u << j; + break; } } - if (previouslyTouching && collisionState.Touching == false) + while (toRemove != 0) + { + 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; + } + + while (toAdd != 0) { - handlerA?.OnStoppedTouching(a, b, ref manifold, false, workerIndex, _simulation); + 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; - handlerB?.OnStoppedTouching(b, a, ref manifold, true, workerIndex, _simulation); + collisionState.FeatureIdB[collisionState.BCount++] = featureId; + handlerB?.OnContactAdded(b, a, ref manifold, bFlipped, index, workerIndex, _simulation); if (collisionState.Alive == false) return; } @@ -276,118 +272,104 @@ private unsafe void HandleManifoldInner(int workerIndex, CollidableCo else { collisionState.Alive = true; // This is set as a flag to check for removal events - collisionState.HandlerA = handlerA; - collisionState.HandlerB = handlerB; + 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; + } - handlerA?.OnPairCreated(a, b, ref manifold, false, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnPairCreated(b, a, ref manifold, true, 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) { if (manifold.GetDepth(i) < 0) continue; - collisionState.Touching = true; + if (handlerA is not null && collisionState.TrySet(Events.TouchingA)) + { + handlerA.OnStartedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } - handlerA?.OnStartedTouching(a, b, ref manifold, false, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnStartedTouching(b, a, ref manifold, true, 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; + } - handlerA?.OnTouching(a, b, ref manifold, false, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnTouching(b, a, ref manifold, true, 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; } for (int i = 0; i < manifold.Count; ++i) { - handlerA?.OnContactAdded(a, b, ref manifold, false, i, workerIndex, _simulation); + 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; - handlerB?.OnContactAdded(b, a, ref manifold, true, i, workerIndex, _simulation); + + collisionState.FeatureIdB[collisionState.BCount++] = featureId; + handlerB?.OnContactAdded(b, a, ref manifold, bFlipped, i, workerIndex, _simulation); if (collisionState.Alive == false) return; } } - handlerA?.OnPairUpdated(a, b, ref manifold, false, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnPairUpdated(b, a, ref manifold, true, workerIndex, _simulation); - if (collisionState.Alive == false) - return; + if (handlerA is not null) + { + handlerA.OnPairUpdated(a, b, ref manifold, aFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } - 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. - for (int j = 0; j < manifold.Count; ++j) - collisionState.FeatureId[j] = manifold.GetFeatureId(j); + if (handlerB is not null) + { + handlerB.OnPairUpdated(b, a, ref manifold, bFlipped, workerIndex, _simulation); + if (collisionState.Alive == false) + return; + } - collisionState.ContactCount = manifold.Count; - collisionState.Flushes = _flushes; + _outdatedPairs.Remove(orderedPair); } - public unsafe void Flush() + public void Flush() { -#error Handle handler exceptions - return; - int workerIndex = 0; var manifold = new EmptyManifold(); - bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one //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. - foreach (var (pair, state) in _trackedCollisions) - { - if (state.Flushes == _flushes) - continue; - - if ((state.Flushes & 1) != (_flushes & 1)) - continue; - - // Two flushes ago, remove the collision - -#if DEBUG - ref var stateRef = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, pair, out _); - _trackedCollisions.Remove(pair); - 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); -#endif - - for (int i = 0; i < state.ContactCount; i++) - { - state.HandlerA?.OnContactRemoved(pair.A, pair.B, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); - state.HandlerB?.OnContactRemoved(pair.B, pair.A, ref manifold, flippedManifold, state.FeatureId[i], workerIndex, _simulation); - } - - if (state.Touching) - { - state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); - state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, workerIndex, _simulation); - } - - state.HandlerA?.OnPairEnded(pair.A, pair.B, _simulation); - state.HandlerB?.OnPairEnded(pair.B, pair.A, _simulation); - } - - _flushes++; + 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. /// - /*void SetFreshnessForCurrentActivityStatus(float dt, IThreadDispatcher threadDispatcher) + private 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. @@ -399,31 +381,15 @@ public unsafe void Flush() var bodyHandleToLocation = _simulation.Simulation.Bodies.HandleToLocation; foreach (var trackedCollision in _trackedCollisions) { - var source = trackedCollision.Key.A.ContactEventHandler; - - //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 + 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)) { - //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; - } + _outdatedPairs.Add(trackedCollision.Key); // It's active, if manifolds did not signal that they touched we should discard this one } } - }*/ + } private unsafe struct LastCollisionState { @@ -431,14 +397,46 @@ private unsafe struct LastCollisionState public IContactEventHandler? HandlerA, HandlerB; public bool Alive; - public bool Touching; - public int Flushes; - public int ContactCount; + 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 FeatureId[FeatureCount]; + 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; + } + + public bool TryClear(Events e) + { + if ((e & EventsTriggered) == e) + { + EventsTriggered ^= e; + return true; + } + + return false; + } + } + + [Flags] + private enum Events + { + CreatedA = 0b0001, + CreatedB = 0b0010, + TouchingA = 0b0100, + TouchingB = 0b1000, } private readonly record struct OrderedPair @@ -446,7 +444,14 @@ private readonly record struct OrderedPair public readonly CollidableComponent A, B; public OrderedPair(CollidableComponent a, CollidableComponent b) { - (A, B) = a.StableId > b.StableId ? (a, b) : (b, a); + 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"); } } 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 800a272ad0..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); - } - - 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.IsRegistered(sRef.Handle); - return false; - } } From 32b448c196bd29909c19d2fbccca02305363b9eb Mon Sep 17 00:00:00 2001 From: Eideren Date: Mon, 27 Jan 2025 21:10:43 +0100 Subject: [PATCH 3/3] Update comments and name of SetFreshnessForCurrentActivityStatus --- .../Definitions/Contacts/ContactEventsManager.cs | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) 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 e020559805..cc629a2f18 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -31,12 +31,12 @@ public ContactEventsManager(BufferPool pool, BepuSimulation simulation) public void Initialize() { - _simulation.Simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; + _simulation.Simulation.Timestepper.BeforeCollisionDetection += TrackActivePairs; } public void Dispose() { - _simulation.Simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + _simulation.Simulation.Timestepper.BeforeCollisionDetection -= TrackActivePairs; if (_bodyListenerFlags.Flags.Allocated) _bodyListenerFlags.Dispose(_pool); if (_staticListenerFlags.Flags.Allocated) @@ -369,15 +369,12 @@ public void Flush() /// /// 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 SetFreshnessForCurrentActivityStatus(float dt, IThreadDispatcher threadDispatcher) + private void TrackActivePairs(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'. + // 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 - //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.Simulation.Bodies.HandleToLocation; foreach (var trackedCollision in _trackedCollisions) {