feat: Bepu, getter to retrieve the impulse and force applied by a given constraint (#2930)

Author: Eideren

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs

@@ -124,6 +124,43 @@ public static void ConstraintsTest()
             RunGameTest(game);
         }
 
+        [Fact]
+        public static void ConstraintsForceTest()
+        {
+            var game = new GameTest();
+            game.Script.AddTask(async () =>
+            {
+                game.ScreenShotAutomationEnabled = false;
+
+                var e1 = new BodyComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider() } } };
+                var c = new OneBodyLinearMotorConstraintComponent
+                {
+                    A = e1,
+                    LocalOffset = Vector3.Zero,
+                    MotorMaximumForce = 3,
+                    MotorDamping = 1,
+                };
+
+                Assert.Equal(0f, c.GetAccumulatedForceMagnitude());
+
+                game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new EntityComponent[] { e1, c }.Select(x => new Entity { x }));
+
+                Assert.Equal(0f, c.GetAccumulatedForceMagnitude());
+
+                do
+                {
+                    await e1.Simulation!.AfterUpdate();
+
+                    // Given current gravity, the constraint should pull the body in under 5 seconds,
+                    // otherwise something is wrong and this loop would likely continue indefinitely
+                    Assert.True(game.UpdateTime.Total.TotalSeconds < 5d);
+                } while (c.GetAccumulatedForceMagnitude() < float.BitDecrement(c.MotorMaximumForce));
+
+                game.Exit();
+            });
+            RunGameTest(game);
+        }
+
         [Fact]
         public static void OnContactRemovalTest()
         {

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponent.cs

@@ -126,5 +126,26 @@ internal void TryUpdateDescription()
         }
     }
 
+    /// <inheritdoc/>
+    public override float GetAccumulatedImpulseMagnitude()
+    {
+        if (_bepuSimulation != null && Attached)
+            return MathF.Sqrt(_bepuSimulation.Simulation.Solver.GetAccumulatedImpulseMagnitudeSquared(_cHandle));
+
+        return 0;
+    }
+
+    /// <inheritdoc/>
+    public override float GetAccumulatedForceMagnitude()
+    {
+        if (_bepuSimulation != null && Attached)
+        {
+            float impulses = GetAccumulatedImpulseMagnitude();
+            return 1f / (float)_bepuSimulation.FixedTimeStep.TotalSeconds * _bepuSimulation.SolverSubStep * impulses;
+        }
+
+        return 0;
+    }
+
     protected ConstraintComponent(int bodies) : base(bodies) { }
 }

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponentBase.cs

@@ -53,6 +53,23 @@ protected BodyComponent? this[int i]
     /// <remarks> May not be attached if it is not in a scene, when not <see cref="Enabled"/>, when any of its target is null, not in a scene or in a different simulation </remarks>
     public abstract bool Attached { get; }
 
+    /// <summary>
+    /// Returns the squared sum of all impulses this constraint applied on the last tick
+    /// </summary>
+    /// <remarks>
+    /// Impulses increase depending on <see cref="BepuSimulation.FixedTimeStep"/>, as well as the amount of <see cref="BepuSimulation.SolverSubStep"/>.
+    /// You may want to use <see cref="GetAccumulatedForceMagnitude"/> instead.
+    /// </remarks>
+    public abstract float GetAccumulatedImpulseMagnitude();
+
+    /// <summary>
+    /// Returns the squared sum of all forces this constraint applied on the last tick
+    /// </summary>
+    /// <remarks>
+    /// This can be used to compare with a given motor constraints' MaximumForce property for example.
+    /// </remarks>
+    public abstract float GetAccumulatedForceMagnitude();
+
     protected abstract void BodiesChanged();
 
     internal abstract void Activate(BepuConfiguration bepuConfig);