diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index afd4bcee6c..58a0b50577 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/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() { diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponent.cs index 343d7bbe03..71becd1369 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponent.cs @@ -126,5 +126,26 @@ internal void TryUpdateDescription() } } + /// + public override float GetAccumulatedImpulseMagnitude() + { + if (_bepuSimulation != null && Attached) + return MathF.Sqrt(_bepuSimulation.Simulation.Solver.GetAccumulatedImpulseMagnitudeSquared(_cHandle)); + + return 0; + } + + /// + 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) { } } diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponentBase.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponentBase.cs index 39a9267954..8ec226ffad 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponentBase.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Constraints/_ConstraintComponentBase.cs @@ -53,6 +53,23 @@ protected BodyComponent? this[int i] /// May not be attached if it is not in a scene, when not , when any of its target is null, not in a scene or in a different simulation public abstract bool Attached { get; } + /// + /// Returns the squared sum of all impulses this constraint applied on the last tick + /// + /// + /// Impulses increase depending on , as well as the amount of . + /// You may want to use instead. + /// + public abstract float GetAccumulatedImpulseMagnitude(); + + /// + /// Returns the squared sum of all forces this constraint applied on the last tick + /// + /// + /// This can be used to compare with a given motor constraints' MaximumForce property for example. + /// + public abstract float GetAccumulatedForceMagnitude(); + protected abstract void BodiesChanged(); internal abstract void Activate(BepuConfiguration bepuConfig);