From eec56a526cae30f64eaa573d2b6850477603fc46 Mon Sep 17 00:00:00 2001 From: Eideren Date: Fri, 19 Sep 2025 17:58:43 +0200 Subject: [PATCH 01/13] fix: Refactor bepu contact management, support compound shape child index lookup --- .../Stride.BepuPhysics/CollidableComponent.cs | 4 +- .../Definitions/Contacts/ContactData.cs | 127 ++++++++ .../Contacts/ContactEventsManager.cs | 295 ++++++------------ .../Contacts/IContactEventHandler.cs | 17 +- .../Definitions/Contacts/IContactHandler.cs | 42 +++ .../Definitions/StrideNarrowPhaseCallbacks.cs | 5 +- 6 files changed, 286 insertions(+), 204 deletions(-) create mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs create mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs index 26be920f67..b76e206b50 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs @@ -46,7 +46,7 @@ public abstract class CollidableComponent : EntityComponent private CollisionGroup _collisionGroup; private ICollider _collider; - private IContactEventHandler? _trigger; + private IContactHandler? _trigger; private ISimulationSelector _simulationSelector = SceneBasedSimulationSelector.Shared; [DataMemberIgnore] @@ -215,7 +215,7 @@ public CollisionGroup CollisionGroup /// Provides the ability to collect and mutate contact data when this object collides with other objects. /// [Display(category: CategoryContacts)] - public IContactEventHandler? ContactEventHandler + public IContactHandler? ContactEventHandler { get { diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs new file mode 100644 index 0000000000..436b787ac7 --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs @@ -0,0 +1,127 @@ +// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) +// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. + +using BepuPhysics.CollisionDetection; +using Stride.Core.Mathematics; + +namespace Stride.BepuPhysics.Definitions.Contacts; + +/// +/// Enumerate over this structure to get individual contacts +/// +/// +/// (ContactData contactData) where TManifold : unmanaged, IContactManifold +/// { +/// foreach (var contact in contactData) +/// { +/// contact.Normal ... +/// } +/// } +/// ]]> +/// +public readonly ref struct ContactData where TManifold : unmanaged, IContactManifold +{ + /// + /// The collidable which is bound to this + /// + public CollidableComponent EventSource { get; init; } + + /// + /// The other collidable + /// + public CollidableComponent Other { get; init; } + + /// + /// The simulation this contact occured in + /// + public BepuSimulation Simulation { get; init; } + + /// + /// The raw contact manifold + /// + /// + /// Make sure that you understand and handle before using this property + /// + public TManifold Manifold { get; init; } + + /// + /// Whether the data within should be treated as flipped from 's perspective, + /// e.g.: the normals in should be inverted. + /// Use instead. + /// + public bool FlippedManifold { get; init; } + + /// + /// When has a , + /// this is the index of the collider in that collection which collided with. + /// + public int ChildIndexSource { get; init; } + + /// + /// When has a , + /// this is the index of the collider in that collection which collided with. + /// + public int ChildIndexOther { get; init; } + + /// + public Enumerator GetEnumerator() => new(this); + + /// + /// The enumerator for + /// + /// + public ref struct Enumerator(ContactData data) + { + private int _index = -1; + private ContactData _data = data; + + public bool MoveNext() + { + while (_index + 1 < _data.Manifold.Count) + { + _index += 1; + if (_data.Manifold.GetDepth(_index) >= 0) + return true; + } + + return false; + } + + public Contact Current => new(_index, _data); + } + + /// + /// An individual contact + /// + /// + public readonly ref struct Contact(int index, ContactData data) + { + public int Index { get; } = index; + public ContactData Data { get; } = data; + + /// + /// The contact's normal, oriented based on + /// + public Vector3 Normal => Data.FlippedManifold ? -Data.Manifold.GetNormal(Index) : Data.Manifold.GetNormal(Index); + + /// How far the two collidables intersect + public float Depth => Data.Manifold.GetDepth(Index); + + /// The position at which the contact occured + /// This may not be accurate if either collidables are not part of the simulation anymore + public Vector3 Point + { + get + { + // Pose! is not safe as the component may not be part of the physics simulation anymore, but there's no straightforward fix for this; + // We collect contacts during the physics tick, after the tick, we send contact events. + // At that point, both objects may not be at the same position they made contact at, + // so we can't make this more robust by storing the position they were at on contact within the physics tick. + if (Data.FlippedManifold) + return Data.Other.Pose!.Value.Position + Data.Manifold.GetOffset(Index); + return Data.EventSource.Pose!.Value.Position + Data.Manifold.GetOffset(Index); + } + } + } +} 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 95a3f2dd02..12e45fd42e 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -2,6 +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.Collidables; using BepuPhysics.CollisionDetection; @@ -87,6 +88,7 @@ public bool IsRegistered(CollidableComponent collidable) /// /// Checks if a collidable is registered as a listener. /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] private bool IsRegistered(CollidableReference reference) { if (reference.Mobility == CollidableMobility.Static) @@ -113,11 +115,11 @@ public void ClearCollisionsOf(CollidableComponent collidable) if (!ReferenceEquals(pair.A, collidable) && !ReferenceEquals(pair.B, collidable)) continue; - ClearCollision(pair, ref manifold, 0); + ClearCollision(pair, in manifold); } } - private unsafe void ClearCollision(OrderedPair pair, ref EmptyManifold manifold, int workerIndex) + private void ClearCollision(OrderedPair pair, in EmptyManifold manifold) { const bool flippedManifold = false; // The flipped manifold argument does not make sense in this context given that we pass an empty one #if DEBUG @@ -128,240 +130,150 @@ private unsafe void ClearCollision(OrderedPair pair, ref EmptyManifold manifold, _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); + { + state.HandlerA?.OnStoppedTouching( + new ContactData + { + EventSource = pair.A, + Other = pair.B, + Manifold = manifold, + FlippedManifold = flippedManifold, + ChildIndexSource = 0, + ChildIndexOther = 0, + Simulation = _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); + if (state.TryClear(Events.TouchingB)) + { + state.HandlerB?.OnStoppedTouching( + new ContactData + { + EventSource = pair.B, + Other = pair.A, + Manifold = manifold, + FlippedManifold = flippedManifold, + ChildIndexSource = 0, + ChildIndexOther = 0, + Simulation = _simulation, + }); + } _outdatedPairs.Remove(pair); } - public void HandleManifold(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void HandleManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { bool aListener = IsRegistered(pair.A); bool bListener = IsRegistered(pair.B); if (aListener == false && bListener == false) return; - IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.B)); + IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.B), childIndexA, childIndexB); } - private unsafe void RunManifoldEvent(int workerIndex, CollidableComponent a, CollidableComponent b, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + private void RunManifoldEvent(CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB, TManifold manifold) where TManifold : unmanaged, IContactManifold { - 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; - (a, b) = (orderedPair.A, orderedPair.B); - IContactEventHandler? handlerA; - IContactEventHandler? handlerB; + var contactDataForA = new ContactData + { + EventSource = a, + Other = b, + Manifold = manifold, + FlippedManifold = aFlipped, + ChildIndexSource = childIndexA, + ChildIndexOther = childIndexB, + Simulation = _simulation, + }; + + var contactDataForB = new ContactData + { + EventSource = b, + Other = a, + Manifold = manifold, + FlippedManifold = bFlipped, + ChildIndexSource = childIndexB, + ChildIndexOther = childIndexA, + Simulation = _simulation, + }; + + IContactHandler? handlerA, 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) - { - if (manifold.GetDepth(contactIndex) < 0) - continue; + } + else + { + collisionState.Alive = true; // This is set as a flag to check for removal events + handlerA = collisionState.HandlerA = a.ContactEventHandler; + handlerB = collisionState.HandlerB = b.ContactEventHandler; + } + bool touching = false; + for (int i = 0; i < manifold.Count; ++i) + { + if (manifold.GetDepth(i) >= 0) + { 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; - } - 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, aFlipped, workerIndex, _simulation); - if (collisionState.Alive == false) - return; - handlerB?.OnTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); - if (collisionState.Alive == false) - return; break; } + } - if (touching == false && handlerA is not null && collisionState.TryClear(Events.TouchingA)) + if (touching) + { + if (handlerA is not null && collisionState.TrySet(Events.TouchingA)) { - handlerA.OnStoppedTouching(a, b, ref manifold, aFlipped, workerIndex, _simulation); + handlerA.OnStartedTouching(contactDataForA); if (collisionState.Alive == false) return; } - if (touching == false && handlerB is not null && collisionState.TryClear(Events.TouchingB)) + if (handlerB is not null && collisionState.TrySet(Events.TouchingB)) { - handlerB.OnStoppedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); + handlerB.OnStartedTouching(contactDataForB); if (collisionState.Alive == false) return; } - 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 + if (handlerA is not null) { - 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; - } - } - - 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); + handlerA.OnTouching(contactDataForA); if (collisionState.Alive == false) return; } - while (toAdd != 0) + if (handlerB is not null) { - 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); + handlerB.OnTouching(contactDataForB); if (collisionState.Alive == false) return; } } else { - 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)) + if (handlerA is not null && collisionState.TryClear(Events.TouchingA)) { - handlerA.OnPairCreated(a, b, ref manifold, aFlipped, workerIndex, _simulation); + handlerA.OnStoppedTouching(contactDataForA); if (collisionState.Alive == false) return; } - if (handlerB is not null && collisionState.TrySet(Events.CreatedB)) + if (handlerB is not null && collisionState.TryClear(Events.TouchingB)) { - handlerB.OnPairCreated(b, a, ref manifold, bFlipped, workerIndex, _simulation); + handlerB.OnStoppedTouching(contactDataForB); if (collisionState.Alive == false) return; } - - for (int i = 0; i < manifold.Count; ++i) - { - 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; - } - - for (int i = 0; i < manifold.Count; ++i) - { - 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; - } - } - - 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); @@ -379,7 +291,7 @@ public void Flush() //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 in _outdatedPairs) - ClearCollision(pair, ref manifold, 0); + ClearCollision(pair, in manifold); } /// @@ -410,7 +322,7 @@ private interface IPerTypeManifoldStore void ClearEventsOf(CollidableComponent collidableComponent); - public static unsafe void StoreManifold(IPerTypeManifoldStore[][] manifoldLists, int workerIndex, ref TManifold manifold, CollidableComponent a, CollidableComponent b) where TManifold : unmanaged, IContactManifold + public static unsafe void StoreManifold(IPerTypeManifoldStore[][] manifoldLists, int workerIndex, ref TManifold manifold, CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB) where TManifold : unmanaged, IContactManifold { var manifoldsForWorker = manifoldLists[workerIndex]; int typeIndex = TypeIndex.Index; @@ -431,7 +343,7 @@ public static unsafe void StoreManifold(IPerTypeManifoldStore[][] man } var handler = (ListOf)manifoldsForWorker[typeIndex]; - handler.Add((manifold, a, b)); + handler.Add((manifold, a, b, childIndexA, childIndexB)); } private static int indexMax = -1; @@ -459,15 +371,16 @@ static unsafe TypeIndex() private static ListOf ManifoldCtor() => new(); } - private class ListOf : List<(TManifold manifold, CollidableComponent a, CollidableComponent b)>, IPerTypeManifoldStore where TManifold : unmanaged, IContactManifold + private class ListOf : List<(TManifold manifold, CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB)>, IPerTypeManifoldStore where TManifold : unmanaged, IContactManifold { public void RunEvents(ContactEventsManager eventsManager) { - var spanOfThis = CollectionsMarshal.AsSpan(this); - for (int i = spanOfThis.Length - 1; i >= 0; i--) // reverse as the scope may end up calling ClearRelatedContacts + for (int i = Count - 1; i >= 0; i--) // reverse as the scope may end up calling ClearRelatedContacts { - var (manifold, a, b) = spanOfThis[i]; - eventsManager.RunManifoldEvent(0, a, b, ref manifold); + var (manifold, a, b, childIndexA, childIndexB) = this[i]; + eventsManager.RunManifoldEvent(a, b, childIndexA, childIndexB, manifold); + if (i > Count) // If the method above ended up removing a significant amount of events, make sure to continue from a sane spot + i = Count; } Clear(); @@ -485,21 +398,11 @@ public void ClearEventsOf(CollidableComponent collidableComponent) } } - private unsafe struct LastCollisionState + private struct LastCollisionState { - public const int FeatureCount = 4; - - public IContactEventHandler? HandlerA, HandlerB; + public IContactHandler? 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) { @@ -527,10 +430,8 @@ public bool TryClear(Events e) [Flags] private enum Events { - CreatedA = 0b0001, - CreatedB = 0b0010, - TouchingA = 0b0100, - TouchingB = 0b1000, + TouchingA = 0b01, + TouchingB = 0b10, } private readonly record struct OrderedPair 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 5768ddd0eb..06eda97517 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs @@ -1,6 +1,7 @@ // Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. +using System.Collections.Generic; using BepuPhysics.CollisionDetection; namespace Stride.BepuPhysics.Definitions.Contacts; @@ -8,12 +9,14 @@ namespace Stride.BepuPhysics.Definitions.Contacts; /// /// Implements handlers for various collision events. /// -public interface IContactEventHandler +[Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, update your contact methods when migrating to this new class", true)] +public interface IContactEventHandler : IContactHandler { /// /// Whether the object this is attached to should let colliders pass through it /// - public bool NoContactResponse { get; } + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this property will never be called")] + public new bool NoContactResponse { get; } /// /// Fires when a contact is added. @@ -30,6 +33,7 @@ public interface IContactEventHandler /// Index of the new contact in the contact manifold. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnContactAdded(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -50,6 +54,7 @@ void OnContactAdded(CollidableComponent eventSource, CollidableCompon /// Feature id of the contact that was removed and is no longer present in the contact manifold. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnContactRemoved(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int removedFeatureId, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -64,6 +69,7 @@ void OnContactRemoved(CollidableComponent eventSource, CollidableComp /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -78,6 +84,7 @@ void OnStartedTouching(CollidableComponent eventSource, CollidableCom /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -93,6 +100,7 @@ void OnTouching(CollidableComponent eventSource, CollidableComponent /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -108,6 +116,7 @@ void OnStoppedTouching(CollidableComponent eventSource, CollidableCom /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnPairCreated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -122,6 +131,7 @@ void OnPairCreated(CollidableComponent eventSource, CollidableCompone /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] void OnPairUpdated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -132,7 +142,8 @@ void OnPairUpdated(CollidableComponent eventSource, CollidableCompone /// Collidable that the event was attached to. /// Other collider collided with. /// The simulation where the contact occured. - void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + new void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) { } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs new file mode 100644 index 0000000000..23f96c2cfb --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs @@ -0,0 +1,42 @@ +// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) +// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. + +using BepuPhysics.CollisionDetection; + +namespace Stride.BepuPhysics.Definitions.Contacts; + +public interface IContactHandler +{ + /// + /// Whether the object this is attached to should let colliders pass through it + /// + public bool NoContactResponse { get; } + + /// + /// Fires the first time a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. + /// + /// Type of the contact manifold detected. + /// Data associated with this contact event. + void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + { + } + + /// + /// Fires whenever a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. Will not fire for sleeping pairs. + /// + /// Type of the contact manifold detected. + /// Data associated with this contact event. + void OnTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + { + } + + + /// + /// Fires when a pair stops touching. Touching means that there are contacts with nonnegative depths in the manifold. + /// + /// Type of the contact manifold detected. + /// Data associated with this contact event. + void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + { + } +} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs index 730f6c3900..05182ad5bb 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs @@ -51,7 +51,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. var a = collidableMaterials[pair.A]; @@ -59,7 +59,7 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab pairMaterial.FrictionCoefficient = a.FrictionCoefficient * b.FrictionCoefficient; pairMaterial.MaximumRecoveryVelocity = MathF.Max(a.MaximumRecoveryVelocity, b.MaximumRecoveryVelocity); pairMaterial.SpringSettings = pairMaterial.MaximumRecoveryVelocity == a.MaximumRecoveryVelocity ? a.SpringSettings : b.SpringSettings; - contactEvents.HandleManifold(workerIndex, pair, ref manifold); + contactEvents.HandleManifold(workerIndex, pair, 0, 0, ref manifold); if (a.IsTrigger || b.IsTrigger) { @@ -72,6 +72,7 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { + contactEvents.HandleManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); return true; } From 058859f8ffe7dc3b360cf1b7487db066a69c98c4 Mon Sep 17 00:00:00 2001 From: Eideren Date: Fri, 19 Sep 2025 19:39:47 +0200 Subject: [PATCH 02/13] Update samples --- .../BepuSample.Game/Components/Utils/CollisionComponent.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs b/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs index 6d4c669fe3..9b26049195 100644 --- a/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs +++ b/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs @@ -41,17 +41,17 @@ public override void Update() } } - public class MyCustomContactEventHandler : IContactEventHandler + public class MyCustomContactEventHandler : IContactHandler { public bool Contact { get; private set; } = false; public bool NoContactResponse => false; - void IContactEventHandler.OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) + void IContactHandler.OnStartedTouching(ContactData contactData) { Contact = true; } - void IContactEventHandler.OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) + void IContactHandler.OnStoppedTouching(ContactData contactData) { Contact = false; } From f15df981dd531751bd5bb44ca40b551971e2c1a4 Mon Sep 17 00:00:00 2001 From: Eideren Date: Fri, 19 Sep 2025 19:41:25 +0200 Subject: [PATCH 03/13] Update Trigger and CharacterComponent --- .../Stride.BepuPhysics/CharacterComponent.cs | 75 ++++++++++++++----- .../Definitions/Contacts/ContactData.cs | 3 + .../Stride.BepuPhysics/Definitions/Trigger.cs | 20 ++--- 3 files changed, 70 insertions(+), 28 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs index 67e92c0cf6..ae91c2fca9 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs @@ -17,7 +17,7 @@ namespace Stride.BepuPhysics; [ComponentCategory("Physics - Bepu")] -public class CharacterComponent : BodyComponent, ISimulationUpdate, IContactEventHandler +public class CharacterComponent : BodyComponent, ISimulationUpdate, IContactHandler { private bool _jumping; @@ -162,38 +162,77 @@ protected bool GroundTest(NVector3 groundNormal, float threshold = 0f) return false; } - bool IContactEventHandler.NoContactResponse => NoContactResponse; - void IContactEventHandler.OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) => OnStartedTouching(eventSource, other, ref contactManifold, flippedManifold, contactIndex, bepuSimulation); - void IContactEventHandler.OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) => OnStoppedTouching(eventSource, other, ref contactManifold, flippedManifold, contactIndex, bepuSimulation); + bool IContactHandler.NoContactResponse => NoContactResponse; + void IContactHandler.OnStartedTouching(ContactData contactData) => OnStartedTouching(contactData); + void IContactHandler.OnTouching(ContactData contactData) => OnTouching(contactData); + void IContactHandler.OnStoppedTouching(ContactData contactData) => OnStoppedTouching(contactData); protected bool NoContactResponse => false; - /// - protected virtual void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + /// + protected virtual void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold { - contactManifold.GetContact(contactIndex, out var contact); + foreach (var contact in contactData) + { + Contacts.Add((contactData.Other, new Contact + { + Normal = contact.Normal, + Depth = contact.Depth, + FeatureId = contact.FeatureId, + Offset = contact.Point - (Vector3)contactData.EventSource.Pose!.Value.Position, + })); + } + } - if (flippedManifold) + /// + protected virtual void OnTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + { + int contactsRetained = 0; + for (int i = Contacts.Count - 1; i >= 0; i--) { - // Contact manifold was computed from the other collidable's point of view, normal and offset should be flipped - contact.Offset = -contact.Offset; - contact.Normal = -contact.Normal; + var contact = Contacts[i]; + if (contact.Source != contactData.Other) + { + foreach (var newContact in contactData) + { + if (newContact.FeatureId == contact.Contact.FeatureId) + { + contactsRetained |= 1 << newContact.Index; + goto RETAIN_CONTACT; + } + } + + Contacts.RemoveAt(i); + } + + RETAIN_CONTACT: + { + } } - contact.Offset = contact.Offset + Entity.Transform.WorldMatrix.TranslationVector.ToNumeric() + CenterOfMass.ToNumeric(); - Contacts.Add((other, contact)); + foreach (var contact in contactData) + { + if ((contactsRetained & (1 << contact.Index)) == 0) + { + Contacts.Add((contactData.Other, new Contact + { + Normal = contact.Normal, + Depth = contact.Depth, + FeatureId = contact.FeatureId, + Offset = contact.Point - (Vector3)contactData.EventSource.Pose!.Value.Position, + })); + } + } } - /// - protected virtual void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + /// + protected virtual void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold { for (int i = Contacts.Count - 1; i >= 0; i--) { - if (Contacts[i].Source == other) + if (Contacts[i].Source == contactData.Other) Contacts.SwapRemoveAt(i); } } } - - diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs index 436b787ac7..f629996b58 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs @@ -123,5 +123,8 @@ public Vector3 Point return Data.EventSource.Pose!.Value.Position + Data.Manifold.GetOffset(Index); } } + + /// Gets the feature id associated with this contact + public int FeatureId => Data.Manifold.GetFeatureId(Index); } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs index ba3fe0bf44..84950bd379 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs @@ -1,7 +1,7 @@ // Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. -using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; using Stride.BepuPhysics.Definitions.Contacts; using Stride.Core; @@ -13,23 +13,23 @@ namespace Stride.BepuPhysics.Definitions; /// A contact event handler without collision response, which runs delegates on enter and exit /// [DataContract] -public class Trigger : IContactEventHandler +public class Trigger : IContactHandler { public bool NoContactResponse => true; public event TriggerDelegate? OnEnter, OnLeave; - void IContactEventHandler.OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) => OnStartedTouching(eventSource, other, ref contactManifold, flippedManifold, contactIndex, bepuSimulation); - void IContactEventHandler.OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) => OnStoppedTouching(eventSource, other, ref contactManifold, flippedManifold, contactIndex, bepuSimulation); + void IContactHandler.OnStartedTouching(ContactData contactData) => OnStartedTouching(contactData); + void IContactHandler.OnStoppedTouching(ContactData contactData) => OnStoppedTouching(contactData); - /// - protected void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) + /// + protected void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold { - OnEnter?.Invoke(eventSource, other); + OnEnter?.Invoke(contactData.EventSource, contactData.Other); } - /// - protected void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) + /// + protected void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold { - OnLeave?.Invoke(eventSource, other); + OnLeave?.Invoke(contactData.EventSource, contactData.Other); } } From d88d8da2f4542d23a711f075169e2d9ecb204678 Mon Sep 17 00:00:00 2001 From: Eideren Date: Fri, 19 Sep 2025 19:41:34 +0200 Subject: [PATCH 04/13] Update tests --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 56 ++++--------------- 1 file changed, 10 insertions(+), 46 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 7acfd0033c..db0d562ece 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -164,19 +164,15 @@ public static void OnTriggerRemovalTest() { game.ScreenShotAutomationEnabled = false; - int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0; + int 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; + trigger.StartedTouching += () => e1.Scene = null; e1.Transform.Position.Y = 3; @@ -184,15 +180,11 @@ public static void OnTriggerRemovalTest() var simulation = e1.GetSimulation(); - while (pairEnded == 0) + while (stoppedTouching == 0) await simulation.AfterUpdate(); - Assert.Equal(1, pairCreated); - Assert.Equal(0, contactAdded); - Assert.Equal(0, startedTouching); + Assert.Equal(1, startedTouching); - Assert.Equal(pairCreated, pairEnded); - Assert.Equal(contactAdded, contactRemoved); Assert.Equal(startedTouching, stoppedTouching); game.Exit(); @@ -208,14 +200,10 @@ public static void OnTriggerTest() { game.ScreenShotAutomationEnabled = false; - int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0; + int 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++; @@ -225,15 +213,11 @@ public static void OnTriggerTest() var simulation = e1.GetSimulation(); - while (pairEnded == 0) + while (stoppedTouching == 0) await simulation.AfterUpdate(); - Assert.Equal(1, pairCreated); - Assert.NotEqual(0, contactAdded); Assert.Equal(1, startedTouching); - Assert.Equal(pairCreated, pairEnded); - Assert.Equal(contactAdded, contactRemoved); Assert.Equal(startedTouching, stoppedTouching); game.Exit(); @@ -281,41 +265,21 @@ int TestRemovalUnsafe(BepuSimulation simulation) } } - private class Trigger : IContactEventHandler + private class Trigger : IContactHandler { public bool NoContactResponse => true; - public event Action? ContactAdded, ContactRemoved, StartedTouching, StoppedTouching, PairCreated, PairEnded; + public event Action? StartedTouching, StoppedTouching; - public void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + public void OnStartedTouching(ContactData manifold) 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 + public void OnStoppedTouching(ContactData manifold) 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(); - } } } } From 0d23f2f08ad3fb565134afd784be8a4d4168f461 Mon Sep 17 00:00:00 2001 From: Eideren Date: Tue, 23 Sep 2025 13:30:12 +0200 Subject: [PATCH 05/13] Fix ChildIndexSource when manifold is flipped --- .../Definitions/Contacts/ContactEventsManager.cs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 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 12e45fd42e..2523ea9c8f 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -180,8 +180,11 @@ private void RunManifoldEvent(CollidableComponent a, CollidableCompon 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; - (a, b) = (orderedPair.A, orderedPair.B); + if (aFlipped) + { + (childIndexA, childIndexB) = (childIndexB, childIndexA); + (a, b) = (b, a); + } var contactDataForA = new ContactData { @@ -199,7 +202,7 @@ private void RunManifoldEvent(CollidableComponent a, CollidableCompon EventSource = b, Other = a, Manifold = manifold, - FlippedManifold = bFlipped, + FlippedManifold = !aFlipped, ChildIndexSource = childIndexB, ChildIndexOther = childIndexA, Simulation = _simulation, From 67a22b5e9b4551003fe9e59c2e329c72635622cc Mon Sep 17 00:00:00 2001 From: Eideren Date: Wed, 24 Sep 2025 19:50:50 +0200 Subject: [PATCH 06/13] ContactGroup: Group multiple contacts generated from compound shapes into the Contacts passed to On*Touching --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 4 +- .../Stride.BepuPhysics/BepuSimulation.cs | 9 - .../Stride.BepuPhysics/BodyComponent.cs | 8 - .../Stride.BepuPhysics/CharacterComponent.cs | 58 ++---- .../Stride.BepuPhysics/CollidableComponent.cs | 15 +- .../Definitions/Contacts/Contact.cs | 75 ++++++++ .../Definitions/Contacts/ContactData.cs | 130 ------------- .../Contacts/ContactEventsManager.cs | 171 ++++++++---------- .../Definitions/Contacts/ContactGroup.cs | 66 +++++++ .../Definitions/Contacts/Contacts.cs | 91 ++++++++++ .../Definitions/Contacts/IContactHandler.cs | 12 +- .../Stride.BepuPhysics/Definitions/Trigger.cs | 12 +- .../Stride.BepuPhysics/StaticComponent.cs | 8 - .../Systems/CollidableProcessor.cs | 2 +- 14 files changed, 348 insertions(+), 313 deletions(-) create mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs delete mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs create mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactGroup.cs create mode 100644 sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index db0d562ece..26bf49664a 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -271,12 +271,12 @@ private class Trigger : IContactHandler public event Action? StartedTouching, StoppedTouching; - public void OnStartedTouching(ContactData manifold) where TManifold : unmanaged, IContactManifold + public void OnStartedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { StartedTouching?.Invoke(); } - public void OnStoppedTouching(ContactData manifold) where TManifold : unmanaged, IContactManifold + public void OnStoppedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { StoppedTouching?.Invoke(); } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs index fdc28f5fad..beec4a61f9 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs @@ -54,9 +54,6 @@ public sealed class BepuSimulation : IDisposable internal List Bodies { get; } = new(); internal List Statics { get; } = new(); - /// Required when a component is removed from the simulation and must have its contacts flushed - internal (int value, CollidableComponent? component) TemporaryDetachedLookup { get; set; } - /// [DataMemberIgnore] public CollisionMatrix CollisionMatrix = CollisionMatrix.All; // Keep this as a field, user need ref access for writes @@ -322,9 +319,6 @@ public CollidableComponent GetComponent(CollidableReference collidable) [MethodImpl(MethodImplOptions.AggressiveInlining)] public BodyComponent GetComponent(BodyHandle handle) { - if (TemporaryDetachedLookup.component is BodyComponent detachedBody && handle.Value == TemporaryDetachedLookup.value) - return detachedBody; - var body = Bodies[handle.Value]; Debug.Assert(body is not null, "Handle is invalid, Bepu's array indexing strategy might have changed under us"); return body; @@ -332,9 +326,6 @@ public BodyComponent GetComponent(BodyHandle handle) public StaticComponent GetComponent(StaticHandle handle) { - if (TemporaryDetachedLookup.component is StaticComponent detachedStatic && handle.Value == TemporaryDetachedLookup.value) - return detachedStatic; - var statics = Statics[handle.Value]; Debug.Assert(statics is not null, "Handle is invalid, Bepu's array indexing strategy might have changed under us"); return statics; diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs index 42d767315b..6db5a6a715 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs @@ -488,14 +488,6 @@ protected override void DetachInner() BodyReference = null; } - protected override int GetHandleValue() - { - if (BodyReference is { } bRef) - return bRef.Handle.Value; - - throw new InvalidOperationException(); - } - /// /// A special variant taking the center of mass into consideration /// diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs index ae91c2fca9..03c53d3dec 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CharacterComponent.cs @@ -163,75 +163,55 @@ protected bool GroundTest(NVector3 groundNormal, float threshold = 0f) } bool IContactHandler.NoContactResponse => NoContactResponse; - void IContactHandler.OnStartedTouching(ContactData contactData) => OnStartedTouching(contactData); - void IContactHandler.OnTouching(ContactData contactData) => OnTouching(contactData); - void IContactHandler.OnStoppedTouching(ContactData contactData) => OnStoppedTouching(contactData); + void IContactHandler.OnStartedTouching(Contacts contacts) => OnStartedTouching(contacts); + void IContactHandler.OnTouching(Contacts contacts) => OnTouching(contacts); + void IContactHandler.OnStoppedTouching(Contacts contacts) => OnStoppedTouching(contacts); protected bool NoContactResponse => false; /// - protected virtual void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + protected virtual void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - foreach (var contact in contactData) + foreach (var contact in contacts) { - Contacts.Add((contactData.Other, new Contact + Contacts.Add((contacts.Other, new Contact { Normal = contact.Normal, Depth = contact.Depth, FeatureId = contact.FeatureId, - Offset = contact.Point - (Vector3)contactData.EventSource.Pose!.Value.Position, + Offset = contact.Point - (Vector3)contacts.EventSource.Pose!.Value.Position, })); } } /// - protected virtual void OnTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + protected virtual void OnTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - int contactsRetained = 0; for (int i = Contacts.Count - 1; i >= 0; i--) { - var contact = Contacts[i]; - if (contact.Source != contactData.Other) - { - foreach (var newContact in contactData) - { - if (newContact.FeatureId == contact.Contact.FeatureId) - { - contactsRetained |= 1 << newContact.Index; - goto RETAIN_CONTACT; - } - } - - Contacts.RemoveAt(i); - } - - RETAIN_CONTACT: - { - } + if (Contacts[i].Source == contacts.Other) + Contacts.SwapRemoveAt(i); } - foreach (var contact in contactData) + foreach (var contact in contacts) { - if ((contactsRetained & (1 << contact.Index)) == 0) + Contacts.Add((contacts.Other, new Contact { - Contacts.Add((contactData.Other, new Contact - { - Normal = contact.Normal, - Depth = contact.Depth, - FeatureId = contact.FeatureId, - Offset = contact.Point - (Vector3)contactData.EventSource.Pose!.Value.Position, - })); - } + Normal = contact.Normal, + Depth = contact.Depth, + FeatureId = contact.FeatureId, + Offset = contact.Point - (Vector3)contacts.EventSource.Pose!.Value.Position, + })); } } /// - protected virtual void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + protected virtual void OnStoppedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { for (int i = Contacts.Count - 1; i >= 0; i--) { - if (Contacts[i].Source == contactData.Other) + if (Contacts[i].Source == contacts.Other) Contacts.SwapRemoveAt(i); } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs index b76e206b50..5c1ef9bc11 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/CollidableComponent.cs @@ -263,7 +263,7 @@ internal void TryUpdateFeatures() internal void ReAttach(BepuSimulation onSimulation) { Versioning = Interlocked.Increment(ref VersioningCounter); - Detach(true); + Detach(); Debug.Assert(Processor is not null); @@ -291,12 +291,12 @@ internal void ReAttach(BepuSimulation onSimulation) Processor?.OnPostAdd?.Invoke(this); } - internal void Detach(bool reAttaching) + internal void Detach() { if (Simulation is null) return; - int getHandleValue = GetHandleValue(); + uint handleValue = CollidableReference!.Value.Packed; Versioning = Interlocked.Increment(ref VersioningCounter); Processor?.OnPreRemove?.Invoke(this); @@ -315,12 +315,7 @@ internal void Detach(bool reAttaching) } DetachInner(); - if (reAttaching == false) - { - Simulation.TemporaryDetachedLookup = (getHandleValue, this); - Simulation.ContactEvents.ClearCollisionsOf(this); // Ensure that removing this collidable sends the appropriate contact events to listeners - Simulation.TemporaryDetachedLookup = (-1, null); - } + Simulation.ContactEvents.ClearCollisionsOf(this, handleValue); // Ensure that removing this collidable sends the appropriate contact events to listeners Simulation = null; } @@ -363,8 +358,6 @@ protected void TryUpdateMaterialProperties() /// protected abstract void DetachInner(); - protected abstract int GetHandleValue(); - protected void RegisterContactHandler() { diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs new file mode 100644 index 0000000000..7d8cb24180 --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs @@ -0,0 +1,75 @@ +// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) +// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. + +using BepuPhysics.CollisionDetection; +using Stride.Core.Mathematics; + +namespace Stride.BepuPhysics.Definitions.Contacts; + +/// +/// An individual contact +/// +/// +public ref struct Contact where TManifold : unmanaged, IContactManifold +{ + /// + /// The index used when reading into this group's manifold to retrieve this contact + /// + public readonly int Index; + + /// + /// The contact info pair this contact is a part of + /// + public readonly Contacts Contacts; + + // This is not readonly specifically because we're calling instance method on this + // object which may cause the JIT to do a copy before each call + /// + /// The group this contact is a part of + /// + public ContactGroup ContactGroup; + + internal Contact(int index, Contacts contacts, in ContactGroup contactGroup) + { + Index = index; + Contacts = contacts; + ContactGroup = contactGroup; + } + + /// How far the two collidables intersect + public float Depth => ContactGroup.Manifold.GetDepth(Index); + + /// Gets the feature id associated with this contact + public int FeatureId => ContactGroup.Manifold.GetFeatureId(Index); + + /// + /// The contact's normal, oriented based on + /// + public Vector3 Normal => Contacts.IsSourceOriginalA ? -ContactGroup.Manifold.GetNormal(Index) : ContactGroup.Manifold.GetNormal(Index); + + /// + /// When has a , + /// this is the index of the collider in that collection which collided with. + /// + public int SourceChildIndex => Contacts.IsSourceOriginalA ? ContactGroup.ChildIndexA : ContactGroup.ChildIndexB; + + /// + /// When has a , + /// this is the index of the collider in that collection which collided with. + /// + public int OtherChildIndex => Contacts.IsSourceOriginalA ? ContactGroup.ChildIndexB : ContactGroup.ChildIndexA; + + /// The position at which the contact occured + /// This may not be accurate if either collidables are not part of the simulation anymore + public Vector3 Point + { + get + { + // Pose! is not safe as the component may not be part of the physics simulation anymore, but there's no straightforward fix for this; + // We collect contacts during the physics tick, after the tick, we send contact events. + // At that point, both objects may not be at the same position they made contact at, + // so we can't make this more robust by storing the position they were at on contact within the physics tick. + return (Contacts.IsSourceOriginalA ? Contacts.EventSource.Pose!.Value.Position : Contacts.Other.Pose!.Value.Position) + ContactGroup.Manifold.GetOffset(Index); + } + } +} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs deleted file mode 100644 index f629996b58..0000000000 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactData.cs +++ /dev/null @@ -1,130 +0,0 @@ -// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) -// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. - -using BepuPhysics.CollisionDetection; -using Stride.Core.Mathematics; - -namespace Stride.BepuPhysics.Definitions.Contacts; - -/// -/// Enumerate over this structure to get individual contacts -/// -/// -/// (ContactData contactData) where TManifold : unmanaged, IContactManifold -/// { -/// foreach (var contact in contactData) -/// { -/// contact.Normal ... -/// } -/// } -/// ]]> -/// -public readonly ref struct ContactData where TManifold : unmanaged, IContactManifold -{ - /// - /// The collidable which is bound to this - /// - public CollidableComponent EventSource { get; init; } - - /// - /// The other collidable - /// - public CollidableComponent Other { get; init; } - - /// - /// The simulation this contact occured in - /// - public BepuSimulation Simulation { get; init; } - - /// - /// The raw contact manifold - /// - /// - /// Make sure that you understand and handle before using this property - /// - public TManifold Manifold { get; init; } - - /// - /// Whether the data within should be treated as flipped from 's perspective, - /// e.g.: the normals in should be inverted. - /// Use instead. - /// - public bool FlippedManifold { get; init; } - - /// - /// When has a , - /// this is the index of the collider in that collection which collided with. - /// - public int ChildIndexSource { get; init; } - - /// - /// When has a , - /// this is the index of the collider in that collection which collided with. - /// - public int ChildIndexOther { get; init; } - - /// - public Enumerator GetEnumerator() => new(this); - - /// - /// The enumerator for - /// - /// - public ref struct Enumerator(ContactData data) - { - private int _index = -1; - private ContactData _data = data; - - public bool MoveNext() - { - while (_index + 1 < _data.Manifold.Count) - { - _index += 1; - if (_data.Manifold.GetDepth(_index) >= 0) - return true; - } - - return false; - } - - public Contact Current => new(_index, _data); - } - - /// - /// An individual contact - /// - /// - public readonly ref struct Contact(int index, ContactData data) - { - public int Index { get; } = index; - public ContactData Data { get; } = data; - - /// - /// The contact's normal, oriented based on - /// - public Vector3 Normal => Data.FlippedManifold ? -Data.Manifold.GetNormal(Index) : Data.Manifold.GetNormal(Index); - - /// How far the two collidables intersect - public float Depth => Data.Manifold.GetDepth(Index); - - /// The position at which the contact occured - /// This may not be accurate if either collidables are not part of the simulation anymore - public Vector3 Point - { - get - { - // Pose! is not safe as the component may not be part of the physics simulation anymore, but there's no straightforward fix for this; - // We collect contacts during the physics tick, after the tick, we send contact events. - // At that point, both objects may not be at the same position they made contact at, - // so we can't make this more robust by storing the position they were at on contact within the physics tick. - if (Data.FlippedManifold) - return Data.Other.Pose!.Value.Position + Data.Manifold.GetOffset(Index); - return Data.EventSource.Pose!.Value.Position + Data.Manifold.GetOffset(Index); - } - } - - /// Gets the feature id associated with this contact - public int FeatureId => Data.Manifold.GetFeatureId(Index); - } -} 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 2523ea9c8f..85636ff1d9 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -1,6 +1,7 @@ // Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -71,7 +72,7 @@ public void Unregister(CollidableComponent collidable) else _bodyListenerFlags.Remove(reference.RawHandleValue); - ClearCollisionsOf(collidable); + ClearCollisionsOf(collidable, reference.Packed); } /// @@ -97,31 +98,29 @@ private bool IsRegistered(CollidableReference reference) return _bodyListenerFlags.Contains(reference.RawHandleValue); } - public void ClearCollisionsOf(CollidableComponent collidable) + public void ClearCollisionsOf(CollidableComponent collidable, uint packed) { foreach (var workerStore in _manifoldStoresPerWorker) { foreach (var typeStore in workerStore) - typeStore.ClearEventsOf(collidable); + typeStore.ClearEventsOf(collidable, packed); } // 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) { if (!ReferenceEquals(pair.A, collidable) && !ReferenceEquals(pair.B, collidable)) continue; - ClearCollision(pair, in manifold); + ClearCollision(pair); } } - private void ClearCollision(OrderedPair pair, in EmptyManifold manifold) + private void ClearCollision(OrderedPair pair) { - 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); @@ -132,32 +131,14 @@ private void ClearCollision(OrderedPair pair, in EmptyManifold manifold) if (state.TryClear(Events.TouchingA)) { - state.HandlerA?.OnStoppedTouching( - new ContactData - { - EventSource = pair.A, - Other = pair.B, - Manifold = manifold, - FlippedManifold = flippedManifold, - ChildIndexSource = 0, - ChildIndexOther = 0, - Simulation = _simulation, - }); + var contactDataForA = new Contacts(pair.A, pair.B, isSourceOriginalA: true, ReadOnlySpan>.Empty, _simulation); + state.HandlerA?.OnStoppedTouching(contactDataForA); } if (state.TryClear(Events.TouchingB)) { - state.HandlerB?.OnStoppedTouching( - new ContactData - { - EventSource = pair.B, - Other = pair.A, - Manifold = manifold, - FlippedManifold = flippedManifold, - ChildIndexSource = 0, - ChildIndexOther = 0, - Simulation = _simulation, - }); + var contactDataForB = new Contacts(pair.B, pair.A, isSourceOriginalA: false, ReadOnlySpan>.Empty, _simulation); + state.HandlerB?.OnStoppedTouching(contactDataForB); } _outdatedPairs.Remove(pair); @@ -171,42 +152,20 @@ public void HandleManifold(int workerIndex, CollidablePair pair, int if (aListener == false && bListener == false) return; - IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, _simulation.GetComponent(pair.A), _simulation.GetComponent(pair.B), childIndexA, childIndexB); + IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, pair, childIndexA, childIndexB); } - private void RunManifoldEvent(CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB, TManifold manifold) where TManifold : unmanaged, IContactManifold + private void RunManifoldEvent(Span> unsafeInfos) where TManifold : unmanaged, IContactManifold { - // 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); + // We have to do a stackalloc'ed copy as On*Touching may end up clearing the memory region where unsafeInfos resides through ClearEventsOf + Span> safeInfos = stackalloc ContactGroup[unsafeInfos.Length]; + unsafeInfos.CopyTo(safeInfos); - bool aFlipped = ReferenceEquals(a, orderedPair.B); // Whether the manifold is flipped from a's point of view - if (aFlipped) - { - (childIndexA, childIndexB) = (childIndexB, childIndexA); - (a, b) = (b, a); - } + var orderedPair = new OrderedPair(_simulation.GetComponent(safeInfos[0].Pair.A), _simulation.GetComponent(safeInfos[0].Pair.B)); - var contactDataForA = new ContactData - { - EventSource = a, - Other = b, - Manifold = manifold, - FlippedManifold = aFlipped, - ChildIndexSource = childIndexA, - ChildIndexOther = childIndexB, - Simulation = _simulation, - }; - - var contactDataForB = new ContactData - { - EventSource = b, - Other = a, - Manifold = manifold, - FlippedManifold = !aFlipped, - ChildIndexSource = childIndexB, - ChildIndexOther = childIndexA, - Simulation = _simulation, - }; + bool isAOriginalA = safeInfos[0].Pair.A.Packed == safeInfos[0].SortedPair.A; + var contactDataForA = new Contacts(orderedPair.A, orderedPair.B, isSourceOriginalA: isAOriginalA, safeInfos, _simulation); + var contactDataForB = new Contacts(orderedPair.B, orderedPair.A, isSourceOriginalA: isAOriginalA == false, safeInfos, _simulation); IContactHandler? handlerA, handlerB; ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, orderedPair, out bool alreadyExisted); @@ -218,17 +177,20 @@ private void RunManifoldEvent(CollidableComponent a, CollidableCompon else { collisionState.Alive = true; // This is set as a flag to check for removal events - handlerA = collisionState.HandlerA = a.ContactEventHandler; - handlerB = collisionState.HandlerB = b.ContactEventHandler; + handlerA = collisionState.HandlerA = orderedPair.A.ContactEventHandler; + handlerB = collisionState.HandlerB = orderedPair.B.ContactEventHandler; } bool touching = false; - for (int i = 0; i < manifold.Count; ++i) + for (int i = 0; i < safeInfos.Length; i++) { - if (manifold.GetDepth(i) >= 0) + for (int j = 0; j < safeInfos[i].Manifold.Count; ++j) { - touching = true; - break; + if (safeInfos[i].Manifold.GetDepth(j) >= 0) + { + touching = true; + break; + } } } @@ -294,7 +256,7 @@ public void Flush() //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 in _outdatedPairs) - ClearCollision(pair, in manifold); + ClearCollision(pair); } /// @@ -323,9 +285,9 @@ private interface IPerTypeManifoldStore { void RunEvents(ContactEventsManager eventsManager); - void ClearEventsOf(CollidableComponent collidableComponent); + void ClearEventsOf(CollidableComponent collidableComponent, uint packed); - public static unsafe void StoreManifold(IPerTypeManifoldStore[][] manifoldLists, int workerIndex, ref TManifold manifold, CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB) where TManifold : unmanaged, IContactManifold + public static unsafe void StoreManifold(IPerTypeManifoldStore[][] manifoldLists, int workerIndex, ref TManifold manifold, CollidablePair pair, int childIndexA, int childIndexB) where TManifold : unmanaged, IContactManifold { var manifoldsForWorker = manifoldLists[workerIndex]; int typeIndex = TypeIndex.Index; @@ -346,15 +308,30 @@ public static unsafe void StoreManifold(IPerTypeManifoldStore[][] man } var handler = (ListOf)manifoldsForWorker[typeIndex]; - handler.Add((manifold, a, b, childIndexA, childIndexB)); + + var newValue = new ContactGroup(ref manifold, pair, childIndexA, childIndexB); + int index = handler.BinarySearch(newValue, Comparer.SharedInstance); + if (index < 0) + handler.Insert(~index, newValue); + else + handler.Insert(index, newValue); } private static int indexMax = -1; private static unsafe delegate*[] manifoldStoreConstructors = []; private static object perTypeLock = new(); - // On purpose, we want - // ReSharper disable once UnusedTypeParameter + private class Comparer : IComparer> where TManifold : unmanaged, IContactManifold + { + public static Comparer SharedInstance = new(); + + public int Compare(ContactGroup x, ContactGroup y) + { + int aComp = x.SortedPair.A.CompareTo(y.SortedPair.A); + return aComp != 0 ? aComp : x.SortedPair.B.CompareTo(y.SortedPair.B); + } + } + private static class TypeIndex where TManifold : unmanaged, IContactManifold { public static readonly int Index; @@ -374,14 +351,19 @@ static unsafe TypeIndex() private static ListOf ManifoldCtor() => new(); } - private class ListOf : List<(TManifold manifold, CollidableComponent a, CollidableComponent b, int childIndexA, int childIndexB)>, IPerTypeManifoldStore where TManifold : unmanaged, IContactManifold + private class ListOf : List>, IPerTypeManifoldStore where TManifold : unmanaged, IContactManifold { public void RunEvents(ContactEventsManager eventsManager) { for (int i = Count - 1; i >= 0; i--) // reverse as the scope may end up calling ClearRelatedContacts { - var (manifold, a, b, childIndexA, childIndexB) = this[i]; - eventsManager.RunManifoldEvent(a, b, childIndexA, childIndexB, manifold); + var refPair = this[i].SortedPair; + int endExclusive = i + 1; + for (; i > 0 && this[i - 1].SortedPair == refPair; i--){ } // Find the range of collisions sharing the same pair + + var transientSpan = CollectionsMarshal.AsSpan(this)[i..endExclusive]; + + eventsManager.RunManifoldEvent(transientSpan); if (i > Count) // If the method above ended up removing a significant amount of events, make sure to continue from a sane spot i = Count; } @@ -389,12 +371,14 @@ public void RunEvents(ContactEventsManager eventsManager) Clear(); } - public void ClearEventsOf(CollidableComponent collidableComponent) + public void ClearEventsOf(CollidableComponent collidableComponent, uint packed) { + Debug.Assert(collidableComponent.CollidableReference.HasValue); + var spanOfThis = CollectionsMarshal.AsSpan(this); for (int i = spanOfThis.Length - 1; i >= 0; i--) { - if (spanOfThis[i].a == collidableComponent || spanOfThis[i].b == collidableComponent) + if (spanOfThis[i].Pair.A.Packed == packed || spanOfThis[i].Pair.B.Packed == packed) RemoveAt(i); } } @@ -437,22 +421,6 @@ private enum Events TouchingB = 0b10, } - private readonly record struct OrderedPair - { - 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 { @@ -473,3 +441,20 @@ private struct EmptyManifold : IContactManifold public Vector3 GetOffset(int contactIndex) => throw new IndexOutOfRangeException("This manifold is empty"); } } + +internal readonly record struct OrderedPair +{ + public readonly CollidableComponent A, B; + + public OrderedPair(CollidableComponent a, CollidableComponent b) + { + Debug.Assert(a.CollidableReference.HasValue); + Debug.Assert(b.CollidableReference.HasValue); + (A, B) = a.CollidableReference.Value.Packed > b.CollidableReference.Value.Packed ? (a, b) : (b, a); + } + + public static (uint A, uint B) Sort(CollidablePair pair) + { + return pair.A.Packed > pair.B.Packed ? (pair.A.Packed, pair.B.Packed) : (pair.B.Packed, pair.A.Packed); + } +} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactGroup.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactGroup.cs new file mode 100644 index 0000000000..f784f1d2e3 --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactGroup.cs @@ -0,0 +1,66 @@ +// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) +// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. + +using BepuPhysics.CollisionDetection; + +namespace Stride.BepuPhysics.Definitions.Contacts; + +/// +/// A set of contacts generated from two collidables +/// +/// +/// (Contacts contacts) where TManifold : unmanaged, IContactManifold +/// { +/// foreach (var contact in contacts) +/// { +/// contact.ContactGroup ... +/// } +/// // Or +/// foreach (var group in contacts.Groups) +/// { +/// ... +/// } +/// } +/// ]]> +/// +public struct ContactGroup where TManifold : unmanaged, IContactManifold +{ + /// + /// The raw id for the two collidables that generated this contact group + /// + public readonly CollidablePair Pair; + + /// + /// sorted in a deterministic order + /// + public readonly (uint A, uint B) SortedPair; + + /// + /// When has a , + /// this is the index of the collider in that collection which is in contact. + /// + public readonly int ChildIndexA; + + /// + /// When has a , + /// this is the index of the collider in that collection which is in contact. + /// + public readonly int ChildIndexB; + + // This is not readonly specifically because we're calling instance method on this + // object which may cause the JIT to do a copy before each call + /// + /// The manifold associated with this collision + /// + public TManifold Manifold; + + public ContactGroup(ref TManifold manifold, CollidablePair pair, int childIndexA, int childIndexB) + { + Pair = pair; + SortedPair = OrderedPair.Sort(pair); + Manifold = manifold; + ChildIndexA = childIndexA; + ChildIndexB = childIndexB; + } +} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs new file mode 100644 index 0000000000..247e4f4abd --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs @@ -0,0 +1,91 @@ +// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) +// Distributed under the MIT license. See the LICENSE.md file in the project root for more information. + +using BepuPhysics.CollisionDetection; + +namespace Stride.BepuPhysics.Definitions.Contacts; + +/// +/// Enumerate over this structure to get individual contact +/// +/// +/// (Contacts contacts) where TManifold : unmanaged, IContactManifold +/// { +/// foreach (var contact in contacts) +/// { +/// contact.Normal ... +/// } +/// } +/// ]]> +/// +public readonly ref struct Contacts where TManifold : unmanaged, IContactManifold +{ + /// + /// Contact group registered between these two bodies, one per compound child hit + /// + public ReadOnlySpan> Groups { get; } + + /// + /// The simulation this contact occured in + /// + public BepuSimulation Simulation { get; } + + /// + /// Whether maps to the unsorted, original A + /// + public bool IsSourceOriginalA { get; } + + /// + /// The collidable which is bound to this + /// + public CollidableComponent EventSource { get; } + + /// + /// The other collidable + /// + public CollidableComponent Other { get; } + + public Contacts(CollidableComponent source, CollidableComponent other, bool isSourceOriginalA, ReadOnlySpan> groups, BepuSimulation simulation) + { + EventSource = source; + Other = other; + IsSourceOriginalA = isSourceOriginalA; + Groups = groups; + Simulation = simulation; + } + + /// + public Enumerator GetEnumerator() => new(this); + + /// + /// The enumerator for + /// + /// + public ref struct Enumerator(Contacts data) + { + private int _infoIndex = 0; + private int _manifoldIndex = -1; + private Contacts _data = data; + + public bool MoveNext() + { + for (; _infoIndex < _data.Groups.Length; _infoIndex++) + { + var manifold = _data.Groups[_infoIndex].Manifold; + while (_manifoldIndex + 1 < manifold.Count) + { + _manifoldIndex += 1; + if (manifold.GetDepth(_manifoldIndex) >= 0) + return true; + } + + _manifoldIndex = -1; + } + + return false; + } + + public Contact Current => new(_manifoldIndex, _data, in _data.Groups[_infoIndex]); + } +} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs index 23f96c2cfb..f2ca9de9b7 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactHandler.cs @@ -16,8 +16,8 @@ public interface IContactHandler /// Fires the first time a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. /// /// Type of the contact manifold detected. - /// Data associated with this contact event. - void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + /// Data associated with this contact event. + void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { } @@ -25,8 +25,8 @@ void OnStartedTouching(ContactData contactData) where TMan /// Fires whenever a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. Will not fire for sleeping pairs. /// /// Type of the contact manifold detected. - /// Data associated with this contact event. - void OnTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + /// Data associated with this contact event. + void OnTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { } @@ -35,8 +35,8 @@ void OnTouching(ContactData contactData) where TManifold : /// Fires when a pair stops touching. Touching means that there are contacts with nonnegative depths in the manifold. /// /// Type of the contact manifold detected. - /// Data associated with this contact event. - void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + /// Data associated with this contact event. + void OnStoppedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs index 84950bd379..4d58f3475f 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs @@ -18,18 +18,18 @@ public class Trigger : IContactHandler public bool NoContactResponse => true; public event TriggerDelegate? OnEnter, OnLeave; - void IContactHandler.OnStartedTouching(ContactData contactData) => OnStartedTouching(contactData); - void IContactHandler.OnStoppedTouching(ContactData contactData) => OnStoppedTouching(contactData); + void IContactHandler.OnStartedTouching(Contacts contacts) => OnStartedTouching(contacts); + void IContactHandler.OnStoppedTouching(Contacts contacts) => OnStoppedTouching(contacts); /// - protected void OnStartedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + protected void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - OnEnter?.Invoke(contactData.EventSource, contactData.Other); + OnEnter?.Invoke(contacts.EventSource, contacts.Other); } /// - protected void OnStoppedTouching(ContactData contactData) where TManifold : unmanaged, IContactManifold + protected void OnStoppedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - OnLeave?.Invoke(contactData.EventSource, contactData.Other); + OnLeave?.Invoke(contacts.EventSource, contacts.Other); } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs index 24fec9c155..667fe39ade 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/StaticComponent.cs @@ -106,12 +106,4 @@ protected override void DetachInner() Processor.Statics.Remove(this); } - - protected override int GetHandleValue() - { - if (StaticReference is { } sRef) - return sRef.Handle.Value; - - throw new InvalidOperationException(); - } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableProcessor.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableProcessor.cs index 26b1061a20..49b425ff2b 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableProcessor.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableProcessor.cs @@ -84,7 +84,7 @@ protected override void OnEntityComponentRemoved(Entity entity, CollidableCompon if (component is ISimulationUpdate simulationUpdate) component.Simulation?.Unregister(simulationUpdate); - component.Detach(false); + component.Detach(); component.Processor = null; } From 8660ea959f6c5c1591c5707b5f7d5ecf26b5943d Mon Sep 17 00:00:00 2001 From: Eideren Date: Thu, 25 Sep 2025 11:16:39 +0200 Subject: [PATCH 07/13] Minor cleanup --- .../Definitions/Contacts/ContactEventsManager.cs | 10 +++------- 1 file changed, 3 insertions(+), 7 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 85636ff1d9..4916ed3257 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -103,7 +103,7 @@ public void ClearCollisionsOf(CollidableComponent collidable, uint packed) foreach (var workerStore in _manifoldStoresPerWorker) { foreach (var typeStore in workerStore) - typeStore.ClearEventsOf(collidable, packed); + typeStore.ClearEventsOf(packed); } // Really slow, but improving performance has a huge amount of gotchas since user code @@ -252,8 +252,6 @@ public void Flush() typeStore.RunEvents(this); } - 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. foreach (var pair in _outdatedPairs) ClearCollision(pair); @@ -285,7 +283,7 @@ private interface IPerTypeManifoldStore { void RunEvents(ContactEventsManager eventsManager); - void ClearEventsOf(CollidableComponent collidableComponent, uint packed); + void ClearEventsOf(uint packed); public static unsafe void StoreManifold(IPerTypeManifoldStore[][] manifoldLists, int workerIndex, ref TManifold manifold, CollidablePair pair, int childIndexA, int childIndexB) where TManifold : unmanaged, IContactManifold { @@ -371,10 +369,8 @@ public void RunEvents(ContactEventsManager eventsManager) Clear(); } - public void ClearEventsOf(CollidableComponent collidableComponent, uint packed) + public void ClearEventsOf(uint packed) { - Debug.Assert(collidableComponent.CollidableReference.HasValue); - var spanOfThis = CollectionsMarshal.AsSpan(this); for (int i = spanOfThis.Length - 1; i >= 0; i--) { From 1818f94759afd59c63a98a983a55af16b323d341 Mon Sep 17 00:00:00 2001 From: Eideren Date: Thu, 25 Sep 2025 11:39:44 +0200 Subject: [PATCH 08/13] Fix samples --- .../BepuSample.Game/Components/Utils/CollisionComponent.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs b/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs index 9b26049195..6f4b7977c2 100644 --- a/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs +++ b/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs @@ -46,12 +46,12 @@ public class MyCustomContactEventHandler : IContactHandler public bool Contact { get; private set; } = false; public bool NoContactResponse => false; - void IContactHandler.OnStartedTouching(ContactData contactData) + void IContactHandler.OnStartedTouching(Contacts contacts) { Contact = true; } - void IContactHandler.OnStoppedTouching(ContactData contactData) + void IContactHandler.OnStoppedTouching(Contacts contacts) { Contact = false; } From 19271d43b6f4b8b007e96c48f38d37e0829ec183 Mon Sep 17 00:00:00 2001 From: Eideren Date: Thu, 25 Sep 2025 18:37:44 +0200 Subject: [PATCH 09/13] Fix duplicate manifolds being registered --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 46 +++++++++++++++++-- .../Contacts/ContactEventsManager.cs | 2 +- .../Definitions/StrideNarrowPhaseCallbacks.cs | 23 ++++++++-- 3 files changed, 63 insertions(+), 8 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 26bf49664a..06b8eba1a1 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -156,6 +156,44 @@ public static void OnContactRemovalTest() RunGameTest(game); } + [Fact] + public static void OnContactRollTest() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + game.ScreenShotAutomationEnabled = false; + + int contactStarted = 0, contactStopped = 0, passedGoal = 0; + var killTrigger = new ContactEvents { NoContactResponse = true }; + var contacts = new ContactEvents { NoContactResponse = false }; + var sphere = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new SphereCollider() } } } }; + var slope = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(2, 0.1f, 2) } } }, ContactEventHandler = contacts } }; + var goal = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(10, 0.1f, 10) } } }, ContactEventHandler = killTrigger } }; + contacts.StartedTouching += () => contactStarted++; + contacts.StoppedTouching += () => contactStopped++; + killTrigger.StoppedTouching += () => passedGoal++; + + sphere.Transform.Position.Y = 3; + slope.Transform.Rotation = Quaternion.RotationZ(10); + goal.Transform.Position.Y = -10; + + game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { sphere, slope, goal }); + + var simulation = sphere.GetSimulation(); + + while (passedGoal == 0) + await simulation.AfterUpdate(); + + Assert.Equal(1, contactStarted); + + Assert.Equal(contactStarted, contactStopped); + + game.Exit(); + }); + RunGameTest(game); + } + [Fact] public static void OnTriggerRemovalTest() { @@ -165,7 +203,7 @@ public static void OnTriggerRemovalTest() game.ScreenShotAutomationEnabled = false; int startedTouching = 0, stoppedTouching = 0; - var trigger = new Trigger(); + var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; trigger.StartedTouching += () => startedTouching++; @@ -201,7 +239,7 @@ public static void OnTriggerTest() game.ScreenShotAutomationEnabled = false; int startedTouching = 0, stoppedTouching = 0; - var trigger = new Trigger(); + var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; trigger.StartedTouching += () => startedTouching++; @@ -265,9 +303,9 @@ int TestRemovalUnsafe(BepuSimulation simulation) } } - private class Trigger : IContactHandler + private class ContactEvents : IContactHandler { - public bool NoContactResponse => true; + public required bool NoContactResponse { get; init; } public event Action? StartedTouching, StoppedTouching; 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 4916ed3257..871616a4ae 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -145,7 +145,7 @@ private void ClearCollision(OrderedPair pair) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void HandleManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + public void StoreManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { bool aListener = IsRegistered(pair.A); bool bListener = IsRegistered(pair.B); diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs index 05182ad5bb..ce38fc50bb 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs @@ -1,6 +1,7 @@ // Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. +using System.Diagnostics; using System.Runtime.CompilerServices; using BepuPhysics; using BepuPhysics.Collidables; @@ -11,6 +12,10 @@ namespace Stride.BepuPhysics.Definitions; internal struct StrideNarrowPhaseCallbacks(BepuSimulation Simulation, ContactEventsManager contactEvents, CollidableProperty collidableMaterials) : INarrowPhaseCallbacks { +#if DEBUG + [ThreadStatic] private static int configuredChildIndex, configuredManifold; +#endif + public void Initialize(Simulation simulation) { } @@ -49,7 +54,6 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi return true; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { @@ -59,7 +63,16 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair pairMaterial.FrictionCoefficient = a.FrictionCoefficient * b.FrictionCoefficient; pairMaterial.MaximumRecoveryVelocity = MathF.Max(a.MaximumRecoveryVelocity, b.MaximumRecoveryVelocity); pairMaterial.SpringSettings = pairMaterial.MaximumRecoveryVelocity == a.MaximumRecoveryVelocity ? a.SpringSettings : b.SpringSettings; - contactEvents.HandleManifold(workerIndex, pair, 0, 0, ref manifold); + +#if DEBUG + // Validate that all manifolds have been stored through the other ConfigureContactManifold, + // previously we would store the manifold from here as well, leading to duplicates + if (manifold.Count != 0) + { + ++configuredManifold; + Debug.Assert(configuredChildIndex == configuredManifold); + } +#endif if (a.IsTrigger || b.IsTrigger) { @@ -72,7 +85,11 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { - contactEvents.HandleManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); + contactEvents.StoreManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); + #if DEBUG + Debug.Assert(manifold.Count > 0); + configuredChildIndex++; + #endif return true; } From e8a42cba51e26f9cd431caad5dc5e707e78833d4 Mon Sep 17 00:00:00 2001 From: Eideren Date: Sat, 27 Sep 2025 12:09:05 +0200 Subject: [PATCH 10/13] ComputeImpactForce --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 84 +++++++++++++--- .../Contacts/ContactEventsManager.cs | 97 ++++++++++++++++--- .../Definitions/Contacts/Contacts.cs | 68 ++++++++++--- .../Definitions/StrideNarrowPhaseCallbacks.cs | 16 +-- 4 files changed, 225 insertions(+), 40 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 06b8eba1a1..0e86994215 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -4,6 +4,7 @@ using System; using System.Collections.Generic; using System.Linq; +using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using Stride.BepuPhysics.Constraints; using Stride.BepuPhysics.Definitions; @@ -170,9 +171,9 @@ public static void OnContactRollTest() var sphere = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new SphereCollider() } } } }; var slope = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(2, 0.1f, 2) } } }, ContactEventHandler = contacts } }; var goal = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(10, 0.1f, 10) } } }, ContactEventHandler = killTrigger } }; - contacts.StartedTouching += () => contactStarted++; - contacts.StoppedTouching += () => contactStopped++; - killTrigger.StoppedTouching += () => passedGoal++; + contacts.StartedTouching += (_, _) => contactStarted++; + contacts.StoppedTouching += (_, _) => contactStopped++; + killTrigger.StoppedTouching += (_, _) => passedGoal++; sphere.Transform.Position.Y = 3; slope.Transform.Rotation = Quaternion.RotationZ(10); @@ -206,11 +207,11 @@ public static void OnTriggerRemovalTest() var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; - trigger.StartedTouching += () => startedTouching++; - trigger.StoppedTouching += () => stoppedTouching++; + trigger.StartedTouching += (_, _) => startedTouching++; + trigger.StoppedTouching += (_, _) => stoppedTouching++; // Remove the component as soon as it enters the trigger to test if the system handles that case properly - trigger.StartedTouching += () => e1.Scene = null; + trigger.StartedTouching += (_, _) => e1.Scene = null; e1.Transform.Position.Y = 3; @@ -230,6 +231,62 @@ public static void OnTriggerRemovalTest() RunGameTest(game); } + [Fact] + public void ContactImpulseTest2() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + game.ScreenShotAutomationEnabled = false; + + var contactE = new ContactSampleForces(); + var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = contactE } }; + var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; + + var source = e1.Get()!; + source.ContinuousDetectionMode = ContinuousDetectionMode.Continuous; + e1.Transform.Position.Y = 3; + + game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { e1, e2 }); + source.LinearVelocity = new Vector3(0, -100, 0); + + var simulation = e1.GetSimulation(); + + while (contactE.Exit == false) + await simulation.AfterUpdate(); + + Assert.NotEmpty(contactE.ImpactForces.Where(x => x.Length() > 100)); + + game.Exit(); + }); + RunGameTest(game); + } + + private class ContactSampleForces : IContactHandler + { + public bool NoContactResponse => false; + + public List ImpactForces = new(); + public bool Exit; + + public void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold + { + foreach (var contact in contacts) + { + ImpactForces.Add(contacts.ComputeImpactForce(contact)); + } + } + + public void OnTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold + { + } + + public void OnStoppedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold + { + Exit = true; + } + } + [Fact] public static void OnTriggerTest() { @@ -242,8 +299,8 @@ public static void OnTriggerTest() var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; - trigger.StartedTouching += () => startedTouching++; - trigger.StoppedTouching += () => stoppedTouching++; + trigger.StartedTouching += (_, _) => startedTouching++; + trigger.StoppedTouching += (_, _) => stoppedTouching++; e1.Transform.Position.Y = 3; @@ -307,16 +364,21 @@ private class ContactEvents : IContactHandler { public required bool NoContactResponse { get; init; } - public event Action? StartedTouching, StoppedTouching; + public event Action? StartedTouching, Touching, StoppedTouching; public void OnStartedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { - StartedTouching?.Invoke(); + StartedTouching?.Invoke(manifold.EventSource, manifold.Other); + } + + public void OnTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold + { + Touching?.Invoke(manifold.EventSource, manifold.Other); } public void OnStoppedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { - StoppedTouching?.Invoke(); + StoppedTouching?.Invoke(manifold.EventSource, manifold.Other); } } } 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 871616a4ae..7f36efd0fe 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -19,7 +19,7 @@ namespace Stride.BepuPhysics.Definitions.Contacts; internal class ContactEventsManager : IDisposable { private readonly Dictionary _trackedCollisions = new(); - private readonly HashSet _outdatedPairs = new(); + private readonly Dictionary _outdatedPairs = new(); private readonly BufferPool _pool; private readonly BepuSimulation _simulation; private IndexSet _staticListenerFlags; @@ -124,24 +124,46 @@ private void ClearCollision(OrderedPair pair) #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 + 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 + _outdatedPairs.Remove(pair, out var velocities); + if (state.TryClear(Events.TouchingA)) { - var contactDataForA = new Contacts(pair.A, pair.B, isSourceOriginalA: true, ReadOnlySpan>.Empty, _simulation); + var contactDataForA = new Contacts + { + Groups = ReadOnlySpan>.Empty, + Simulation = _simulation, + IsSourceOriginalA = true, + EventSource = pair.A, + Other = pair.B, + SourceLinearVelocity = velocities.LinearA, + SourceAngularVelocity = velocities.AngularA, + OtherLinearVelocity = velocities.LinearB, + OtherAngularVelocity = velocities.AngularB, + }; state.HandlerA?.OnStoppedTouching(contactDataForA); } if (state.TryClear(Events.TouchingB)) { - var contactDataForB = new Contacts(pair.B, pair.A, isSourceOriginalA: false, ReadOnlySpan>.Empty, _simulation); + var contactDataForB = new Contacts + { + Groups = ReadOnlySpan>.Empty, + Simulation = _simulation, + IsSourceOriginalA = false, + EventSource = pair.B, + Other = pair.A, + SourceLinearVelocity = velocities.LinearB, + SourceAngularVelocity = velocities.AngularB, + OtherLinearVelocity = velocities.LinearA, + OtherAngularVelocity = velocities.AngularA, + }; state.HandlerB?.OnStoppedTouching(contactDataForB); } - - _outdatedPairs.Remove(pair); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -163,9 +185,33 @@ private void RunManifoldEvent(Span> unsafeInf var orderedPair = new OrderedPair(_simulation.GetComponent(safeInfos[0].Pair.A), _simulation.GetComponent(safeInfos[0].Pair.B)); + _outdatedPairs.Remove(orderedPair, out var velocities); + bool isAOriginalA = safeInfos[0].Pair.A.Packed == safeInfos[0].SortedPair.A; - var contactDataForA = new Contacts(orderedPair.A, orderedPair.B, isSourceOriginalA: isAOriginalA, safeInfos, _simulation); - var contactDataForB = new Contacts(orderedPair.B, orderedPair.A, isSourceOriginalA: isAOriginalA == false, safeInfos, _simulation); + var contactDataForA = new Contacts + { + Groups = safeInfos, + Simulation = _simulation, + IsSourceOriginalA = isAOriginalA, + EventSource = orderedPair.A, + Other = orderedPair.B, + SourceLinearVelocity = velocities.LinearA, + SourceAngularVelocity = velocities.AngularA, + OtherLinearVelocity = velocities.LinearB, + OtherAngularVelocity = velocities.AngularB, + }; + var contactDataForB = new Contacts + { + Groups = safeInfos, + Simulation = _simulation, + IsSourceOriginalA = isAOriginalA == false, + EventSource = orderedPair.B, + Other = orderedPair.A, + SourceLinearVelocity = velocities.LinearB, + SourceAngularVelocity = velocities.AngularB, + OtherLinearVelocity = velocities.LinearA, + OtherAngularVelocity = velocities.AngularA, + }; IContactHandler? handlerA, handlerB; ref var collisionState = ref CollectionsMarshal.GetValueRefOrAddDefault(_trackedCollisions, orderedPair, out bool alreadyExisted); @@ -240,8 +286,6 @@ private void RunManifoldEvent(Span> unsafeInf return; } } - - _outdatedPairs.Remove(orderedPair); } public void Flush() @@ -254,7 +298,7 @@ public void Flush() //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 in _outdatedPairs) - ClearCollision(pair); + ClearCollision(pair.Key); } /// @@ -274,11 +318,40 @@ private void TrackActivePairs(float dt, IThreadDispatcher threadDispatcher) if ((aRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[aRef.BodyHandle.Value].SetIndex == 0) || (bRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[bRef.BodyHandle.Value].SetIndex == 0)) { - _outdatedPairs.Add(trackedCollision.Key); // It's active, if manifolds did not signal that they touched we should discard this one + PreCollisionVelocities velocities; + if (trackedCollision.Key.A is BodyComponent aBody) + { + velocities.AngularA = aBody.AngularVelocity; + velocities.LinearA = aBody.LinearVelocity; + } + else + { + velocities.AngularA = default; + velocities.LinearA = default; + } + + if (trackedCollision.Key.B is BodyComponent bBody) + { + velocities.AngularB = bBody.AngularVelocity; + velocities.LinearB = bBody.LinearVelocity; + } + else + { + velocities.AngularB = default; + velocities.LinearB = default; + } + + _outdatedPairs.Add(trackedCollision.Key, velocities); // It's active, if manifolds did not signal that they touched we should discard this one } } } + private struct PreCollisionVelocities + { + public Vector3 LinearA, AngularA; + public Vector3 LinearB, AngularB; + } + private interface IPerTypeManifoldStore { void RunEvents(ContactEventsManager eventsManager); diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs index 247e4f4abd..5bdfabf456 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs @@ -1,7 +1,9 @@ // Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. +using System.Diagnostics.Contracts; using BepuPhysics.CollisionDetection; +using Stride.Core.Mathematics; namespace Stride.BepuPhysics.Definitions.Contacts; @@ -24,35 +26,79 @@ namespace Stride.BepuPhysics.Definitions.Contacts; /// /// Contact group registered between these two bodies, one per compound child hit /// - public ReadOnlySpan> Groups { get; } + public required ReadOnlySpan> Groups { get; init; } /// /// The simulation this contact occured in /// - public BepuSimulation Simulation { get; } + public required BepuSimulation Simulation { get; init; } /// /// Whether maps to the unsorted, original A /// - public bool IsSourceOriginalA { get; } + public required bool IsSourceOriginalA { get; init; } /// /// The collidable which is bound to this /// - public CollidableComponent EventSource { get; } + public required CollidableComponent EventSource { get; init; } /// /// The other collidable /// - public CollidableComponent Other { get; } + public required CollidableComponent Other { get; init; } - public Contacts(CollidableComponent source, CollidableComponent other, bool isSourceOriginalA, ReadOnlySpan> groups, BepuSimulation simulation) + /// + /// The linear velocity had at the time of contact + /// + public required Vector3 SourceLinearVelocity { get; init; } + + /// + /// The angular velocity had at the time of contact + /// + public required Vector3 SourceAngularVelocity { get; init; } + + /// + /// The linear velocity had at the time of contact + /// + public required Vector3 OtherLinearVelocity { get; init; } + + /// + /// The angular velocity had at the time of contact + /// + public required Vector3 OtherAngularVelocity { get; init; } + + [Pure] + public Vector3 ComputeImpactForce(Contact contact) { - EventSource = source; - Other = other; - IsSourceOriginalA = isSourceOriginalA; - Groups = groups; - Simulation = simulation; + var impactPos = contact.Point; + float invMassOther, invMassThis; + Vector3 impactVelOther, impactVelThis; + if (Other is BodyComponent bodyOther) + { + impactVelOther = OtherLinearVelocity + Vector3.Cross(OtherAngularVelocity, impactPos - bodyOther.Position); + invMassOther = bodyOther.BodyInertia.InverseMass; + } + else + { + impactVelOther = default; + invMassOther = 0; + } + + if (EventSource is BodyComponent bodySource) + { + impactVelThis = SourceLinearVelocity + Vector3.Cross(SourceAngularVelocity, impactPos - bodySource.Position); + invMassThis = bodySource.BodyInertia.InverseMass; + } + else + { + impactVelThis = default; + invMassThis = 0; + } + + var relativeImpactVel = impactVelOther - impactVelThis; + float effectiveMass = 1f / (invMassOther + invMassThis); + return relativeImpactVel * effectiveMass / (float)Simulation.FixedTimeStepSeconds; } /// diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs index ce38fc50bb..e91f2d931c 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/StrideNarrowPhaseCallbacks.cs @@ -70,7 +70,7 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair if (manifold.Count != 0) { ++configuredManifold; - Debug.Assert(configuredChildIndex == configuredManifold); + Debug.Assert(configuredChildIndex >= configuredManifold); } #endif @@ -85,11 +85,15 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { - contactEvents.StoreManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); - #if DEBUG - Debug.Assert(manifold.Count > 0); - configuredChildIndex++; - #endif + if (manifold.Count != 0) + { + contactEvents.StoreManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); + #if DEBUG + Debug.Assert(configuredChildIndex >= configuredManifold); + configuredChildIndex++; + #endif + } + return true; } From 20e43944de9d529cc354d937d769a2658eaa4a5b Mon Sep 17 00:00:00 2001 From: Eideren Date: Sat, 27 Sep 2025 12:30:31 +0200 Subject: [PATCH 11/13] Clarify some of the comments and swap normal flip to align with previous iteration --- .../Stride.BepuPhysics/Definitions/Contacts/Contact.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs index 7d8cb24180..b42e652c4d 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contact.cs @@ -43,9 +43,9 @@ internal Contact(int index, Contacts contacts, in ContactGroup ContactGroup.Manifold.GetFeatureId(Index); /// - /// The contact's normal, oriented based on + /// The normal on 's surface. Points from towards 's surface /// - public Vector3 Normal => Contacts.IsSourceOriginalA ? -ContactGroup.Manifold.GetNormal(Index) : ContactGroup.Manifold.GetNormal(Index); + public Vector3 Normal => Contacts.IsSourceOriginalA ? ContactGroup.Manifold.GetNormal(Index) : -ContactGroup.Manifold.GetNormal(Index); /// /// When has a , @@ -60,7 +60,7 @@ internal Contact(int index, Contacts contacts, in ContactGroup Contacts.IsSourceOriginalA ? ContactGroup.ChildIndexB : ContactGroup.ChildIndexA; /// The position at which the contact occured - /// This may not be accurate if either collidables are not part of the simulation anymore + /// This may not be accurate if they separated within this tick, or when you removed either of them from the simulation within this scope public Vector3 Point { get From 7f55bb5a4b8c2196353ff116c66a58aa543f80d7 Mon Sep 17 00:00:00 2001 From: Eideren Date: Sat, 27 Sep 2025 12:56:07 +0200 Subject: [PATCH 12/13] Make IContactEventHandler non-breaking where possible --- .../Contacts/IContactEventHandler.cs | 49 +++++++++++++++---- 1 file changed, 39 insertions(+), 10 deletions(-) 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 06eda97517..4c03b4ac1f 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs @@ -9,7 +9,7 @@ namespace Stride.BepuPhysics.Definitions.Contacts; /// /// Implements handlers for various collision events. /// -[Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, update your contact methods when migrating to this new class", true)] +[Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, update your contact methods when migrating to this new class")] public interface IContactEventHandler : IContactHandler { /// @@ -33,7 +33,7 @@ public interface IContactEventHandler : IContactHandler /// Index of the new contact in the contact manifold. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called", true)] void OnContactAdded(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -54,7 +54,7 @@ void OnContactAdded(CollidableComponent eventSource, CollidableCompon /// Feature id of the contact that was removed and is no longer present in the contact manifold. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called", true)] void OnContactRemoved(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int removedFeatureId, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -69,7 +69,7 @@ void OnContactRemoved(CollidableComponent eventSource, CollidableComp /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}")] void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -84,7 +84,7 @@ void OnStartedTouching(CollidableComponent eventSource, CollidableCom /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}")] void OnTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -100,7 +100,7 @@ void OnTouching(CollidableComponent eventSource, CollidableComponent /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}")] void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -116,7 +116,7 @@ void OnStoppedTouching(CollidableComponent eventSource, CollidableCom /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called", true)] void OnPairCreated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -131,7 +131,7 @@ void OnPairCreated(CollidableComponent eventSource, CollidableCompone /// Whether the manifold's normals and offset is flipped from the source's point of view. /// Index of the worker thread that fired this event. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called", true)] void OnPairUpdated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -142,8 +142,37 @@ void OnPairUpdated(CollidableComponent eventSource, CollidableCompone /// Collidable that the event was attached to. /// Other collider collided with. /// The simulation where the contact occured. - [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called")] - new void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) + [Obsolete($"{nameof(IContactEventHandler)} as been superseded by {nameof(IContactHandler)}, this method will never be called", true)] + void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) { } + + bool IContactHandler.NoContactResponse => NoContactResponse; + + void IContactHandler.OnStartedTouching(Contacts contacts) + { + foreach (var contact in contacts) + { + var manifold = contact.ContactGroup.Manifold; + OnStartedTouching(contacts.EventSource, contacts.Other, ref manifold, contacts.IsSourceOriginalA == false, 0, contacts.Simulation); + } + } + + void IContactHandler.OnTouching(Contacts contacts) + { + foreach (var contact in contacts) + { + var manifold = contact.ContactGroup.Manifold; + OnTouching(contacts.EventSource, contacts.Other, ref manifold, contacts.IsSourceOriginalA == false, 0, contacts.Simulation); + } + } + + void IContactHandler.OnStoppedTouching(Contacts contacts) + { + foreach (var contact in contacts) + { + var manifold = contact.ContactGroup.Manifold; + OnStoppedTouching(contacts.EventSource, contacts.Other, ref manifold, contacts.IsSourceOriginalA == false, 0, contacts.Simulation); + } + } } From cfc48c0ff92801a3d5ed549c5f9f21b0aa59b09f Mon Sep 17 00:00:00 2001 From: Eideren Date: Wed, 1 Oct 2025 12:54:40 +0200 Subject: [PATCH 13/13] Collect previous velocities before contacts are generated --- .../Stride.BepuPhysics.Tests/BepuTests.cs | 2 +- .../Stride.BepuPhysics/BepuSimulation.cs | 20 +++++++ .../Stride.BepuPhysics/BodyComponent.cs | 19 ++++++ .../Contacts/ContactEventsManager.cs | 59 +++---------------- .../Definitions/Contacts/Contacts.cs | 24 +------- 5 files changed, 49 insertions(+), 75 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 0e86994215..afd4bcee6c 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -232,7 +232,7 @@ public static void OnTriggerRemovalTest() } [Fact] - public void ContactImpulseTest2() + public void ContactImpulseTest() { var game = new GameTest(); game.Script.AddTask(async () => diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs index beec4a61f9..53df12fd3a 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs @@ -717,6 +717,8 @@ internal void Update(TimeSpan elapsed) Elider.SimulationUpdate(_simulationUpdateComponents, this, simTimeStepInSec); + Dispatcher.ForBatched(Bodies.Count, new UpdatePreviousVelocities { Bodies = Bodies }); + Simulation.Timestep(simTimeStepInSec, _threadDispatcher); //perform physic simulation using SimulationFixedStep ContactEvents.Flush(); //Fire event handler stuff. @@ -970,4 +972,22 @@ public void GetResult() { } public TickAwaiter GetAwaiter() => this; } + + private readonly struct UpdatePreviousVelocities : Dispatcher.IBatchJob + { + public required List Bodies { get; init; } + + public void Process(int start, int endExclusive) + { + for (; start < endExclusive; start++) + { + var body = Bodies[start]; + if (body is not null) + { + body.PreviousAngularVelocity = body.AngularVelocity; + body.PreviousLinearVelocity = body.LinearVelocity; + } + } + } + } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs index 6db5a6a715..2e1a488788 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BodyComponent.cs @@ -220,6 +220,23 @@ public Vector3 AngularVelocity } } + /// + /// The translation velocity in unit per second during the previous physics tick + /// + [DataMemberIgnore] + public Vector3 PreviousLinearVelocity { get; internal set; } + + /// + /// The rotation velocity in unit per second during the previous physics tick + /// + /// + /// 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] + public Vector3 PreviousAngularVelocity { get; internal set; } + /// /// The position of this body in the physics scene, setting it will teleport this object to the position provided. /// @@ -437,6 +454,8 @@ protected override void AttachInner(NRigidPose pose, BodyInertia shapeInertia, T } else { + LinearVelocity = AngularVelocity = default; + var bHandle = Simulation.Simulation.Bodies.Add(bDescription); BodyReference = Simulation.Simulation.Bodies[bHandle]; BodyReference.Value.Collidable.Continuity = ContinuousDetection; 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 7f36efd0fe..dcd5adaddb 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -19,7 +19,7 @@ namespace Stride.BepuPhysics.Definitions.Contacts; internal class ContactEventsManager : IDisposable { private readonly Dictionary _trackedCollisions = new(); - private readonly Dictionary _outdatedPairs = new(); + private readonly HashSet _outdatedPairs = new(); private readonly BufferPool _pool; private readonly BepuSimulation _simulation; private IndexSet _staticListenerFlags; @@ -129,7 +129,7 @@ private void ClearCollision(OrderedPair pair) _trackedCollisions.Remove(pair, out var state); #endif - _outdatedPairs.Remove(pair, out var velocities); + _outdatedPairs.Remove(pair); if (state.TryClear(Events.TouchingA)) { @@ -139,11 +139,7 @@ private void ClearCollision(OrderedPair pair) Simulation = _simulation, IsSourceOriginalA = true, EventSource = pair.A, - Other = pair.B, - SourceLinearVelocity = velocities.LinearA, - SourceAngularVelocity = velocities.AngularA, - OtherLinearVelocity = velocities.LinearB, - OtherAngularVelocity = velocities.AngularB, + Other = pair.B }; state.HandlerA?.OnStoppedTouching(contactDataForA); } @@ -156,11 +152,7 @@ private void ClearCollision(OrderedPair pair) Simulation = _simulation, IsSourceOriginalA = false, EventSource = pair.B, - Other = pair.A, - SourceLinearVelocity = velocities.LinearB, - SourceAngularVelocity = velocities.AngularB, - OtherLinearVelocity = velocities.LinearA, - OtherAngularVelocity = velocities.AngularA, + Other = pair.A }; state.HandlerB?.OnStoppedTouching(contactDataForB); } @@ -185,7 +177,7 @@ private void RunManifoldEvent(Span> unsafeInf var orderedPair = new OrderedPair(_simulation.GetComponent(safeInfos[0].Pair.A), _simulation.GetComponent(safeInfos[0].Pair.B)); - _outdatedPairs.Remove(orderedPair, out var velocities); + _outdatedPairs.Remove(orderedPair); bool isAOriginalA = safeInfos[0].Pair.A.Packed == safeInfos[0].SortedPair.A; var contactDataForA = new Contacts @@ -195,10 +187,6 @@ private void RunManifoldEvent(Span> unsafeInf IsSourceOriginalA = isAOriginalA, EventSource = orderedPair.A, Other = orderedPair.B, - SourceLinearVelocity = velocities.LinearA, - SourceAngularVelocity = velocities.AngularA, - OtherLinearVelocity = velocities.LinearB, - OtherAngularVelocity = velocities.AngularB, }; var contactDataForB = new Contacts { @@ -207,10 +195,6 @@ private void RunManifoldEvent(Span> unsafeInf IsSourceOriginalA = isAOriginalA == false, EventSource = orderedPair.B, Other = orderedPair.A, - SourceLinearVelocity = velocities.LinearB, - SourceAngularVelocity = velocities.AngularB, - OtherLinearVelocity = velocities.LinearA, - OtherAngularVelocity = velocities.AngularA, }; IContactHandler? handlerA, handlerB; @@ -298,7 +282,7 @@ public void Flush() //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 in _outdatedPairs) - ClearCollision(pair.Key); + ClearCollision(pair); } /// @@ -318,40 +302,11 @@ private void TrackActivePairs(float dt, IThreadDispatcher threadDispatcher) if ((aRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[aRef.BodyHandle.Value].SetIndex == 0) || (bRef.Mobility != CollidableMobility.Static && bodyHandleToLocation[bRef.BodyHandle.Value].SetIndex == 0)) { - PreCollisionVelocities velocities; - if (trackedCollision.Key.A is BodyComponent aBody) - { - velocities.AngularA = aBody.AngularVelocity; - velocities.LinearA = aBody.LinearVelocity; - } - else - { - velocities.AngularA = default; - velocities.LinearA = default; - } - - if (trackedCollision.Key.B is BodyComponent bBody) - { - velocities.AngularB = bBody.AngularVelocity; - velocities.LinearB = bBody.LinearVelocity; - } - else - { - velocities.AngularB = default; - velocities.LinearB = default; - } - - _outdatedPairs.Add(trackedCollision.Key, velocities); // It's active, if manifolds did not signal that they touched we should discard this one + _outdatedPairs.Add(trackedCollision.Key); // It's active, if manifolds did not signal that they touched we should discard this one } } } - private struct PreCollisionVelocities - { - public Vector3 LinearA, AngularA; - public Vector3 LinearB, AngularB; - } - private interface IPerTypeManifoldStore { void RunEvents(ContactEventsManager eventsManager); diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs index 5bdfabf456..392e5ca2a5 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs @@ -48,26 +48,6 @@ namespace Stride.BepuPhysics.Definitions.Contacts; /// public required CollidableComponent Other { get; init; } - /// - /// The linear velocity had at the time of contact - /// - public required Vector3 SourceLinearVelocity { get; init; } - - /// - /// The angular velocity had at the time of contact - /// - public required Vector3 SourceAngularVelocity { get; init; } - - /// - /// The linear velocity had at the time of contact - /// - public required Vector3 OtherLinearVelocity { get; init; } - - /// - /// The angular velocity had at the time of contact - /// - public required Vector3 OtherAngularVelocity { get; init; } - [Pure] public Vector3 ComputeImpactForce(Contact contact) { @@ -76,7 +56,7 @@ public Vector3 ComputeImpactForce(Contact contact) Vector3 impactVelOther, impactVelThis; if (Other is BodyComponent bodyOther) { - impactVelOther = OtherLinearVelocity + Vector3.Cross(OtherAngularVelocity, impactPos - bodyOther.Position); + impactVelOther = bodyOther.PreviousLinearVelocity + Vector3.Cross(bodyOther.PreviousAngularVelocity, impactPos - bodyOther.Position); invMassOther = bodyOther.BodyInertia.InverseMass; } else @@ -87,7 +67,7 @@ public Vector3 ComputeImpactForce(Contact contact) if (EventSource is BodyComponent bodySource) { - impactVelThis = SourceLinearVelocity + Vector3.Cross(SourceAngularVelocity, impactPos - bodySource.Position); + impactVelThis = bodySource.PreviousLinearVelocity + Vector3.Cross(bodySource.PreviousAngularVelocity, impactPos - bodySource.Position); invMassThis = bodySource.BodyInertia.InverseMass; } else