From c9cb14a8d1cf1bf6b3bfa89ee938cd6f9586347f Mon Sep 17 00:00:00 2001 From: Eideren Date: Tue, 16 Dec 2025 18:22:08 +0100 Subject: [PATCH] fix: Bepu transformation for bodies children of bodies --- .../Stride.BepuPhysics/BepuSimulation.cs | 147 ++++++++++-------- 1 file changed, 81 insertions(+), 66 deletions(-) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs index b55a439f99..48e54b6c70 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs @@ -557,7 +557,7 @@ public ConversionEnum OverlapIn /// /// Called by the BroadPhase.GetOverlaps to collect all encountered collidables. /// - struct BroadPhaseOverlapEnumerator : IBreakableForEach + private struct BroadPhaseOverlapEnumerator : IBreakableForEach { public QuickList References; //The enumerator never gets stored into unmanaged memory, so it's safe to include a reference type instance. @@ -574,7 +574,7 @@ public bool LoopBody(CollidableReference reference) /// /// Provides callbacks for filtering and data collection to the CollisionBatcher we'll be using to test query shapes against the detected environment. /// - struct BatcherCallbacks : ICollisionCallbacks where T : IOverlapCollector + private struct BatcherCallbacks : ICollisionCallbacks where T : IOverlapCollector { public required CollisionMask CollisionMask; public required QuickList References; @@ -605,7 +605,7 @@ public void OnPairCompleted(int pairId, ref TManifold manifold) where } } - unsafe void OverlapInner(in TShape shape, in SRigidPose pose, CollisionMask collisionMask, ref TCollector collector) where TShape : unmanaged, IConvexShape where TCollector : IOverlapCollector + private unsafe void OverlapInner(in TShape shape, in SRigidPose pose, CollisionMask collisionMask, ref TCollector collector) where TShape : unmanaged, IConvexShape where TCollector : IOverlapCollector { fixed (TShape* queryShapeData = &shape) { @@ -749,49 +749,14 @@ internal void Update(TimeSpan elapsed) private void SyncActiveTransformsWithPhysics() { + var job = new SyncTransformsJob(Simulation.Bodies, this); if (ParallelUpdate) { - Dispatcher.For(0, Simulation.Bodies.ActiveSet.Count, (i) => SyncTransformsWithPhysics(Simulation.Bodies.GetBodyReference(Simulation.Bodies.ActiveSet.IndexToHandle[i]), this)); + Dispatcher.ForBatched(Simulation.Bodies.ActiveSet.Count, job); } else { - for (int i = 0; i < Simulation.Bodies.ActiveSet.Count; i++) - { - SyncTransformsWithPhysics(Simulation.Bodies.GetBodyReference(Simulation.Bodies.ActiveSet.IndexToHandle[i]), this); - } - } - - static void SyncTransformsWithPhysics(in BodyReference body, BepuSimulation bepuSim) - { - var collidable = bepuSim.GetComponent(body.Handle); - - for (var item = collidable.Parent; item != null; item = item.Parent) - { - if (item.BodyReference is { } bRef) - { - // Have to go through our parents to make sure they're up to date since we're reading from the parent's world matrix - // This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing - SyncTransformsWithPhysics(bRef, bepuSim); - // This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc. - // It's not that likely but should still be documented as suboptimal somewhere - item.Entity.Transform.Parent.UpdateWorldMatrix(); - } - } - - var localPosition = body.Pose.Position.ToStride(); - var localRotation = body.Pose.Orientation.ToStride(); - - var entityTransform = collidable.Entity.Transform; - if (entityTransform.Parent is { } parent) - { - parent.WorldMatrix.Decompose(out Vector3 _, out Quaternion parentEntityRotation, out Vector3 parentEntityPosition); - var iRotation = Quaternion.Invert(parentEntityRotation); - localPosition = Vector3.Transform(localPosition - parentEntityPosition, iRotation); - localRotation = localRotation * iRotation; - } - - entityTransform.Rotation = localRotation; - entityTransform.Position = localPosition - Vector3.Transform(collidable.CenterOfMass, localRotation); + job.Process(0, Simulation.Bodies.ActiveSet.Count); } } @@ -801,30 +766,63 @@ private void InterpolateTransforms() // a value of 0.5 means that we're halfway to the next physics update, just have to wait for the same amount of time. var interpolationFactor = (float)(_remainingUpdateTime.TotalSeconds / FixedTimeStep.TotalSeconds); interpolationFactor = MathF.Min(interpolationFactor, 1f); + var job = new InterpolateTransformsJob(interpolationFactor, _interpolatedBodies); if (ParallelUpdate) { - Dispatcher.For(0, _interpolatedBodies.Count, i => InterpolateBody(_interpolatedBodies[i], interpolationFactor)); + Dispatcher.ForBatched(_interpolatedBodies.Count, job); } else { - foreach (var body in _interpolatedBodies) + job.Process(0, _interpolatedBodies.Count); + } + } + + internal void Register(ISimulationUpdate simulationUpdateComponent) + { + Elider.AddToHandlers(simulationUpdateComponent, _simulationUpdateComponents); + } + + internal bool Unregister(ISimulationUpdate simulationUpdateComponent) + { + return Elider.RemoveFromHandlers(simulationUpdateComponent, _simulationUpdateComponents); + } + + internal void RegisterInterpolated(BodyComponent body) + { + _interpolatedBodies.Add(body); + + Debug.Assert(body.BodyReference.HasValue); + + body.PreviousPose = body.CurrentPose = body.BodyReference.Value.Pose; + } + + internal void UnregisterInterpolated(BodyComponent body) + { + _interpolatedBodies.Remove(body); + } + + private readonly struct InterpolateTransformsJob(float interpolationFactor, List bodies) : Dispatcher.IBatchJob + { + public void Process(int start, int endExclusive) + { + for (int i = start; i < endExclusive; i++) { - InterpolateBody(body, interpolationFactor); + InterpolateBody(bodies[i], interpolationFactor); } } - static void InterpolateBody(BodyComponent body, float interpolationFactor) + private static void InterpolateBody(BodyComponent body, float interpolationFactor) { // Have to go through our parents to make sure they're up-to-date since we're reading from the parent's world matrix // This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing - for (var item = body.Parent; item != null; item = item.Parent) + for (var parent = body.Parent; parent != null; parent = parent.Parent) { - if (item is BodyComponent parentBody && parentBody.InterpolationMode != InterpolationMode.None) + if (parent.InterpolationMode != InterpolationMode.None) { - InterpolateBody(parentBody, interpolationFactor); // This one will take care of his parents too. + InterpolateBody(parent, interpolationFactor); // This one will take care of his parents too. // This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc. // It's not that likely but should still be documented as suboptimal somewhere - parentBody.Entity.Transform.Parent.UpdateWorldMatrix(); + parent.Entity.Transform.UpdateWorldMatrix(); break; } } @@ -842,28 +840,45 @@ static void InterpolateBody(BodyComponent body, float interpolationFactor) } } - internal void Register(ISimulationUpdate simulationUpdateComponent) + private readonly struct SyncTransformsJob(Bodies bodies, BepuSimulation bepuSimulation) : Dispatcher.IBatchJob { - Elider.AddToHandlers(simulationUpdateComponent, _simulationUpdateComponents); - } - - internal bool Unregister(ISimulationUpdate simulationUpdateComponent) - { - return Elider.RemoveFromHandlers(simulationUpdateComponent, _simulationUpdateComponents); - } + public void Process(int start, int endExclusive) + { + for (int i = start; i < endExclusive; i++) + { + var bepuBody = bodies.GetBodyReference(bodies.ActiveSet.IndexToHandle[i]); + var strideBody = bepuSimulation.GetComponent(bepuBody); + SyncTransformsWithPhysics(bepuBody, strideBody); + } + } - internal void RegisterInterpolated(BodyComponent body) - { - _interpolatedBodies.Add(body); + private static void SyncTransformsWithPhysics(in BodyReference body, BodyComponent component) + { + if (component.Parent?.BodyReference is { } bRef) + { + // Have to go through our parents to make sure they're up to date since we're reading from the parent's world matrix + // This means that we're potentially updating bodies that are not part of the active set but checking that may be more costly than just doing the thing + SyncTransformsWithPhysics(bRef, component.Parent); + // This can be slower than expected when we have multiple collidables as parents recursively since we would recompute the topmost collidable n times, the second topmost n-1 etc. + // It's not that likely but should still be documented as suboptimal somewhere + component.Parent.Entity.Transform.UpdateWorldMatrix(); + } - Debug.Assert(body.BodyReference.HasValue); + var localPosition = body.Pose.Position.ToStride(); + var localRotation = body.Pose.Orientation.ToStride(); - body.PreviousPose = body.CurrentPose = body.BodyReference.Value.Pose; - } + var entityTransform = component.Entity.Transform; + if (entityTransform.Parent is { } parent) + { + parent.WorldMatrix.Decompose(out Vector3 _, out Quaternion parentEntityRotation, out Vector3 parentEntityPosition); + var iRotation = Quaternion.Invert(parentEntityRotation); + localPosition = Vector3.Transform(localPosition - parentEntityPosition, iRotation); + localRotation = localRotation * iRotation; + } - internal void UnregisterInterpolated(BodyComponent body) - { - _interpolatedBodies.Remove(body); + entityTransform.Rotation = localRotation; + entityTransform.Position = localPosition - Vector3.Transform(component.CenterOfMass, localRotation); + } } ///