diff --git a/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs b/samples/Physics/BepuSample/BepuSample.Game/Components/Utils/CollisionComponent.cs index 6d4c669fe3..6f4b7977c2 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(Contacts contacts) { Contact = true; } - void IContactEventHandler.OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) + void IContactHandler.OnStoppedTouching(Contacts contacts) { Contact = false; } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 7acfd0033c..afd4bcee6c 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; @@ -156,6 +157,44 @@ public static void OnContactRemovalTest() RunGameTest(game); } + [Fact] + public static void OnContactRollTest() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + game.ScreenShotAutomationEnabled = false; + + int contactStarted = 0, contactStopped = 0, passedGoal = 0; + var killTrigger = new ContactEvents { NoContactResponse = true }; + var contacts = new ContactEvents { NoContactResponse = false }; + var sphere = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new SphereCollider() } } } }; + var slope = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(2, 0.1f, 2) } } }, ContactEventHandler = contacts } }; + var goal = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(10, 0.1f, 10) } } }, ContactEventHandler = killTrigger } }; + contacts.StartedTouching += (_, _) => contactStarted++; + contacts.StoppedTouching += (_, _) => contactStopped++; + killTrigger.StoppedTouching += (_, _) => passedGoal++; + + sphere.Transform.Position.Y = 3; + slope.Transform.Rotation = Quaternion.RotationZ(10); + goal.Transform.Position.Y = -10; + + game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { sphere, slope, goal }); + + var simulation = sphere.GetSimulation(); + + while (passedGoal == 0) + await simulation.AfterUpdate(); + + Assert.Equal(1, contactStarted); + + Assert.Equal(contactStarted, contactStopped); + + game.Exit(); + }); + RunGameTest(game); + } + [Fact] public static void OnTriggerRemovalTest() { @@ -164,19 +203,15 @@ public static void OnTriggerRemovalTest() { game.ScreenShotAutomationEnabled = false; - int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0; - var trigger = new Trigger(); + int startedTouching = 0, stoppedTouching = 0; + var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; - trigger.PairCreated += () => pairCreated++; - trigger.PairEnded += () => pairEnded++; - trigger.ContactAdded += () => contactAdded++; - trigger.ContactRemoved += () => contactRemoved++; - trigger.StartedTouching += () => startedTouching++; - trigger.StoppedTouching += () => stoppedTouching++; + trigger.StartedTouching += (_, _) => startedTouching++; + trigger.StoppedTouching += (_, _) => stoppedTouching++; // Remove the component as soon as it enters the trigger to test if the system handles that case properly - trigger.PairCreated += () => e1.Scene = null; + trigger.StartedTouching += (_, _) => e1.Scene = null; e1.Transform.Position.Y = 3; @@ -184,15 +219,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(); @@ -200,6 +231,62 @@ public static void OnTriggerRemovalTest() RunGameTest(game); } + [Fact] + public void ContactImpulseTest() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + game.ScreenShotAutomationEnabled = false; + + var contactE = new ContactSampleForces(); + var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = contactE } }; + var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; + + var source = e1.Get()!; + 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() { @@ -208,16 +295,12 @@ public static void OnTriggerTest() { game.ScreenShotAutomationEnabled = false; - int pairEnded = 0, pairCreated = 0, contactAdded = 0, contactRemoved = 0, startedTouching = 0, stoppedTouching = 0; - var trigger = new Trigger(); + int startedTouching = 0, stoppedTouching = 0; + var trigger = new ContactEvents { NoContactResponse = true }; var e1 = new Entity { new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, ContactEventHandler = trigger } }; var e2 = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } } }; - trigger.PairCreated += () => pairCreated++; - trigger.PairEnded += () => pairEnded++; - trigger.ContactAdded += () => contactAdded++; - trigger.ContactRemoved += () => contactRemoved++; - trigger.StartedTouching += () => startedTouching++; - trigger.StoppedTouching += () => stoppedTouching++; + trigger.StartedTouching += (_, _) => startedTouching++; + trigger.StoppedTouching += (_, _) => stoppedTouching++; e1.Transform.Position.Y = 3; @@ -225,15 +308,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,40 +360,25 @@ int TestRemovalUnsafe(BepuSimulation simulation) } } - private class Trigger : IContactEventHandler + private class ContactEvents : IContactHandler { - public bool NoContactResponse => true; - - public event Action? ContactAdded, ContactRemoved, StartedTouching, StoppedTouching, PairCreated, PairEnded; + public required bool NoContactResponse { get; init; } - public void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold - { - StartedTouching?.Invoke(); - } - - public void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold - { - StoppedTouching?.Invoke(); - } - - public void OnContactAdded(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold - { - ContactAdded?.Invoke(); - } + public event Action? StartedTouching, Touching, StoppedTouching; - public void OnContactRemoved(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + public void OnStartedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { - ContactRemoved?.Invoke(); + StartedTouching?.Invoke(manifold.EventSource, manifold.Other); } - public void OnPairCreated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + public void OnTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { - PairCreated?.Invoke(); + Touching?.Invoke(manifold.EventSource, manifold.Other); } - public void OnPairEnded(CollidableComponent eventSource, CollidableComponent other, BepuSimulation bepuSimulation) + public void OnStoppedTouching(Contacts manifold) where TManifold : unmanaged, IContactManifold { - PairEnded?.Invoke(); + StoppedTouching?.Invoke(manifold.EventSource, manifold.Other); } } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs index fdc28f5fad..53df12fd3a 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; @@ -726,6 +717,8 @@ internal void Update(TimeSpan elapsed) Elider.SimulationUpdate(_simulationUpdateComponents, this, simTimeStepInSec); + Dispatcher.ForBatched(Bodies.Count, new UpdatePreviousVelocities { Bodies = Bodies }); + Simulation.Timestep(simTimeStepInSec, _threadDispatcher); //perform physic simulation using SimulationFixedStep ContactEvents.Flush(); //Fire event handler stuff. @@ -979,4 +972,22 @@ public void GetResult() { } public TickAwaiter GetAwaiter() => this; } + + private readonly struct UpdatePreviousVelocities : Dispatcher.IBatchJob + { + public required List 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 42d767315b..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; @@ -488,14 +507,6 @@ protected override void DetachInner() BodyReference = null; } - protected override int GetHandleValue() - { - if (BodyReference is { } bRef) - return bRef.Handle.Value; - - throw new InvalidOperationException(); - } - /// /// 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 67e92c0cf6..03c53d3dec 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,57 @@ 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(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(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold + /// + protected virtual void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - contactManifold.GetContact(contactIndex, out var contact); + foreach (var contact in contacts) + { + Contacts.Add((contacts.Other, new Contact + { + Normal = contact.Normal, + Depth = contact.Depth, + FeatureId = contact.FeatureId, + Offset = contact.Point - (Vector3)contacts.EventSource.Pose!.Value.Position, + })); + } + } - if (flippedManifold) + /// + protected virtual void OnTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold + { + 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; + if (Contacts[i].Source == contacts.Other) + Contacts.SwapRemoveAt(i); } - contact.Offset = contact.Offset + Entity.Transform.WorldMatrix.TranslationVector.ToNumeric() + CenterOfMass.ToNumeric(); - Contacts.Add((other, contact)); + foreach (var contact in contacts) + { + Contacts.Add((contacts.Other, new Contact + { + Normal = contact.Normal, + Depth = contact.Depth, + FeatureId = contact.FeatureId, + Offset = contact.Point - (Vector3)contacts.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(Contacts contacts) where TManifold : unmanaged, IContactManifold { for (int i = Contacts.Count - 1; i >= 0; i--) { - if (Contacts[i].Source == 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 26be920f67..5c1ef9bc11 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 { @@ -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..b42e652c4d --- /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 normal on 's surface. Points from towards 's surface + /// + 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 they separated within this tick, or when you removed either of them from the simulation within this scope + 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/ContactEventsManager.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs index 95a3f2dd02..dcd5adaddb 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.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; using System.Numerics; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; @@ -70,7 +72,7 @@ public void Unregister(CollidableComponent collidable) else _bodyListenerFlags.Remove(reference.RawHandleValue); - ClearCollisionsOf(collidable); + ClearCollisionsOf(collidable, reference.Packed); } /// @@ -87,6 +89,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) @@ -95,276 +98,178 @@ 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(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, ref manifold, 0); + ClearCollision(pair); } } - private unsafe void ClearCollision(OrderedPair pair, ref EmptyManifold manifold, int workerIndex) + 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); - 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 - 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); + _outdatedPairs.Remove(pair); if (state.TryClear(Events.TouchingA)) - state.HandlerA?.OnStoppedTouching(pair.A, pair.B, ref manifold, flippedManifold, workerIndex, _simulation); - if (state.TryClear(Events.TouchingB)) - state.HandlerB?.OnStoppedTouching(pair.B, pair.A, ref manifold, flippedManifold, workerIndex, _simulation); - - if (state.TryClear(Events.CreatedA)) - state.HandlerA?.OnPairEnded(pair.A, pair.B, _simulation); - if (state.TryClear(Events.CreatedB)) - state.HandlerB?.OnPairEnded(pair.B, pair.A, _simulation); + { + var contactDataForA = new Contacts + { + Groups = ReadOnlySpan>.Empty, + Simulation = _simulation, + IsSourceOriginalA = true, + EventSource = pair.A, + Other = pair.B + }; + state.HandlerA?.OnStoppedTouching(contactDataForA); + } - _outdatedPairs.Remove(pair); + if (state.TryClear(Events.TouchingB)) + { + var contactDataForB = new Contacts + { + Groups = ReadOnlySpan>.Empty, + Simulation = _simulation, + IsSourceOriginalA = false, + EventSource = pair.B, + Other = pair.A + }; + state.HandlerB?.OnStoppedTouching(contactDataForB); + } } - public void HandleManifold(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + [MethodImpl(MethodImplOptions.AggressiveInlining)] + 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); 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, pair, childIndexA, childIndexB); } - private unsafe void RunManifoldEvent(int workerIndex, CollidableComponent a, CollidableComponent b, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + private void RunManifoldEvent(Span> unsafeInfos) 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); + // 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 - bool bFlipped = !aFlipped; + var orderedPair = new OrderedPair(_simulation.GetComponent(safeInfos[0].Pair.A), _simulation.GetComponent(safeInfos[0].Pair.B)); - (a, b) = (orderedPair.A, orderedPair.B); + _outdatedPairs.Remove(orderedPair); - IContactEventHandler? handlerA; - IContactEventHandler? handlerB; + bool isAOriginalA = safeInfos[0].Pair.A.Packed == safeInfos[0].SortedPair.A; + var contactDataForA = new Contacts + { + Groups = safeInfos, + Simulation = _simulation, + IsSourceOriginalA = isAOriginalA, + EventSource = orderedPair.A, + Other = orderedPair.B, + }; + var contactDataForB = new Contacts + { + Groups = safeInfos, + Simulation = _simulation, + IsSourceOriginalA = isAOriginalA == false, + EventSource = orderedPair.B, + Other = orderedPair.A, + }; + + 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 = orderedPair.A.ContactEventHandler; + handlerB = collisionState.HandlerB = orderedPair.B.ContactEventHandler; + } - 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)) + bool touching = false; + for (int i = 0; i < safeInfos.Length; i++) + { + for (int j = 0; j < safeInfos[i].Manifold.Count; ++j) + { + if (safeInfos[i].Manifold.GetDepth(j) >= 0) { - handlerB.OnStartedTouching(b, a, ref manifold, bFlipped, workerIndex, _simulation); - if (collisionState.Alive == false) - return; + touching = true; + break; } - - 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); } public void Flush() @@ -375,11 +280,9 @@ 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, ref manifold, 0); + ClearCollision(pair); } /// @@ -408,9 +311,9 @@ private interface IPerTypeManifoldStore { void RunEvents(ContactEventsManager eventsManager); - void ClearEventsOf(CollidableComponent collidableComponent); + void ClearEventsOf(uint packed); - 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, CollidablePair pair, int childIndexA, int childIndexB) where TManifold : unmanaged, IContactManifold { var manifoldsForWorker = manifoldLists[workerIndex]; int typeIndex = TypeIndex.Index; @@ -431,15 +334,30 @@ public static unsafe void StoreManifold(IPerTypeManifoldStore[][] man } var handler = (ListOf)manifoldsForWorker[typeIndex]; - handler.Add((manifold, a, b)); + + 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; @@ -459,47 +377,43 @@ 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>, 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 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; } Clear(); } - public void ClearEventsOf(CollidableComponent collidableComponent) + public void ClearEventsOf(uint packed) { 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); } } } } - 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,26 +441,8 @@ public bool TryClear(Events e) [Flags] private enum Events { - CreatedA = 0b0001, - CreatedB = 0b0010, - TouchingA = 0b0100, - TouchingB = 0b1000, - } - - 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"); - } + TouchingA = 0b01, + TouchingB = 0b10, } @@ -569,3 +465,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..392e5ca2a5 --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/Contacts.cs @@ -0,0 +1,117 @@ +// 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; + +/// +/// 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 required ReadOnlySpan> Groups { get; init; } + + /// + /// The simulation this contact occured in + /// + public required BepuSimulation Simulation { get; init; } + + /// + /// Whether maps to the unsorted, original A + /// + public required bool IsSourceOriginalA { get; init; } + + /// + /// The collidable which is bound to this + /// + public required CollidableComponent EventSource { get; init; } + + /// + /// The other collidable + /// + public required CollidableComponent Other { get; init; } + + [Pure] + public Vector3 ComputeImpactForce(Contact contact) + { + var impactPos = contact.Point; + float invMassOther, invMassThis; + Vector3 impactVelOther, impactVelThis; + if (Other is BodyComponent bodyOther) + { + impactVelOther = bodyOther.PreviousLinearVelocity + Vector3.Cross(bodyOther.PreviousAngularVelocity, impactPos - bodyOther.Position); + invMassOther = bodyOther.BodyInertia.InverseMass; + } + else + { + impactVelOther = default; + invMassOther = 0; + } + + if (EventSource is BodyComponent bodySource) + { + impactVelThis = bodySource.PreviousLinearVelocity + Vector3.Cross(bodySource.PreviousAngularVelocity, 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; + } + + /// + 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/IContactEventHandler.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/IContactEventHandler.cs index 5768ddd0eb..4c03b4ac1f 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")] +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", true)] 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", true)] 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)}")] 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)}")] 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)}")] 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", true)] 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", true)] void OnPairUpdated(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int workerIndex, BepuSimulation bepuSimulation) where TManifold : unmanaged, IContactManifold { } @@ -132,7 +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", 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); + } + } } 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..f2ca9de9b7 --- /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(Contacts contacts) 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(Contacts contacts) 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(Contacts contacts) 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..e91f2d931c 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,9 +54,8 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi return true; } - [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 +63,16 @@ 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); + +#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,6 +85,15 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { + if (manifold.Count != 0) + { + contactEvents.StoreManifold(workerIndex, pair, childIndexA, childIndexB, ref manifold); + #if DEBUG + Debug.Assert(configuredChildIndex >= configuredManifold); + configuredChildIndex++; + #endif + } + return true; } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Trigger.cs index ba3fe0bf44..4d58f3475f 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(Contacts contacts) => OnStartedTouching(contacts); + void IContactHandler.OnStoppedTouching(Contacts contacts) => OnStoppedTouching(contacts); - /// - protected void OnStartedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) + /// + protected void OnStartedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - OnEnter?.Invoke(eventSource, other); + OnEnter?.Invoke(contacts.EventSource, contacts.Other); } - /// - protected void OnStoppedTouching(CollidableComponent eventSource, CollidableComponent other, ref TManifold contactManifold, bool flippedManifold, int contactIndex, BepuSimulation bepuSimulation) + /// + protected void OnStoppedTouching(Contacts contacts) where TManifold : unmanaged, IContactManifold { - OnLeave?.Invoke(eventSource, 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; }