fix: Bepu transformation for bodies children of bodies (#3013)

Author: Eideren

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/BepuSimulation.cs

@@ -557,7 +557,7 @@ public ConversionEnum<ManagedConverter, OverlapInfoStack, OverlapInfo> OverlapIn
     /// <summary>
     /// Called by the BroadPhase.GetOverlaps to collect all encountered collidables.
     /// </summary>
-    struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
+    private struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
     {
         public QuickList<CollidableReference> 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)
     /// <summary>
     /// Provides callbacks for filtering and data collection to the CollisionBatcher we'll be using to test query shapes against the detected environment.
     /// </summary>
-    struct BatcherCallbacks<T> : ICollisionCallbacks where T : IOverlapCollector
+    private struct BatcherCallbacks<T> : ICollisionCallbacks where T : IOverlapCollector
     {
         public required CollisionMask CollisionMask;
         public required QuickList<CollidableReference> References;
@@ -605,7 +605,7 @@ public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where
         }
     }
 
-    unsafe void OverlapInner<TShape, TCollector>(in TShape shape, in SRigidPose pose, CollisionMask collisionMask, ref TCollector collector) where TShape : unmanaged, IConvexShape where TCollector : IOverlapCollector
+    private unsafe void OverlapInner<TShape, TCollector>(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<BodyComponent> 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);
+        }
     }
 
     /// <summary>