diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs index 945772df4b..48cd307a08 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Tests/BepuTests.cs @@ -79,6 +79,221 @@ public static void MatrixTest() Assert.Equal(new CollisionMatrix(), collisions); } + [Fact] + public static void Body2DConstructorSetsInterpolatedMode() + { + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + }; + + Assert.Equal(InterpolationMode.Interpolated, body.InterpolationMode); + } + + [Fact] + public static void Body2DZToleranceRejectsInvalidValues() + { + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + }; + + Assert.Equal(0.001f, body.ZTolerance); + + body.ZTolerance = 0.25f; + Assert.Equal(0.25f, body.ZTolerance); + + body.ZTolerance = 0f; + Assert.Equal(0.001f, body.ZTolerance); + + body.ZTolerance = -1f; + Assert.Equal(0.001f, body.ZTolerance); + + body.ZTolerance = float.NaN; + Assert.Equal(0.001f, body.ZTolerance); + + body.ZTolerance = float.PositiveInfinity; + Assert.Equal(0.001f, body.ZTolerance); + } + + [Fact] + public static void Body2DSimulationUpdateConstrainsPlaneMotion() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + try + { + game.ScreenShotAutomationEnabled = false; + + var entity = new Entity(); + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + }; + + entity.Transform.Position = new Vector3(0, 0, 2f); + entity.Add(body); + game.SceneSystem.SceneInstance.RootScene.Entities.Add(entity); + + await body.Simulation!.AfterUpdate(); + + body.AngularVelocity = new Vector3(2f, -3f, 4f); + body.LinearVelocity = new Vector3(0f, 0f, 5f); + + await body.Simulation.AfterUpdate(); + + Assert.InRange(body.LinearVelocity.Z, -1.01f, -0.98f); + Assert.InRange(body.AngularVelocity.X, -0.001f, 0.001f); + Assert.InRange(body.AngularVelocity.Y, -0.001f, 0.001f); + Assert.InRange(body.AngularVelocity.Z, 3.99f, 4.01f); + } + finally + { + game.Exit(); + } + }); + RunGameTest(game); + } + + [Fact] + public static void Body2DSimulationUpdateConvergesToPlane() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + try + { + game.ScreenShotAutomationEnabled = false; + + var entity = new Entity(); + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + // A wider band than the default, so the body converges in about a second of + // simulated time rather than the six the default would need from this offset + ZTolerance = 0.01f, + }; + + entity.Transform.Position = new Vector3(0, 0, 0.05f); + entity.Add(body); + game.SceneSystem.SceneInstance.RootScene.Entities.Add(entity); + + var simulation = body.Simulation!; + + // The correction pulls the body back at a velocity equal to the error, so the + // offset decays exponentially rather than snapping - this proves it converges + while (MathF.Abs(body.Position.Z) > body.ZTolerance) + { + await simulation.AfterUpdate(); + + Assert.True(game.UpdateTime.Total.TotalSeconds < 10d, "The body never returned to the Z = 0 plane."); + } + + Assert.InRange(body.Position.Z, -body.ZTolerance, body.ZTolerance); + } + finally + { + game.Exit(); + } + }); + RunGameTest(game); + } + + [Fact] + public static void Body2DFallsAsleepOnceSettled() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + try + { + game.ScreenShotAutomationEnabled = false; + + var floor = new Entity { new StaticComponent { Collider = new CompoundCollider { Colliders = { new BoxCollider { Size = new(10, 1, 10) } } } } }; + var entity = new Entity(); + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + }; + + floor.Transform.Position = new Vector3(0, -2, 0); + // Started on the plane, so the positional correction has nothing to do and cannot + // be what keeps the body awake + entity.Transform.Position = new Vector3(0, 0, 0); + entity.Add(body); + game.SceneSystem.SceneInstance.RootScene.Entities.AddRange(new[] { floor, entity }); + + var simulation = body.Simulation!; + + // Sleeping is what makes large 2D scenes cheap: a component that writes velocity + // every step would hold bodies awake forever and this would never terminate + while (body.Awake) + { + await simulation.AfterUpdate(); + + Assert.True(game.UpdateTime.Total.TotalSeconds < 15d, "The body never fell asleep, so something is writing to it every step."); + } + + Assert.False(body.Awake); + } + finally + { + game.Exit(); + } + }); + RunGameTest(game); + } + + [Fact] + public static void Body2DKeepsRotationLockAfterKinematicToggle() + { + var game = new GameTest(); + game.Script.AddTask(async () => + { + try + { + game.ScreenShotAutomationEnabled = false; + + var entity = new Entity(); + var body = new Body2DComponent + { + Collider = new CompoundCollider { Colliders = { new BoxCollider() } }, + }; + + entity.Add(body); + game.SceneSystem.SceneInstance.RootScene.Entities.Add(entity); + + var simulation = body.Simulation!; + + await simulation.AfterUpdate(); + + // Going kinematic and back restores the body's full shape inertia, which silently + // undoes the lock applied at attach time unless it is reapplied + body.Kinematic = true; + + await simulation.AfterUpdate(); + + body.Kinematic = false; + + await simulation.AfterUpdate(); + + var inverseInertia = body.BodyInertia.InverseInertiaTensor; + + Assert.Equal(0f, inverseInertia.XX); + Assert.Equal(0f, inverseInertia.YY); + + // Only X and Y are locked - the body must still be able to roll in the plane + Assert.True(inverseInertia.ZZ > 0f, "Rotation about Z was locked too, so the body can no longer roll."); + } + finally + { + game.Exit(); + } + }); + RunGameTest(game); + } + [Fact] public static void ConstraintsTest() { diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Body2DComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Body2DComponent.cs deleted file mode 100644 index 4471f27daf..0000000000 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Body2DComponent.cs +++ /dev/null @@ -1,47 +0,0 @@ -// 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; -using BepuPhysics.Collidables; -using Stride.Core; -using Stride.Core.Mathematics; -using Stride.Engine; - -namespace Stride.BepuPhysics -{ - [ComponentCategory("Bepu")] - public class Body2DComponent : BodyComponent - { - Vector3 _rotationLock = new Vector3(0, 0, 0); - - [DataMemberIgnore] - internal Vector3 RotationLock - { - get - { - return _rotationLock; - } - set - { - _rotationLock = value; - if (BodyReference is { } bRef) - { - bRef.LocalInertia.InverseInertiaTensor.XX *= value.X; - bRef.LocalInertia.InverseInertiaTensor.YX *= value.X * value.Y; - bRef.LocalInertia.InverseInertiaTensor.ZX *= value.Z * value.X; - bRef.LocalInertia.InverseInertiaTensor.YY *= value.Y; - bRef.LocalInertia.InverseInertiaTensor.ZY *= value.Z * value.Y; - bRef.LocalInertia.InverseInertiaTensor.ZZ *= value.Z; - } - } - } - - protected override void AttachInner(RigidPose pose, BodyInertia shapeInertia, TypedIndex shapeIndex) - { - base.AttachInner(pose, shapeInertia, shapeIndex); -#warning what about a body that become kinematic after some time ? - if (!Kinematic) - RotationLock = new Vector3(0, 0, 1); - } - } -} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Module.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Module.cs deleted file mode 100644 index 4c09c41e0d..0000000000 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Module.cs +++ /dev/null @@ -1,17 +0,0 @@ -// 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.Reflection; -using Stride.Core.Reflection; - -namespace Stride.BepuPhysics._2D -{ - internal class Module - { - [Stride.Core.ModuleInitializer] - public static void Initialize() - { - AssemblyRegistry.Register(typeof(Module).GetTypeInfo().Assembly, AssemblyCommonCategories.Assets); - } - } -} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Simulation2DComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Simulation2DComponent.cs deleted file mode 100644 index 7fcefbaae5..0000000000 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Simulation2DComponent.cs +++ /dev/null @@ -1,49 +0,0 @@ -// 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 Stride.BepuPhysics.Components; -using Stride.Core.Mathematics; -using Stride.Engine; - -namespace Stride.BepuPhysics._2D; - -[ComponentCategory("Bepu")] -public class Simulation2DComponent : SyncScript, ISimulationUpdate -{ - //public float MaxZLiberty { get; set; } = 0.05f; - - public void SimulationUpdate(BepuSimulation sim, float simTimeStep) - { - - } - public void AfterSimulationUpdate(BepuSimulation sim, float simTimeStep) - { - for (int i = 0; i < sim.Simulation.Bodies.ActiveSet.Count; i++) - { - var handle = sim.Simulation.Bodies.ActiveSet.IndexToHandle[i]; - var body = sim.GetComponent(handle); - - if (body is not Body2DComponent) - continue; - - //if (body.Position.Z > MaxZLiberty || body.Position.Z < -MaxZLiberty) - if (body.Position.Z != 0) - body.Position *= new Vector3(1, 1, 0);//Fix Z = 0 - //if (body.LinearVelocity.Z > MaxZLiberty || body.LinearVelocity.Z < -MaxZLiberty) - if (body.LinearVelocity.Z != 0) - body.LinearVelocity *= new Vector3(1, 1, 0); - - var bodyRot = body.Orientation; - Quaternion.RotationYawPitchRoll(ref bodyRot, out var yaw, out var pitch, out var roll); - //if (yaw > MaxZLiberty || pitch > MaxZLiberty || yaw < -MaxZLiberty || pitch < -MaxZLiberty) - if (yaw != 0 || pitch != 0) - body.Orientation = Quaternion.RotationYawPitchRoll(0, 0, roll); - //if (body.AngularVelocity.X > MaxZLiberty || body.AngularVelocity.Y > MaxZLiberty || body.AngularVelocity.X < -MaxZLiberty || body.AngularVelocity.Y < -MaxZLiberty) - if (body.AngularVelocity.X != 0 || body.AngularVelocity.Y != 0) - body.AngularVelocity *= new Vector3(0, 0, 1); - } - } - public override void Update() - { - } -} diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Stride.BepuPhysics._2D.csproj b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Stride.BepuPhysics._2D.csproj deleted file mode 100644 index 991b3b10be..0000000000 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics._2D/Stride.BepuPhysics._2D.csproj +++ /dev/null @@ -1,17 +0,0 @@ - - - - $(StrideRuntimeTargetFrameworks) - enable - enable - true - --serialization --parameter-key - true - - - - - - - - diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Body2DComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Body2DComponent.cs new file mode 100644 index 0000000000..006920dbad --- /dev/null +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Body2DComponent.cs @@ -0,0 +1,215 @@ +// 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; +using BepuPhysics.Collidables; +using Stride.BepuPhysics.Components; +using Stride.BepuPhysics.Definitions; +using Stride.BepuPhysics.Definitions.Colliders; +using Stride.Core; +using Stride.Engine; +using NRigidPose = BepuPhysics.RigidPose; + +namespace Stride.BepuPhysics; + +/// +/// A dynamic body confined to the XY plane, simulated by Bepu's ordinary 3D solver. +/// +/// +/// +/// Planar behavior is enforced in two places: X/Y rotation is locked by zeroing the corresponding +/// inverse-inertia terms when the body attaches, and Z drift is corrected by setting linear Z velocity +/// before each solve. +/// +/// +/// The positional correction is velocity-based (not teleport-based) so contact resolution stays stable. +/// Sleeping bodies are left untouched. +/// +/// +/// For hull colliders, attach-time tuning applies conservative contact settings: it caps +/// and +/// , and raises +/// to at least one. +/// Set values after attach if you want stricter behavior. +/// +/// +[ComponentCategory("Physics - Bepu 2D")] +public class Body2DComponent : BodyComponent, ISimulationUpdate +{ + /// Cap on recovery velocity for hull colliders, which are prone to energetic corrections. + private const float HullMaximumRecoveryVelocity = 1.5f; + + /// Minimum contact spring damping for hull colliders, to settle piles rather than bounce them. + private const float HullSpringDampingRatio = 1f; + + /// Cap on contact spring frequency for hull colliders; stiffer springs fight the substep count. + private const float HullSpringFrequency = 30f; + + /// + /// Ceiling on the plane-restoring speed, in world units per second. + /// + /// + /// This mainly guards extreme cases (for example, a body spawned far from the plane) by preventing + /// an overly aggressive snap-back velocity. + /// + private const float MaximumCorrectionSpeed = 1f; + + /// + /// Tracks the kinematic state the rotation lock was applied for, so it can be restored when the + /// body switches back to dynamic and Bepu reinstates the full shape inertia. + /// + private bool _lockedWhileKinematic; + + /// One millimetre at Stride's default scale. + private const float DefaultZTolerance = 0.001f; + + private float _zTolerance = DefaultZTolerance; + + /// + /// Gets or sets how far the body may drift off the Z = 0 plane before it is pulled back, in world + /// units. Defaults to 0.001 (one millimetre at Stride's default scale). + /// + /// + /// Out-of-plane velocity is always cleared; this value only controls when positional correction + /// starts. Invalid values (non-finite or non-positive) are replaced with the default. + /// + [Display("Z tolerance", category: CategoryActivity)] + public float ZTolerance + { + get => _zTolerance; + set => _zTolerance = float.IsFinite(value) && value > 0f ? value : DefaultZTolerance; + } + + /// + /// Initializes a new with interpolation enabled, so rendering stays + /// smooth when the display refreshes faster than the fixed physics step. + /// + public Body2DComponent() => InterpolationMode = InterpolationMode.Interpolated; + + /// + /// + /// Preserves Z-axis rotation while locking X/Y rotation. Hull colliders also receive softer default + /// contact tuning. + /// + protected override void AttachInner(NRigidPose pose, BodyInertia shapeInertia, TypedIndex shapeIndex) + { + base.AttachInner(pose, shapeInertia, shapeIndex); + + ApplyRotationLock(); + + if (!HasConvexHull(Collider)) return; + + MaximumRecoveryVelocity = MathF.Min(MaximumRecoveryVelocity, HullMaximumRecoveryVelocity); + SpringDampingRatio = MathF.Max(SpringDampingRatio, HullSpringDampingRatio); + SpringFrequency = MathF.Min(SpringFrequency, HullSpringFrequency); + } + + /// + /// Confines the body to the plane, before the solver runs for this step. + /// + /// The simulation stepping this body. + /// The fixed time step, in seconds. + /// + /// Runs before solve so correction participates in contact resolution. Sleeping bodies are skipped, + /// but the rotation lock is refreshed first so state stays valid across kinematic changes. + /// + /// Z correction uses a bounded velocity target (not teleporting), with a gentle proportional pull + /// toward the plane. is intentionally unused. + /// + public virtual void SimulationUpdate(BepuSimulation sim, float simTimeStep) + { + if (BodyReference is not { } bodyRef) return; + + // Deliberately ahead of the sleep check. Turning off Kinematic hands the body its full shape + // inertia back, and if that happened while it slept it would be free to tumble during the + // first solve after waking - the lock freezes rotation rather than correcting it, so any tilt + // picked up in that one step would stay for good + RestoreRotationLockIfKinematicChanged(); + + if (!bodyRef.Awake) return; + + // Out-of-plane velocity is never wanted. Removing it even inside the tolerance band is what + // stops slow drift accumulating until it crosses the threshold + var zError = bodyRef.Pose.Position.Z; + var targetVelocityZ = MathF.Abs(zError) > ZTolerance + ? Math.Clamp(-zError, -MaximumCorrectionSpeed, MaximumCorrectionSpeed) + : 0f; + + if (bodyRef.Velocity.Linear.Z != targetVelocityZ) + { + bodyRef.Velocity.Linear.Z = targetVelocityZ; + } + + // Rotation about X and Y is already impossible, but a velocity can survive from before the + // body attached or from a direct assignment + if (bodyRef.Velocity.Angular.X != 0f || bodyRef.Velocity.Angular.Y != 0f) + { + bodyRef.Velocity.Angular.X = 0f; + bodyRef.Velocity.Angular.Y = 0f; + } + } + + /// + /// Does nothing. The whole correction happens before the solve, in . + /// + /// The simulation that stepped this body. + /// The fixed time step, in seconds. + public virtual void AfterSimulationUpdate(BepuSimulation sim, float simTimeStep) { } + + /// + /// Removes the body's ability to rotate about X and Y, leaving Z free. + /// + private void ApplyRotationLock() + { + var inertia = BodyInertia; + var inverseInertia = inertia.InverseInertiaTensor; + + inverseInertia.XX = 0f; + inverseInertia.YY = 0f; + inverseInertia.YX = 0f; + inverseInertia.ZX = 0f; + inverseInertia.ZY = 0f; // ZZ is left alone, so the body can still roll in the plane + + inertia.InverseInertiaTensor = inverseInertia; + BodyInertia = inertia; + + _lockedWhileKinematic = Kinematic; + } + + /// + /// Reapplies the rotation lock after a switch between kinematic and dynamic. + /// + /// + /// Turning off restores the body's full shape inertia, which + /// silently undoes the lock applied at attach time and would let the body tumble out of the plane. + /// + private void RestoreRotationLockIfKinematicChanged() + { + if (_lockedWhileKinematic == Kinematic) return; + + ApplyRotationLock(); + } + + /// + /// Determines whether a collider contains at least one . + /// + /// The collidable's collider, which may be . + /// if a convex hull is present. + /// + /// In this collider model, hull colliders appear as children of . + /// A single pass over direct children is therefore sufficient. + /// + private static bool HasConvexHull(ICollider? collider) + { + if (collider is not CompoundCollider compound) return false; + + var colliders = compound.Colliders; + + for (var i = 0; i < colliders.Count; i++) + { + if (colliders[i] is ConvexHullCollider) return true; + } + + return false; + } +}