diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderComponent.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderComponent.cs index ed77314830..d2f2616868 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderComponent.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderComponent.cs @@ -5,6 +5,7 @@ using Stride.Engine; using Stride.Engine.Design; using Stride.Input; +using static Stride.BepuPhysics.Debug.DebugRenderProcessor; namespace Stride.BepuPhysics.Debug; @@ -14,21 +15,34 @@ namespace Stride.BepuPhysics.Debug; public class DebugRenderComponent : SyncScript { internal DebugRenderProcessor? _processor; - bool _state = true; + bool _visibleState = true; + SynchronizationMode _modeState = SynchronizationMode.Physics; public Keys Key { get; set; } = Keys.F11; [DataMember] public bool Visible { - get => _processor?.Visible ?? _state; + get => _processor?.Visible ?? _visibleState; set { - _state = value; + _visibleState = value; if (_processor is not null) _processor.Visible = value; } } + [DataMember] + public SynchronizationMode Mode + { + get => _processor?.Mode ?? _modeState; + set + { + _modeState = value; + if (_processor is not null) + _processor.Mode = value; + } + } + public override void Update() { diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderProcessor.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderProcessor.cs index 427c0489a7..f9728ec999 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderProcessor.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Debug/DebugRenderProcessor.cs @@ -1,6 +1,7 @@ // 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.Collidables; using Stride.BepuPhysics.Debug.Effects; using Stride.BepuPhysics.Debug.Effects.RenderFeatures; using Stride.BepuPhysics.Definitions; @@ -17,6 +18,7 @@ namespace Stride.BepuPhysics.Debug; public class DebugRenderProcessor : EntityProcessor { public SynchronizationMode Mode { get; set; } = SynchronizationMode.Physics; // Setting it to Physics by default to show when there is a large discrepancy between the entity and physics + public bool ShowCollisions { get; set; } = true; private bool _latent; private bool _visible; @@ -26,6 +28,43 @@ public class DebugRenderProcessor : EntityProcessor private VisibilityGroup _visibilityGroup = null!; private readonly Dictionary _wireFrameRenderObject = new(); + // Collision gizmo pooling: + private readonly List _collisionGizmos = new(); + private int _collisionGizmosUsedThisFrame; + + private sealed class CollisionGizmo + { + public WireFrameRenderObject Point = null!; + public WireFrameRenderObject Shaft = null!; + public WireFrameRenderObject Tip = null!; + + public void SetVisible(bool visible, VisibilityGroup visibilityGroup) + { + if (visible && !Point.Enabled) + { + visibilityGroup.RenderObjects.Add(Point); + visibilityGroup.RenderObjects.Add(Shaft); + visibilityGroup.RenderObjects.Add(Tip); + } + else if (!visible && Point.Enabled) + { + visibilityGroup.RenderObjects.Remove(Point); + visibilityGroup.RenderObjects.Remove(Shaft); + visibilityGroup.RenderObjects.Remove(Tip); + } + Point.Enabled = visible; + Shaft.Enabled = visible; + Tip.Enabled = visible; + } + + public void Dispose() + { + Point.Dispose(); + Shaft.Dispose(); + Tip.Dispose(); + } + } + public DebugRenderProcessor() { Order = SystemsOrderHelper.ORDER_OF_DEBUG_P; @@ -36,7 +75,7 @@ public bool Visible get => _visible; set { - if (_sceneSystem.SceneInstance.GetProcessor() is { } proc) + if (_sceneSystem.SceneInstance.GetProcessor() is { } proc && _visibilityGroup is not null) { if (_visible == value) return; @@ -66,29 +105,19 @@ public bool Visible protected override void OnEntityComponentAdding(Entity entity, DebugRenderComponent component, DebugRenderComponent data) { base.OnEntityComponentAdding(entity, component, data); - if (_sceneSystem.SceneInstance.GetProcessor() is not null) + if (_sceneSystem?.SceneInstance?.GetProcessor() is not null && _visibilityGroup is not null) Visible = component.Visible; else if (component.Visible) _latent = true; - + Mode = component.Mode; component._processor = this; } protected override void OnSystemAdd() { - SinglePassWireframeRenderFeature wireframeRenderFeature; - _shapeCacheSystem = Services.GetOrCreate(); _game = Services.GetSafeServiceAs(); _sceneSystem = Services.GetSafeServiceAs(); - - if (_sceneSystem.GraphicsCompositor.RenderFeatures.OfType().FirstOrDefault() is null) - { - wireframeRenderFeature = new(); - _sceneSystem.GraphicsCompositor.RenderFeatures.Add(wireframeRenderFeature); - } - - _visibilityGroup = _sceneSystem.SceneInstance.VisibilityGroups.First(); } protected override void OnSystemRemove() @@ -98,6 +127,18 @@ protected override void OnSystemRemove() public override void Draw(RenderContext context) { + if (_visibilityGroup is null) + { + if (_sceneSystem.SceneInstance.VisibilityGroups.Count == 0) + return; + + _visibilityGroup = _sceneSystem.SceneInstance.VisibilityGroups.First(); + if (_sceneSystem.GraphicsCompositor.RenderFeatures.OfType().FirstOrDefault() is null) + { + _sceneSystem.GraphicsCompositor.RenderFeatures.Add(new SinglePassWireframeRenderFeature()); + } + } + if (_latent) { Visible = true; @@ -107,8 +148,13 @@ public override void Draw(RenderContext context) base.Draw(context); - foreach (var (collidable, (wireframes, cache)) in _wireFrameRenderObject) + // Collect sims that have at least one tracked collidable. + var simulations = new List(); + foreach (var (collidable, (wireframes, _)) in _wireFrameRenderObject) { + if (collidable.Simulation != null && !simulations.Contains(collidable.Simulation)) + simulations.Add(collidable.Simulation); + Matrix matrix; switch (Mode) { @@ -142,15 +188,103 @@ public override void Draw(RenderContext context) wireframe.Color = GetCurrentColor(collidable); } } + + // Collisions rendering (pooled) + if (!ShowCollisions) + { + // Ensure physics doesn't waste time collecting. + for (int s = 0; s < simulations.Count; s++) + simulations[s].ContactEvents.DebugCollectAllContacts = false; + + HideAllCollisionGizmos(); + return; + } + + _collisionGizmosUsedThisFrame = 0; + + for (int s = 0; s < simulations.Count; s++) + { + var sim = simulations[s]; + sim.ContactEvents.DebugCollectAllContacts = true; + + var points = sim.ContactEvents.DebugPoints; + if (points.Length == 0) + continue; + + EnsureCollisionGizmoCapacity(_collisionGizmosUsedThisFrame + points.Length); + + for (int i = 0; i < points.Length; i++) + { + var p = points[i]; + + // Visual parameters + float pointRadius = 0.05f; + float normalLength = 0.35f; + float shaftRadius = 0.02f; + float tipRadius = 0.035f; + + var n = p.WorldNormal; + if (n.LengthSquared() < 1e-6f) + continue; + + n.Normalize(); + + var start = p.WorldPoint; + var end = start + n * normalLength; + var mid = (start + end) * 0.5f; + + // Align cylinder Y axis to normal + var rot = Quaternion.BetweenDirections(Vector3.UnitY, n); + + var gizmo = _collisionGizmos[_collisionGizmosUsedThisFrame++]; + + // 1) contact point sphere + { + gizmo.Point.Color = Color.Red; + + var scale = new Vector3(pointRadius); + Matrix.Scaling(ref scale, out gizmo.Point.CollidableBaseMatrix); + + Matrix world; + Matrix.Translation(ref start, out world); + + gizmo.Point.WorldMatrix = gizmo.Point.CollidableBaseMatrix * world; + } + + // 2) normal shaft (cylinder) + { + gizmo.Shaft.Color = Color.Yellow; + + var shaftScale = new Vector3(shaftRadius, normalLength, shaftRadius); + + // WorldMatrix directly + Matrix.Transformation(ref shaftScale, ref rot, ref mid, out gizmo.Shaft.WorldMatrix); + gizmo.Shaft.CollidableBaseMatrix = gizmo.Shaft.WorldMatrix; + } + + // 3) normal tip (sphere) + { + gizmo.Tip.Color = Color.Yellow; + + var tipScale = new Vector3(tipRadius); + Matrix.Transformation(ref tipScale, ref rot, ref end, out gizmo.Tip.WorldMatrix); + gizmo.Tip.CollidableBaseMatrix = gizmo.Tip.WorldMatrix; + } + + gizmo.SetVisible(true, _visibilityGroup); + } + } + + // Hide unused + for (int i = _collisionGizmosUsedThisFrame; i < _collisionGizmos.Count; i++) + _collisionGizmos[i].SetVisible(false, _visibilityGroup); } private void StartTracking(CollidableProcessor proc) { var shapeAndOffsets = new List(); for (var collidables = proc.ComponentDataEnumerator; collidables.MoveNext();) - { StartTrackingCollidable(collidables.Current.Key, shapeAndOffsets); - } } private void StartTrackingCollidable(CollidableComponent collidable) => StartTrackingCollidable(collidable, new()); @@ -177,10 +311,11 @@ private void StartTrackingCollidable(CollidableComponent collidable, List, /// and 1 would return true only when a surface matches exactly. /// - protected bool GroundTest(NVector3 groundNormal, float threshold = 0f) + protected bool GroundTest(NVector3 groundNormal, float threshold = 0.1f) { IsGrounded = false; if (Simulation == null || Contacts.Count == 0) diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs index dcd5adaddb..976d3302a1 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Definitions/Contacts/ContactEventsManager.cs @@ -25,7 +25,10 @@ internal class ContactEventsManager : IDisposable private IndexSet _staticListenerFlags; private IndexSet _bodyListenerFlags; private IPerTypeManifoldStore[][] _manifoldStoresPerWorker; - + private readonly List[] _debugPointsPerWorker; + private readonly List _debugPointsMerged = new(256); + internal bool DebugCollectAllContacts; + internal ReadOnlySpan DebugPoints => CollectionsMarshal.AsSpan(_debugPointsMerged); public ContactEventsManager(BufferPool pool, BepuSimulation simulation, int workerCount) { _pool = pool; @@ -33,6 +36,9 @@ public ContactEventsManager(BufferPool pool, BepuSimulation simulation, int work _manifoldStoresPerWorker = new IPerTypeManifoldStore[workerCount][]; for (int i = 0; i < _manifoldStoresPerWorker.Length; i++) _manifoldStoresPerWorker[i] = []; + _debugPointsPerWorker = new List[workerCount]; + for (int i = 0; i < workerCount; i++) + _debugPointsPerWorker[i] = new List(128); } public void Initialize() @@ -163,10 +169,50 @@ public void StoreManifold(int workerIndex, CollidablePair pair, int c { bool aListener = IsRegistered(pair.A); bool bListener = IsRegistered(pair.B); - if (aListener == false && bListener == false) + if (aListener == false && bListener == false && !DebugCollectAllContacts) return; - IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, pair, childIndexA, childIndexB); + if (aListener || bListener) + IPerTypeManifoldStore.StoreManifold(_manifoldStoresPerWorker, workerIndex, ref manifold, pair, childIndexA, childIndexB); + + CollectDebugContacts(workerIndex, pair, ref manifold); + } + private void CollectDebugContacts(int workerIndex, CollidablePair pair, ref TManifold manifold) + where TManifold : unmanaged, IContactManifold + { + var compA = _simulation.GetComponent(pair.A); + if (compA.Pose is not { } poseA) + return; + + var worldRotStride = poseA.Orientation.ToStride(); + var worldOriginStride = poseA.Position.ToStride(); + + for (int j = 0; j < manifold.Count; j++) + { + float depth = manifold.GetDepth(j); + if (depth < 0) + continue; + + var offsetN = manifold.GetOffset(j); + var normalN = manifold.GetNormal(j); + + var offsetStride = offsetN.ToStride(); + var normalStride = normalN.ToStride(); + + var worldPointStride = worldOriginStride + offsetStride; + + var worldNormalStride = normalStride;// Vector3.Transform(normalStride, worldRotStride); + + _debugPointsPerWorker[workerIndex].Add(new DebugContactPoint + { + WorldPoint = worldPointStride, + WorldNormal = worldNormalStride, + Depth = depth, + PackedA = pair.A.Packed, + PackedB = pair.B.Packed + }); + } + } private void RunManifoldEvent(Span> unsafeInfos) where TManifold : unmanaged, IContactManifold @@ -283,6 +329,17 @@ public void Flush() //Remove any stale collisions. Stale collisions are those which should have received a new manifold update but did not because the manifold is no longer active. foreach (var pair in _outdatedPairs) ClearCollision(pair); + + _debugPointsMerged.Clear(); + for (int i = 0; i < _debugPointsPerWorker.Length; i++) + { + var list = _debugPointsPerWorker[i]; + if (list.Count > 0) + { + _debugPointsMerged.AddRange(list); + list.Clear(); + } + } } /// @@ -445,6 +502,14 @@ private enum Events TouchingB = 0b10, } + internal struct DebugContactPoint + { + public Stride.Core.Mathematics.Vector3 WorldPoint; + public Stride.Core.Mathematics.Vector3 WorldNormal; + public float Depth; + public uint PackedA; + public uint PackedB; + } private struct EmptyManifold : IContactManifold {