diff --git a/Robust.Client/Physics/BroadPhaseSystem.cs b/Robust.Client/Physics/BroadPhaseSystem.cs index 9e94d564960..4966899c7a3 100644 --- a/Robust.Client/Physics/BroadPhaseSystem.cs +++ b/Robust.Client/Physics/BroadPhaseSystem.cs @@ -1,13 +1,12 @@ using Robust.Shared.Physics.Systems; -namespace Robust.Client.Physics +namespace Robust.Client.Physics; + +internal sealed class BroadPhaseSystem : SharedBroadphaseSystem { - internal sealed class BroadPhaseSystem : SharedBroadphaseSystem + public override void Initialize() { - public override void Initialize() - { - base.Initialize(); - UpdatesBefore.Add(typeof(PhysicsSystem)); - } + base.Initialize(); + UpdatesBefore.Add(typeof(PhysicsSystem)); } } diff --git a/Robust.Server/Physics/BroadPhaseSystem.cs b/Robust.Server/Physics/BroadPhaseSystem.cs index 9613eb10eae..e0d780106dd 100644 --- a/Robust.Server/Physics/BroadPhaseSystem.cs +++ b/Robust.Server/Physics/BroadPhaseSystem.cs @@ -2,14 +2,13 @@ using Robust.Shared.Physics; using Robust.Shared.Physics.Systems; -namespace Robust.Server.Physics +namespace Robust.Server.Physics; + +internal sealed class BroadPhaseSystem : SharedBroadphaseSystem { - internal sealed class BroadPhaseSystem : SharedBroadphaseSystem + public override void Initialize() { - public override void Initialize() - { - base.Initialize(); - UpdatesBefore.Add(typeof(PhysicsSystem)); - } + base.Initialize(); + UpdatesBefore.Add(typeof(PhysicsSystem)); } } diff --git a/Robust.Server/Physics/GridFixtureSystem.cs b/Robust.Server/Physics/GridFixtureSystem.cs index 873e2e7c31e..3373e4178ba 100644 --- a/Robust.Server/Physics/GridFixtureSystem.cs +++ b/Robust.Server/Physics/GridFixtureSystem.cs @@ -8,7 +8,6 @@ using Robust.Shared.Configuration; using Robust.Shared.GameObjects; using Robust.Shared.IoC; -using Robust.Shared.Log; using Robust.Shared.Map; using Robust.Shared.Map.Components; using Robust.Shared.Maths; @@ -18,716 +17,707 @@ using Robust.Shared.Timing; using Robust.Shared.Utility; -namespace Robust.Server.Physics +namespace Robust.Server.Physics; + +/// +/// Handles generating fixtures for MapGrids. +/// +public sealed partial class GridFixtureSystem : SharedGridFixtureSystem { + [Dependency] private IMapManager _mapManager = default!; + [Dependency] private IConfigurationManager _cfg = default!; + [Dependency] private IConGroupController _conGroup = default!; + [Dependency] private EntityLookupSystem _lookup = default!; + [Dependency] private SharedMapSystem _maps = default!; + [Dependency] private SharedPhysicsSystem _physics = default!; + [Dependency] private SharedTransformSystem _xformSystem = default!; + + private readonly Dictionary> _nodes = new(); + /// - /// Handles generating fixtures for MapGrids. + /// Sessions to receive nodes for debug purposes. /// - public sealed partial class GridFixtureSystem : SharedGridFixtureSystem - { - [Dependency] private IMapManager _mapManager = default!; - [Dependency] private IConfigurationManager _cfg = default!; - [Dependency] private IConGroupController _conGroup = default!; - [Dependency] private EntityLookupSystem _lookup = default!; - [Dependency] private SharedMapSystem _maps = default!; - [Dependency] private SharedPhysicsSystem _physics = default!; - [Dependency] private SharedTransformSystem _xformSystem = default!; + private readonly HashSet _subscribedSessions = new(); - private readonly Dictionary> _nodes = new(); + /// + /// Recursion detection to avoid splitting while handling an existing split + /// + private bool _isSplitting; - /// - /// Sessions to receive nodes for debug purposes. - /// - private readonly HashSet _subscribedSessions = new(); + internal bool SplitAllowed = true; - /// - /// Recursion detection to avoid splitting while handling an existing split - /// - private bool _isSplitting; + private HashSet _entSet = new(); - internal bool SplitAllowed = true; + private EntityQuery _gridQuery; + private EntityQuery _bodyQuery; + private EntityQuery _xformQuery; - private HashSet _entSet = new(); + public override void Initialize() + { + base.Initialize(); - private EntityQuery _gridQuery; - private EntityQuery _bodyQuery; - private EntityQuery _xformQuery; + _gridQuery = GetEntityQuery(); + _bodyQuery = GetEntityQuery(); + _xformQuery = GetEntityQuery(); + SubscribeLocalEvent(OnGridRemoval); + SubscribeNetworkEvent(OnDebugRequest); + SubscribeNetworkEvent(OnDebugStopRequest); - public override void Initialize() - { - base.Initialize(); + Subs.CVar(_cfg, CVars.GridSplitting, SetSplitAllowed, true); + } - _gridQuery = GetEntityQuery(); - _bodyQuery = GetEntityQuery(); - _xformQuery = GetEntityQuery(); - SubscribeLocalEvent(OnGridRemoval); - SubscribeNetworkEvent(OnDebugRequest); - SubscribeNetworkEvent(OnDebugStopRequest); + private void SetSplitAllowed(bool value) => SplitAllowed = value; - Subs.CVar(_cfg, CVars.GridSplitting, SetSplitAllowed, true); - } + public override void Shutdown() + { + base.Shutdown(); + _subscribedSessions.Clear(); + } - private void SetSplitAllowed(bool value) => SplitAllowed = value; + /// + /// Due to how MapLoader works need to ensure grid exists in dictionary before it's initialised. + /// + internal void EnsureGrid(EntityUid uid) + { + if (!_nodes.ContainsKey(uid)) + _nodes[uid] = new Dictionary(); + } - public override void Shutdown() - { - base.Shutdown(); - _subscribedSessions.Clear(); - } + protected override void OnGridInit(GridInitializeEvent ev) + { + EnsureGrid(ev.EntityUid); + base.OnGridInit(ev); + } - /// - /// Due to how MapLoader works need to ensure grid exists in dictionary before it's initialised. - /// - internal void EnsureGrid(EntityUid uid) - { - if (!_nodes.ContainsKey(uid)) - _nodes[uid] = new Dictionary(); - } + private void OnGridRemoval(GridRemovalEvent ev) + { + _nodes.Remove(ev.EntityUid); + } - protected override void OnGridInit(GridInitializeEvent ev) - { - EnsureGrid(ev.EntityUid); - base.OnGridInit(ev); - } + #region Debug - private void OnGridRemoval(GridRemovalEvent ev) - { - _nodes.Remove(ev.EntityUid); - } + private void OnDebugRequest(RequestGridNodesMessage msg, EntitySessionEventArgs args) + { + if (!_conGroup.CanCommand(args.SenderSession, ShowGridNodesCommand)) return; - #region Debug + AddDebugSubscriber(args.SenderSession); + } - private void OnDebugRequest(RequestGridNodesMessage msg, EntitySessionEventArgs args) - { - if (!_conGroup.CanCommand(args.SenderSession, ShowGridNodesCommand)) return; + private void OnDebugStopRequest(StopGridNodesMessage msg, EntitySessionEventArgs args) + { + RemoveDebugSubscriber(args.SenderSession); + } - AddDebugSubscriber(args.SenderSession); - } + public bool IsSubscribed(ICommonSession session) + { + return _subscribedSessions.Contains(session); + } - private void OnDebugStopRequest(StopGridNodesMessage msg, EntitySessionEventArgs args) - { - RemoveDebugSubscriber(args.SenderSession); - } + public void AddDebugSubscriber(ICommonSession session) + { + if (!_subscribedSessions.Add(session)) return; - public bool IsSubscribed(ICommonSession session) + foreach (var (uid, _) in _nodes) { - return _subscribedSessions.Contains(session); + SendNodeDebug(uid); } + } - public void AddDebugSubscriber(ICommonSession session) - { - if (!_subscribedSessions.Add(session)) return; + public void RemoveDebugSubscriber(ICommonSession session) + { + _subscribedSessions.Remove(session); + } - foreach (var (uid, _) in _nodes) - { - SendNodeDebug(uid); - } - } + private void SendNodeDebug(EntityUid uid) + { + if (_subscribedSessions.Count == 0) return; - public void RemoveDebugSubscriber(ICommonSession session) + var msg = new ChunkSplitDebugMessage { - _subscribedSessions.Remove(session); - } + Grid = GetNetEntity(uid), + }; - private void SendNodeDebug(EntityUid uid) + foreach (var (index, group) in _nodes[uid]) { - if (_subscribedSessions.Count == 0) return; - - var msg = new ChunkSplitDebugMessage - { - Grid = GetNetEntity(uid), - }; + var list = new List>(); + // To avoid double-sending connections. + var conns = new HashSet(); - foreach (var (index, group) in _nodes[uid]) + foreach (var node in group.Nodes) { - var list = new List>(); - // To avoid double-sending connections. - var conns = new HashSet(); + conns.Add(node); + list.Add(node.Indices.ToList()); - foreach (var node in group.Nodes) + foreach (var neighbor in node.Neighbors) { - conns.Add(node); - list.Add(node.Indices.ToList()); - - foreach (var neighbor in node.Neighbors) - { - if (conns.Contains(neighbor)) continue; + if (conns.Contains(neighbor)) continue; - msg.Connections.Add(( - node.GetCentre() + node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize, - neighbor.GetCentre() + neighbor.Group.Chunk.Indices * neighbor.Group.Chunk.ChunkSize)); - } + msg.Connections.Add(( + node.GetCentre() + node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize, + neighbor.GetCentre() + neighbor.Group.Chunk.Indices * neighbor.Group.Chunk.ChunkSize)); } - - msg.Nodes.Add(index, list); } - foreach (var session in _subscribedSessions) + msg.Nodes.Add(index, list); + } + + foreach (var session in _subscribedSessions) + { + RaiseNetworkEvent(msg, session.Channel); + } + } + + #endregion + + /// + /// Check for any potential splits. + /// + public void CheckSplits(EntityUid uid) + { + if (!_nodes.TryGetValue(uid, out var nodes)) + return; + + var dirtyNodes = new HashSet(nodes.Count); + + foreach (var group in nodes.Values) + { + foreach (var node in group.Nodes) { - RaiseNetworkEvent(msg, session.Channel); + dirtyNodes.Add(node); } } - #endregion + CheckSplits(uid, dirtyNodes); + } - /// - /// Check for any potential splits. - /// - public void CheckSplits(EntityUid uid) + /// + /// Check for splits on the specified nodes. + /// + private void CheckSplits(EntityUid uid, HashSet dirtyNodes) + { + // TODO: We already have mapgrid elsewhere + if (_isSplitting || !SplitAllowed || + !TryComp(uid, out var grid) || + !grid.CanSplit) { - if (!_nodes.TryGetValue(uid, out var nodes)) - return; + return; + } - var dirtyNodes = new HashSet(nodes.Count); + _isSplitting = true; + Log.Debug($"Started split check for {ToPrettyString(uid)}"); + var splitFrontier = new Queue(4); + var grids = new List>(1); + + while (dirtyNodes.Count > 0) + { + var originEnumerator = dirtyNodes.GetEnumerator(); + originEnumerator.MoveNext(); + var origin = originEnumerator.Current; + originEnumerator.Dispose(); + splitFrontier.Enqueue(origin); + var foundSplits = new HashSet + { + origin + }; - foreach (var group in nodes.Values) + while (splitFrontier.TryDequeue(out var split)) { - foreach (var node in group.Nodes) + dirtyNodes.Remove(split); + + foreach (var neighbor in split.Neighbors) { - dirtyNodes.Add(node); + if (!foundSplits.Add(neighbor)) continue; + + splitFrontier.Enqueue(neighbor); } } - CheckSplits(uid, dirtyNodes); + grids.Add(foundSplits); } - /// - /// Check for splits on the specified nodes. - /// - private void CheckSplits(EntityUid uid, HashSet dirtyNodes) - { - // TODO: We already have mapgrid elsewhere - if (_isSplitting || !SplitAllowed || - !TryComp(uid, out var grid) || - !grid.CanSplit) - { - return; - } - - _isSplitting = true; - Log.Debug($"Started split check for {ToPrettyString(uid)}"); - var splitFrontier = new Queue(4); - var grids = new List>(1); + var oldGrid = Comp(uid); + var oldGridUid = uid; - while (dirtyNodes.Count > 0) + // Split time + if (grids.Count > 1) + { + Log.Info($"Splitting {ToPrettyString(uid)} into {grids.Count} grids."); + var sw = new Stopwatch(); + sw.Start(); + + // We'll leave the biggest group as the original grid + // anything smaller gets split off. + grids.Sort((x, y) => + x.Sum(o => o.Indices.Count) + .CompareTo(y.Sum(o => o.Indices.Count))); + + var oldGridXform = _xformQuery.GetComponent(oldGridUid); + var (gridPos, gridRot) = _xformSystem.GetWorldPositionRotation(oldGridXform); + var mapBody = _bodyQuery.GetComponent(oldGridUid); + var oldGridComp = _gridQuery.GetComponent(oldGridUid); + var newGrids = new EntityUid[grids.Count - 1]; + var mapId = oldGridXform.MapID; + + for (var i = 0; i < grids.Count - 1; i++) { - var originEnumerator = dirtyNodes.GetEnumerator(); - originEnumerator.MoveNext(); - var origin = originEnumerator.Current; - originEnumerator.Dispose(); - splitFrontier.Enqueue(origin); - var foundSplits = new HashSet - { - origin - }; - - while (splitFrontier.TryDequeue(out var split)) + var group = grids[i]; + var newGrid = _mapManager.CreateGridEntity(mapId); + var newGridUid = newGrid.Owner; + var newGridXform = _xformQuery.GetComponent(newGridUid); + newGrids[i] = newGridUid; + + // Keep same origin / velocity etc; this makes updating a lot faster and easier. + _xformSystem.SetWorldPositionRotation(newGridUid, gridPos, gridRot, newGridXform); + var splitBody = _bodyQuery.GetComponent(newGridUid); + _physics.SetLinearVelocity(newGridUid, mapBody.LinearVelocity, body: splitBody); + _physics.SetAngularVelocity(newGridUid, mapBody.AngularVelocity, body: splitBody); + + var gridComp = _gridQuery.GetComponent(newGridUid); + var tileData = new List<(Vector2i GridIndices, Tile Tile)>(group.Sum(o => o.Indices.Count)); + + // Gather all tiles up front and set once to minimise fixture change events + foreach (var node in group) { - dirtyNodes.Remove(split); + var offset = node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize; - foreach (var neighbor in split.Neighbors) + foreach (var index in node.Indices) { - if (!foundSplits.Add(neighbor)) continue; - - splitFrontier.Enqueue(neighbor); + var tilePos = offset + index; + tileData.Add((tilePos, _maps.GetTileRef(oldGridUid, oldGrid, tilePos).Tile)); } } - grids.Add(foundSplits); - } - - var oldGrid = Comp(uid); - var oldGridUid = uid; + _maps.SetTiles(newGrid.Owner, newGrid.Comp, tileData); + DebugTools.Assert(_gridQuery.HasComp(newGridUid), "A split grid had no tiles?"); - // Split time - if (grids.Count > 1) - { - Log.Info($"Splitting {ToPrettyString(uid)} into {grids.Count} grids."); - var sw = new Stopwatch(); - sw.Start(); - - // We'll leave the biggest group as the original grid - // anything smaller gets split off. - grids.Sort((x, y) => - x.Sum(o => o.Indices.Count) - .CompareTo(y.Sum(o => o.Indices.Count))); - - var oldGridXform = _xformQuery.GetComponent(oldGridUid); - var (gridPos, gridRot) = _xformSystem.GetWorldPositionRotation(oldGridXform); - var mapBody = _bodyQuery.GetComponent(oldGridUid); - var oldGridComp = _gridQuery.GetComponent(oldGridUid); - var newGrids = new EntityUid[grids.Count - 1]; - var mapId = oldGridXform.MapID; - - for (var i = 0; i < grids.Count - 1; i++) + // Set tiles on new grid + update anchored entities + foreach (var node in group) { - var group = grids[i]; - var newGrid = _mapManager.CreateGridEntity(mapId); - var newGridUid = newGrid.Owner; - var newGridXform = _xformQuery.GetComponent(newGridUid); - newGrids[i] = newGridUid; - - // Keep same origin / velocity etc; this makes updating a lot faster and easier. - _xformSystem.SetWorldPositionRotation(newGridUid, gridPos, gridRot, newGridXform); - var splitBody = _bodyQuery.GetComponent(newGridUid); - _physics.SetLinearVelocity(newGridUid, mapBody.LinearVelocity, body: splitBody); - _physics.SetAngularVelocity(newGridUid, mapBody.AngularVelocity, body: splitBody); - - var gridComp = _gridQuery.GetComponent(newGridUid); - var tileData = new List<(Vector2i GridIndices, Tile Tile)>(group.Sum(o => o.Indices.Count)); - - // Gather all tiles up front and set once to minimise fixture change events - foreach (var node in group) + var offset = node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize; + + foreach (var tile in node.Indices) { - var offset = node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize; + var tilePos = offset + tile; + + // Access it directly because we're gonna be hammering it and want to keep allocs down. + var snapgrid = node.Group.Chunk.GetSnapGrid((ushort) tile.X, (ushort) tile.Y); + if (snapgrid == null || snapgrid.Count == 0) continue; - foreach (var index in node.Indices) + for (var j = snapgrid.Count - 1; j >= 0; j--) { - var tilePos = offset + index; - tileData.Add((tilePos, _maps.GetTileRef(oldGridUid, oldGrid, tilePos).Tile)); + var ent = snapgrid[j]; + var xform = _xformQuery.GetComponent(ent); + _xformSystem.ReAnchor(ent, xform, + oldGridComp, gridComp, + tilePos, tilePos, + oldGridUid, newGridUid, + oldGridXform, newGridXform, + Angle.Zero); + DebugTools.Assert(xform.Anchored); } } - _maps.SetTiles(newGrid.Owner, newGrid.Comp, tileData); - DebugTools.Assert(_gridQuery.HasComp(newGridUid), "A split grid had no tiles?"); - - // Set tiles on new grid + update anchored entities - foreach (var node in group) + // Update lookup ents + // Needs to be done before setting old tiles as they will be re-parented to the map. + // TODO: Combine tiles into larger rectangles or something; this is gonna be the killer bit. + foreach (var tile in node.Indices) { - var offset = node.Group.Chunk.Indices * node.Group.Chunk.ChunkSize; + var tilePos = offset + tile; + var bounds = _lookup.GetLocalBounds(tilePos, oldGrid.TileSize); - foreach (var tile in node.Indices) - { - var tilePos = offset + tile; - - // Access it directly because we're gonna be hammering it and want to keep allocs down. - var snapgrid = node.Group.Chunk.GetSnapGrid((ushort) tile.X, (ushort) tile.Y); - if (snapgrid == null || snapgrid.Count == 0) continue; - - for (var j = snapgrid.Count - 1; j >= 0; j--) - { - var ent = snapgrid[j]; - var xform = _xformQuery.GetComponent(ent); - _xformSystem.ReAnchor(ent, xform, - oldGridComp, gridComp, - tilePos, tilePos, - oldGridUid, newGridUid, - oldGridXform, newGridXform, - Angle.Zero); - DebugTools.Assert(xform.Anchored); - } - } + _entSet.Clear(); + _lookup.GetLocalEntitiesIntersecting(oldGridUid, tilePos, _entSet, 0f, LookupFlags.All | ~LookupFlags.Uncontained | LookupFlags.Approximate); - // Update lookup ents - // Needs to be done before setting old tiles as they will be re-parented to the map. - // TODO: Combine tiles into larger rectangles or something; this is gonna be the killer bit. - foreach (var tile in node.Indices) + foreach (var ent in _entSet) { - var tilePos = offset + tile; - var bounds = _lookup.GetLocalBounds(tilePos, oldGrid.TileSize); - - _entSet.Clear(); - _lookup.GetLocalEntitiesIntersecting(oldGridUid, tilePos, _entSet, 0f, LookupFlags.All | ~LookupFlags.Uncontained | LookupFlags.Approximate); + // Consider centre of entity position maybe? + var entXform = _xformQuery.GetComponent(ent); - foreach (var ent in _entSet) - { - // Consider centre of entity position maybe? - var entXform = _xformQuery.GetComponent(ent); + if (entXform.ParentUid != oldGridUid || + !bounds.Contains(entXform.LocalPosition)) continue; - if (entXform.ParentUid != oldGridUid || - !bounds.Contains(entXform.LocalPosition)) continue; - - _xformSystem.SetParent(ent, entXform, newGridUid, _xformQuery, newGridXform); - } + _xformSystem.SetParent(ent, entXform, newGridUid, _xformQuery, newGridXform); } - - _nodes[oldGridUid][node.Group.Chunk.Indices].Nodes.Remove(node); } - var eevee = new PostGridSplitEvent(oldGridUid, newGridUid); - RaiseLocalEvent(uid, ref eevee, true); - - for (var j = 0; j < tileData.Count; j++) - { - var (index, _) = tileData[j]; - tileData[j] = (index, Tile.Empty); - } - - // Set tiles on old grid - _maps.SetTiles(oldGridUid, oldGrid, tileData); - GenerateSplitNodes(newGridUid, newGrid); - SendNodeDebug(newGridUid); + _nodes[oldGridUid][node.Group.Chunk.Indices].Nodes.Remove(node); } - // Cull all of the old chunk nodes. - var toRemove = new RemQueue(); + var eevee = new PostGridSplitEvent(oldGridUid, newGridUid); + RaiseLocalEvent(uid, ref eevee, true); - foreach (var group in _nodes[oldGridUid].Values) + for (var j = 0; j < tileData.Count; j++) { - if (group.Nodes.Count > 0) continue; - toRemove.Add(group); + var (index, _) = tileData[j]; + tileData[j] = (index, Tile.Empty); } - foreach (var group in toRemove) - { - _nodes[oldGridUid].Remove(group.Chunk.Indices); - } + // Set tiles on old grid + _maps.SetTiles(oldGridUid, oldGrid, tileData); + GenerateSplitNodes(newGridUid, newGrid); + SendNodeDebug(newGridUid); + } + + // Cull all of the old chunk nodes. + var toRemove = new RemQueue(); - // Allow content to react to the grid being split... - var ev = new GridSplitEvent(newGrids, oldGridUid); - RaiseLocalEvent(uid, ref ev, true); + foreach (var group in _nodes[oldGridUid].Values) + { + if (group.Nodes.Count > 0) continue; + toRemove.Add(group); + } - Log.Debug($"Split {grids.Count} grids in {sw.Elapsed}"); + foreach (var group in toRemove) + { + _nodes[oldGridUid].Remove(group.Chunk.Indices); } - Log.Debug($"Stopped split check for {ToPrettyString(uid)}"); - _isSplitting = false; - SendNodeDebug(oldGridUid); + // Allow content to react to the grid being split... + var ev = new GridSplitEvent(newGrids, oldGridUid); + RaiseLocalEvent(uid, ref ev, true); + + Log.Debug($"Split {grids.Count} grids in {sw.Elapsed}"); } - private void GenerateSplitNodes(EntityUid gridUid, MapGridComponent grid) + Log.Debug($"Stopped split check for {ToPrettyString(uid)}"); + _isSplitting = false; + SendNodeDebug(oldGridUid); + } + + private void GenerateSplitNodes(EntityUid gridUid, MapGridComponent grid) + { + foreach (var chunk in _maps.GetMapChunks(gridUid, grid).Values) { - foreach (var chunk in _maps.GetMapChunks(gridUid, grid).Values) - { - var group = CreateNodes(gridUid, grid, chunk); - _nodes[gridUid].Add(chunk.Indices, group); - } + var group = CreateNodes(gridUid, grid, chunk); + _nodes[gridUid].Add(chunk.Indices, group); } + } - /// - /// Creates all of the splitting nodes within this chunk; also consider neighbor chunks. - /// - private ChunkNodeGroup CreateNodes(EntityUid gridEuid, MapGridComponent grid, MapChunk chunk) + /// + /// Creates all of the splitting nodes within this chunk; also consider neighbor chunks. + /// + private ChunkNodeGroup CreateNodes(EntityUid gridEuid, MapGridComponent grid, MapChunk chunk) + { + var group = new ChunkNodeGroup { - var group = new ChunkNodeGroup - { - Chunk = chunk, - }; + Chunk = chunk, + }; - var tiles = new HashSet(chunk.ChunkSize * chunk.ChunkSize); + var tiles = new HashSet(chunk.ChunkSize * chunk.ChunkSize); - for (var x = 0; x < chunk.ChunkSize; x++) + for (var x = 0; x < chunk.ChunkSize; x++) + { + for (var y = 0; y < chunk.ChunkSize; y++) { - for (var y = 0; y < chunk.ChunkSize; y++) - { - tiles.Add(new Vector2i(x, y)); - } + tiles.Add(new Vector2i(x, y)); } + } - var frontier = new Queue(); - var node = new ChunkSplitNode - { - Group = group, - }; + var frontier = new Queue(); + var node = new ChunkSplitNode + { + Group = group, + }; - // Simple BFS search to get all of the nodes in the chunk. - while (tiles.Count > 0) + // Simple BFS search to get all of the nodes in the chunk. + while (tiles.Count > 0) + { + var originEnumerator = tiles.GetEnumerator(); + originEnumerator.MoveNext(); + var origin = originEnumerator.Current; + frontier.Enqueue(origin); + originEnumerator.Dispose(); + + // Just reuse the node if we couldn't use it last time. + // This is in case weh ave 1 chunk with 255 empty tiles and 1 valid tile. + if (node.Indices.Count > 0) { - var originEnumerator = tiles.GetEnumerator(); - originEnumerator.MoveNext(); - var origin = originEnumerator.Current; - frontier.Enqueue(origin); - originEnumerator.Dispose(); - - // Just reuse the node if we couldn't use it last time. - // This is in case weh ave 1 chunk with 255 empty tiles and 1 valid tile. - if (node.Indices.Count > 0) + node = new ChunkSplitNode { - node = new ChunkSplitNode - { - Group = group, - }; - } + Group = group, + }; + } - tiles.Remove(origin); + tiles.Remove(origin); - // Check for valid neighbours and add them to the frontier. - while (frontier.TryDequeue(out var index)) - { - var tile = chunk.GetTile((ushort) index.X, (ushort) index.Y); - if (tile.IsEmpty) continue; + // Check for valid neighbours and add them to the frontier. + while (frontier.TryDequeue(out var index)) + { + var tile = chunk.GetTile((ushort) index.X, (ushort) index.Y); + if (tile.IsEmpty) continue; - node.Indices.Add(index); - var enumerator = new NeighborEnumerator(chunk, index); + node.Indices.Add(index); + var enumerator = new NeighborEnumerator(chunk, index); - while (enumerator.MoveNext(out var neighbor)) - { - // Already iterated this tile before so just ignore it. - if (!tiles.Remove(neighbor.Value)) continue; - frontier.Enqueue(neighbor.Value); - } + while (enumerator.MoveNext(out var neighbor)) + { + // Already iterated this tile before so just ignore it. + if (!tiles.Remove(neighbor.Value)) continue; + frontier.Enqueue(neighbor.Value); } + } - if (node.Indices.Count == 0) continue; + if (node.Indices.Count == 0) continue; - group.Nodes.Add(node); - } + group.Nodes.Add(node); + } - // Build neighbors - ChunkSplitNode? neighborNode; - MapChunk? neighborChunk; + // Build neighbors + ChunkSplitNode? neighborNode; + MapChunk? neighborChunk; - // Check each tile for node neighbours on other chunks (not possible for us to have neighbours on the same chunk - // as they would already be in our node). - // TODO: This could be better (maybe only check edges of the chunk or something). - foreach (var chunkNode in group.Nodes) + // Check each tile for node neighbours on other chunks (not possible for us to have neighbours on the same chunk + // as they would already be in our node). + // TODO: This could be better (maybe only check edges of the chunk or something). + foreach (var chunkNode in group.Nodes) + { + foreach (var index in chunkNode.Indices) { - foreach (var index in chunkNode.Indices) + // Check for edge tiles. + if (index.X == 0) { - // Check for edge tiles. - if (index.X == 0) + // Check West + if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X - 1, chunk.Indices.Y), out neighborChunk) && + TryGetNode(gridEuid, neighborChunk, new Vector2i(chunk.ChunkSize - 1, index.Y), out neighborNode)) { - // Check West - if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X - 1, chunk.Indices.Y), out neighborChunk) && - TryGetNode(gridEuid, neighborChunk, new Vector2i(chunk.ChunkSize - 1, index.Y), out neighborNode)) - { - chunkNode.Neighbors.Add(neighborNode); - neighborNode.Neighbors.Add(chunkNode); - } + chunkNode.Neighbors.Add(neighborNode); + neighborNode.Neighbors.Add(chunkNode); } + } - if (index.Y == 0) + if (index.Y == 0) + { + // Check South + if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X, chunk.Indices.Y - 1), out neighborChunk) && + TryGetNode(gridEuid, neighborChunk, new Vector2i(index.X, chunk.ChunkSize - 1), out neighborNode)) { - // Check South - if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X, chunk.Indices.Y - 1), out neighborChunk) && - TryGetNode(gridEuid, neighborChunk, new Vector2i(index.X, chunk.ChunkSize - 1), out neighborNode)) - { - chunkNode.Neighbors.Add(neighborNode); - neighborNode.Neighbors.Add(chunkNode); - } + chunkNode.Neighbors.Add(neighborNode); + neighborNode.Neighbors.Add(chunkNode); } + } - if (index.X == chunk.ChunkSize - 1) + if (index.X == chunk.ChunkSize - 1) + { + // Check East + if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X + 1, chunk.Indices.Y), out neighborChunk) && + TryGetNode(gridEuid, neighborChunk, new Vector2i(0, index.Y), out neighborNode)) { - // Check East - if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X + 1, chunk.Indices.Y), out neighborChunk) && - TryGetNode(gridEuid, neighborChunk, new Vector2i(0, index.Y), out neighborNode)) - { - chunkNode.Neighbors.Add(neighborNode); - neighborNode.Neighbors.Add(chunkNode); - } + chunkNode.Neighbors.Add(neighborNode); + neighborNode.Neighbors.Add(chunkNode); } + } - if (index.Y == chunk.ChunkSize - 1) + if (index.Y == chunk.ChunkSize - 1) + { + // Check North + if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X, chunk.Indices.Y + 1), out neighborChunk) && + TryGetNode(gridEuid, neighborChunk, new Vector2i(index.X, 0), out neighborNode)) { - // Check North - if (_maps.TryGetChunk(gridEuid, grid, new Vector2i(chunk.Indices.X, chunk.Indices.Y + 1), out neighborChunk) && - TryGetNode(gridEuid, neighborChunk, new Vector2i(index.X, 0), out neighborNode)) - { - chunkNode.Neighbors.Add(neighborNode); - neighborNode.Neighbors.Add(chunkNode); - } + chunkNode.Neighbors.Add(neighborNode); + neighborNode.Neighbors.Add(chunkNode); } } } - - return group; } - /// - /// Checks for grid split with 1 chunk updated. - /// - internal override void CheckSplit(EntityUid gridEuid, MapChunk chunk, List rectangles) - { - HashSet nodes; + return group; + } - if (chunk.FilledTiles == 0) - { - nodes = RemoveSplitNode(gridEuid, chunk); - } - else - { - nodes = GenerateSplitNode(gridEuid, chunk); - } + /// + /// Checks for grid split with 1 chunk updated. + /// + internal override void CheckSplit(EntityUid gridEuid, MapChunk chunk, List rectangles) + { + HashSet nodes; - CheckSplits(gridEuid, nodes); + if (chunk.FilledTiles == 0) + { + nodes = RemoveSplitNode(gridEuid, chunk); } - - /// - /// Checks for grid split with many chunks updated. - /// - internal override void CheckSplit(EntityUid gridEuid, Dictionary> mapChunks, List removedChunks) + else { - var nodes = new HashSet(); + nodes = GenerateSplitNode(gridEuid, chunk); + } - foreach (var chunk in removedChunks) - { - nodes.UnionWith(RemoveSplitNode(gridEuid, chunk)); - } + CheckSplits(gridEuid, nodes); + } - foreach (var (chunk, _) in mapChunks) - { - nodes.UnionWith(GenerateSplitNode(gridEuid, chunk)); - } + /// + /// Checks for grid split with many chunks updated. + /// + internal override void CheckSplit(EntityUid gridEuid, Dictionary> mapChunks, List removedChunks) + { + var nodes = new HashSet(); - var toRemove = new ValueList(); + foreach (var chunk in removedChunks) + { + nodes.UnionWith(RemoveSplitNode(gridEuid, chunk)); + } - // Some of the neighbour nodes may have been added that were since deleted during the above enumeration - // e.g. if NodeA and NodeB both had their counts set to 0 and are neighbours then either might add - // the other to dirtynodes. - foreach (var node in nodes) - { - if (node.Indices.Count > 0) continue; - toRemove.Add(node); - } + foreach (var (chunk, _) in mapChunks) + { + nodes.UnionWith(GenerateSplitNode(gridEuid, chunk)); + } - foreach (var node in toRemove) - { - nodes.Remove(node); - } + var toRemove = new ValueList(); - CheckSplits(gridEuid, nodes); + // Some of the neighbour nodes may have been added that were since deleted during the above enumeration + // e.g. if NodeA and NodeB both had their counts set to 0 and are neighbours then either might add + // the other to dirtynodes. + foreach (var node in nodes) + { + if (node.Indices.Count > 0) continue; + toRemove.Add(node); } - /// - /// Removes this chunk from nodes and dirties its neighbours. - /// - private HashSet RemoveSplitNode(EntityUid gridEuid, MapChunk chunk) + foreach (var node in toRemove) { - var dirtyNodes = new HashSet(); + nodes.Remove(node); + } - if (_isSplitting) return new HashSet(); + CheckSplits(gridEuid, nodes); + } - Cleanup(gridEuid, chunk, dirtyNodes); - DebugTools.Assert(dirtyNodes.All(o => o.Group.Chunk != chunk)); - return dirtyNodes; - } + /// + /// Removes this chunk from nodes and dirties its neighbours. + /// + private HashSet RemoveSplitNode(EntityUid gridEuid, MapChunk chunk) + { + var dirtyNodes = new HashSet(); - /// - /// Re-adds this chunk to nodes and dirties its neighbours and itself. - /// - private HashSet GenerateSplitNode(EntityUid gridEuid, MapChunk chunk) - { - var dirtyNodes = RemoveSplitNode(gridEuid, chunk); + if (_isSplitting) return new HashSet(); - if (_isSplitting) return dirtyNodes; + Cleanup(gridEuid, chunk, dirtyNodes); + DebugTools.Assert(dirtyNodes.All(o => o.Group.Chunk != chunk)); + return dirtyNodes; + } - DebugTools.Assert(chunk.FilledTiles > 0); + /// + /// Re-adds this chunk to nodes and dirties its neighbours and itself. + /// + private HashSet GenerateSplitNode(EntityUid gridEuid, MapChunk chunk) + { + var dirtyNodes = RemoveSplitNode(gridEuid, chunk); - var grid = Comp(gridEuid); - var group = CreateNodes(gridEuid, grid, chunk); - _nodes[gridEuid][chunk.Indices] = group; + if (_isSplitting) return dirtyNodes; - foreach (var chunkNode in group.Nodes) - { - dirtyNodes.Add(chunkNode); - } + DebugTools.Assert(chunk.FilledTiles > 0); - return dirtyNodes; - } + var grid = Comp(gridEuid); + var group = CreateNodes(gridEuid, grid, chunk); + _nodes[gridEuid][chunk.Indices] = group; - /// - /// Tries to get the relevant split node from a neighbor chunk. - /// - private bool TryGetNode(EntityUid gridEuid, MapChunk chunk, Vector2i index, [NotNullWhen(true)] out ChunkSplitNode? node) + foreach (var chunkNode in group.Nodes) { - if (!_nodes[gridEuid].TryGetValue(chunk.Indices, out var neighborGroup)) - { - node = null; - return false; - } + dirtyNodes.Add(chunkNode); + } - foreach (var neighborNode in neighborGroup.Nodes) - { - if (!neighborNode.Indices.Contains(index)) continue; - node = neighborNode; - return true; - } + return dirtyNodes; + } + /// + /// Tries to get the relevant split node from a neighbor chunk. + /// + private bool TryGetNode(EntityUid gridEuid, MapChunk chunk, Vector2i index, [NotNullWhen(true)] out ChunkSplitNode? node) + { + if (!_nodes[gridEuid].TryGetValue(chunk.Indices, out var neighborGroup)) + { node = null; return false; } - private void Cleanup(EntityUid gridEuid, MapChunk chunk, HashSet dirtyNodes) + foreach (var neighborNode in neighborGroup.Nodes) { - if (!_nodes[gridEuid].TryGetValue(chunk.Indices, out var group)) return; + if (!neighborNode.Indices.Contains(index)) continue; + node = neighborNode; + return true; + } - foreach (var node in group.Nodes) - { - // Most important thing is updating our neighbor nodes. - foreach (var neighbor in node.Neighbors) - { - neighbor.Neighbors.Remove(node); - // If neighbor is on a different chunk mark it for checking connections later. - if (neighbor.Group.Equals(group)) continue; - dirtyNodes.Add(neighbor); - } + node = null; + return false; + } - node.Indices.Clear(); - node.Neighbors.Clear(); + private void Cleanup(EntityUid gridEuid, MapChunk chunk, HashSet dirtyNodes) + { + if (!_nodes[gridEuid].TryGetValue(chunk.Indices, out var group)) return; + + foreach (var node in group.Nodes) + { + // Most important thing is updating our neighbor nodes. + foreach (var neighbor in node.Neighbors) + { + neighbor.Neighbors.Remove(node); + // If neighbor is on a different chunk mark it for checking connections later. + if (neighbor.Group.Equals(group)) continue; + dirtyNodes.Add(neighbor); } - _nodes[gridEuid].Remove(chunk.Indices); + node.Indices.Clear(); + node.Neighbors.Clear(); } - internal sealed class ChunkNodeGroup - { - internal MapChunk Chunk = default!; - public HashSet Nodes = new(); - } + _nodes[gridEuid].Remove(chunk.Indices); + } - internal sealed class ChunkSplitNode - { - public ChunkNodeGroup Group = default!; - public HashSet Indices { get; set; } = new(); - public HashSet Neighbors { get; set; } = new(); + internal sealed class ChunkNodeGroup + { + internal MapChunk Chunk = default!; + public HashSet Nodes = new(); + } - public Vector2 GetCentre() - { - var centre = Vector2.Zero; + internal sealed class ChunkSplitNode + { + public ChunkNodeGroup Group = default!; + public HashSet Indices { get; set; } = new(); + public HashSet Neighbors { get; set; } = new(); - foreach (var index in Indices) - { - centre += index; - } + public Vector2 GetCentre() + { + var centre = Vector2.Zero; - centre /= Indices.Count; - return centre; + foreach (var index in Indices) + { + centre += index; } + + centre /= Indices.Count; + return centre; } + } + + private struct NeighborEnumerator(MapChunk chunk, Vector2i index) + { + private int _count = -1; - private struct NeighborEnumerator + public bool MoveNext([NotNullWhen(true)] out Vector2i? neighbor) { - private MapChunk _chunk; - private Vector2i _index; - private int _count = -1; + _count++; - public NeighborEnumerator(MapChunk chunk, Vector2i index) + // Just go through S E N W + switch (_count) { - _chunk = chunk; - _index = index; + case 0: + if (index.Y == 0) break; + neighbor = new Vector2i(index.X, index.Y - 1); + return true; + case 1: + if (index.X == chunk.ChunkSize - 1) break; + neighbor = new Vector2i(index.X + 1, index.Y); + return true; + case 2: + if (index.Y == chunk.ChunkSize + 1) break; + neighbor = new Vector2i(index.X, index.Y + 1); + return true; + case 3: + if (index.X == 0) break; + neighbor = new Vector2i(index.X - 1, index.Y); + return true; + default: + neighbor = null; + return false; } - public bool MoveNext([NotNullWhen(true)] out Vector2i? neighbor) - { - _count++; - - // Just go through S E N W - switch (_count) - { - case 0: - if (_index.Y == 0) break; - neighbor = new Vector2i(_index.X, _index.Y - 1); - return true; - case 1: - if (_index.X == _chunk.ChunkSize - 1) break; - neighbor = new Vector2i(_index.X + 1, _index.Y); - return true; - case 2: - if (_index.Y == _chunk.ChunkSize + 1) break; - neighbor = new Vector2i(_index.X, _index.Y + 1); - return true; - case 3: - if (_index.X == 0) break; - neighbor = new Vector2i(_index.X - 1, _index.Y); - return true; - default: - neighbor = null; - return false; - } - - return MoveNext(out neighbor); - } + return MoveNext(out neighbor); } } } @@ -736,44 +726,32 @@ public bool MoveNext([NotNullWhen(true)] out Vector2i? neighbor) /// Event raised on a grid after it has been split but before the old grid has been cleaned up. /// [ByRefEvent] -public readonly struct PostGridSplitEvent +public readonly struct PostGridSplitEvent(EntityUid oldGrid, EntityUid grid) { /// /// The grid it was part of previously. /// - public readonly EntityUid OldGrid; + public readonly EntityUid OldGrid = oldGrid; /// /// The grid that has been split. /// - public readonly EntityUid Grid; - - public PostGridSplitEvent(EntityUid oldGrid, EntityUid grid) - { - OldGrid = oldGrid; - Grid = grid; - } + public readonly EntityUid Grid = grid; } /// /// Event raised on a grid that has been split into multiple grids. /// [ByRefEvent] -public readonly struct GridSplitEvent +public readonly struct GridSplitEvent(EntityUid[] newGrids, EntityUid grid) { /// /// Contains the IDs of the newly created grids. /// - public readonly EntityUid[] NewGrids; + public readonly EntityUid[] NewGrids = newGrids; /// /// The grid that has been split. /// - public readonly EntityUid Grid; - - public GridSplitEvent(EntityUid[] newGrids, EntityUid grid) - { - NewGrids = newGrids; - Grid = grid; - } + public readonly EntityUid Grid = grid; } diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index e6fbf61f9b2..a484a79ad1b 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -13,652 +13,641 @@ using Robust.Shared.Threading; using Robust.Shared.Utility; -namespace Robust.Shared.Physics.Systems +namespace Robust.Shared.Physics.Systems; + +public abstract partial class SharedBroadphaseSystem : EntitySystem { - public abstract partial class SharedBroadphaseSystem : EntitySystem + [Dependency] private IConfigurationManager _cfg = default!; + [Dependency] private IMapManagerInternal _mapManager = default!; + [Dependency] private IParallelManager _parallel = default!; + [Dependency] private EntityLookupSystem _lookup = default!; + [Dependency] private SharedGridTraversalSystem _traversal = default!; + [Dependency] private SharedMapSystem _map = default!; + [Dependency] private SharedPhysicsSystem _physicsSystem = default!; + [Dependency] private SharedTransformSystem _transform = default!; + + [Dependency] private EntityQuery _broadphaseQuery = default!; + [Dependency] private EntityQuery _fixturesQuery = default!; + [Dependency] private EntityQuery _gridQuery = default!; + [Dependency] private EntityQuery _physicsQuery = default!; + [Dependency] private EntityQuery _xformQuery = default!; + + private readonly HashSet _gridMoveBuffer = new(); + + private float _frameTime; + + /* + * Okay so Box2D has its own "MoveProxy" stuff so you can easily find new contacts when required. + * Our problem is that we have nested broadphases (rather than being on separate maps) which makes this + * not feasible because a body could be intersecting 2 broadphases. + * Hence we need to check which broadphases it does intersect and checkar for colliding bodies. + */ + + private BroadphaseContactJob _contactJob; + + public override void Initialize() { - [Dependency] private IConfigurationManager _cfg = default!; - [Dependency] private IMapManagerInternal _mapManager = default!; - [Dependency] private IParallelManager _parallel = default!; - [Dependency] private EntityLookupSystem _lookup = default!; - [Dependency] private SharedGridTraversalSystem _traversal = default!; - [Dependency] private SharedMapSystem _map = default!; - [Dependency] private SharedPhysicsSystem _physicsSystem = default!; - [Dependency] private SharedTransformSystem _transform = default!; - - private EntityQuery _broadphaseQuery; - private EntityQuery _fixturesQuery; - private EntityQuery _gridQuery; - private EntityQuery _physicsQuery; - private EntityQuery _xformQuery; - - private readonly HashSet _gridMoveBuffer = new(); - - private float _frameTime; - - /* - * Okay so Box2D has its own "MoveProxy" stuff so you can easily find new contacts when required. - * Our problem is that we have nested broadphases (rather than being on separate maps) which makes this - * not feasible because a body could be intersecting 2 broadphases. - * Hence we need to check which broadphases it does intersect and checkar for colliding bodies. - */ - - private BroadphaseContactJob _contactJob; - - public override void Initialize() - { - base.Initialize(); - - _contactJob = new() - { - MapManager = _mapManager, - System = this, - TransformSys = EntityManager.System(), - // TODO: EntityManager one isn't ready yet? - XformQuery = GetEntityQuery(), - }; - - _broadphaseQuery = GetEntityQuery(); - _fixturesQuery = GetEntityQuery(); - _gridQuery = GetEntityQuery(); - _physicsQuery = GetEntityQuery(); - _xformQuery = GetEntityQuery(); - - UpdatesOutsidePrediction = true; - UpdatesAfter.Add(typeof(SharedTransformSystem)); - - Subs.CVar(_cfg, - CVars.TargetMinimumTickrate, - val => - { - _frameTime = 1f / val; - }, - true); - } + base.Initialize(); - public void Rebuild(BroadphaseComponent component, bool fullBuild) + _contactJob = new() { - component.StaticTree.Rebuild(fullBuild); - component.DynamicTree.Rebuild(fullBuild); - component.SundriesTree._b2Tree.Rebuild(fullBuild); - component.StaticSundriesTree._b2Tree.Rebuild(fullBuild); - } + MapManager = _mapManager, + System = this, + TransformSys = EntityManager.System(), + // TODO: EntityManager one isn't ready yet? + XformQuery = GetEntityQuery(), + }; + + UpdatesOutsidePrediction = true; + UpdatesAfter.Add(typeof(SharedTransformSystem)); + + Subs.CVar(_cfg, + CVars.TargetMinimumTickrate, + val => + { + _frameTime = 1f / val; + }, + true); + } - public void RebuildBottomUp(BroadphaseComponent component) - { - component.StaticTree.RebuildBottomUp(); - component.DynamicTree.RebuildBottomUp(); - component.SundriesTree._b2Tree.RebuildBottomUp(); - component.StaticSundriesTree._b2Tree.RebuildBottomUp(); - } + public void Rebuild(BroadphaseComponent component, bool fullBuild) + { + component.StaticTree.Rebuild(fullBuild); + component.DynamicTree.Rebuild(fullBuild); + component.SundriesTree._b2Tree.Rebuild(fullBuild); + component.StaticSundriesTree._b2Tree.Rebuild(fullBuild); + } - #region Find Contacts + public void RebuildBottomUp(BroadphaseComponent component) + { + component.StaticTree.RebuildBottomUp(); + component.DynamicTree.RebuildBottomUp(); + component.SundriesTree._b2Tree.RebuildBottomUp(); + component.StaticSundriesTree._b2Tree.RebuildBottomUp(); + } - /// - /// Check the AABB for each moved broadphase fixture and add any colliding entities to the movebuffer in case. - /// - private void FindGridContacts(HashSet movedGrids) - { - // None moved this tick - if (movedGrids.Count == 0) return; + #region Find Contacts - // This is so that if we're on a broadphase that's moving (e.g. a grid) we need to make sure anything - // we move over is getting checked for collisions, and putting it on the movebuffer is the easiest way to do so. - var moveBuffer = _physicsSystem.MoveBuffer; - _gridMoveBuffer.Clear(); + /// + /// Check the AABB for each moved broadphase fixture and add any colliding entities to the movebuffer in case. + /// + private void FindGridContacts(HashSet movedGrids) + { + // None moved this tick + if (movedGrids.Count == 0) return; - foreach (var gridUid in movedGrids) - { - var grid = _gridQuery.GetComponent(gridUid); - var xform = _xformQuery.GetComponent(gridUid); + // This is so that if we're on a broadphase that's moving (e.g. a grid) we need to make sure anything + // we move over is getting checked for collisions, and putting it on the movebuffer is the easiest way to do so. + var moveBuffer = _physicsSystem.MoveBuffer; + _gridMoveBuffer.Clear(); - // Moved to nullspace? - if (!_broadphaseQuery.TryComp(xform.MapUid, out var mapBroadphase)) - continue; + foreach (var gridUid in movedGrids) + { + var grid = _gridQuery.GetComponent(gridUid); + var xform = _xformQuery.GetComponent(gridUid); - var worldAABB = _transform.GetWorldMatrix(xform).TransformBox(grid.LocalAABB); - var enlargedAABB = worldAABB.Enlarged(GetBroadphaseExpand(_physicsQuery.GetComponent(gridUid), _frameTime)); - var state = (moveBuffer, _gridMoveBuffer); + // Moved to nullspace? + if (!_broadphaseQuery.TryComp(xform.MapUid, out var mapBroadphase)) + continue; - QueryMapBroadphase(mapBroadphase.DynamicTree, ref state, enlargedAABB); - QueryMapBroadphase(mapBroadphase.StaticTree, ref state, enlargedAABB); - } + var worldAABB = _transform.GetWorldMatrix(xform).TransformBox(grid.LocalAABB); + var enlargedAABB = worldAABB.Enlarged(GetBroadphaseExpand(_physicsQuery.GetComponent(gridUid), _frameTime)); + var state = (moveBuffer, _gridMoveBuffer); - foreach (var proxy in _gridMoveBuffer) - { - moveBuffer.Add(proxy); - // If something is in our AABB then try grid traversal for it - _traversal.CheckTraverse((proxy.Entity, _xformQuery.GetComponent(proxy.Entity))); - } + QueryMapBroadphase(mapBroadphase.DynamicTree, ref state, enlargedAABB); + QueryMapBroadphase(mapBroadphase.StaticTree, ref state, enlargedAABB); } - private float GetBroadphaseExpand(PhysicsComponent body, float frameTime) + foreach (var proxy in _gridMoveBuffer) { - return body.LinearVelocity.Length() * 1.2f * frameTime; + moveBuffer.Add(proxy); + // If something is in our AABB then try grid traversal for it + _traversal.CheckTraverse((proxy.Entity, _xformQuery.GetComponent(proxy.Entity))); } + } - private void QueryMapBroadphase(IBroadPhase broadPhase, - ref (HashSet, HashSet) state, - Box2 enlargedAABB) - { - // Easier to just not go over each proxy as we already unioned the fixture's worldaabb. - broadPhase.QueryAabb(ref state, static (ref ( - HashSet moveBuffer, - HashSet gridMoveBuffer) tuple, - in FixtureProxy value) => - { - // 99% of the time it's just going to be the broadphase (for now the grid) itself. - // hence this body check makes this run significantly better. - // Also check if it's not already on the movebuffer. - if (tuple.moveBuffer.Contains(value)) - return true; + private float GetBroadphaseExpand(PhysicsComponent body, float frameTime) + { + return body.LinearVelocity.Length() * 1.2f * frameTime; + } - // To avoid updating during iteration. - // Don't need to transform as it's already in map terms. - tuple.gridMoveBuffer.Add(value); + private void QueryMapBroadphase(IBroadPhase broadPhase, + ref (HashSet, HashSet) state, + Box2 enlargedAABB) + { + // Easier to just not go over each proxy as we already unioned the fixture's worldaabb. + broadPhase.QueryAabb(ref state, static (ref ( + HashSet moveBuffer, + HashSet gridMoveBuffer) tuple, + in FixtureProxy value) => + { + // 99% of the time it's just going to be the broadphase (for now the grid) itself. + // hence this body check makes this run significantly better. + // Also check if it's not already on the movebuffer. + if (tuple.moveBuffer.Contains(value)) return true; - }, enlargedAABB, true); - } - /// - /// Go through every single created, moved, or touched proxy on the map and try to find any new contacts that should be created. - /// - internal void FindNewContacts() - { - _contactJob.FrameTime = _frameTime; - _contactJob.Pairs.Clear(); + // To avoid updating during iteration. + // Don't need to transform as it's already in map terms. + tuple.gridMoveBuffer.Add(value); + return true; + }, enlargedAABB, true); + } - var moveBuffer = _physicsSystem.MoveBuffer; - var movedGrids = _physicsSystem.MovedGrids; + /// + /// Go through every single created, moved, or touched proxy on the map and try to find any new contacts that should be created. + /// + internal void FindNewContacts() + { + _contactJob.FrameTime = _frameTime; + _contactJob.Pairs.Clear(); - // Find any entities being driven over that might need to be considered - FindGridContacts(movedGrids); + var moveBuffer = _physicsSystem.MoveBuffer; + var movedGrids = _physicsSystem.MovedGrids; - // There is some mariana trench levels of bullshit going on. - // We essentially need to re-create Box2D's FindNewContacts but in a way that allows us to check every - // broadphase intersecting a particular proxy instead of just on the 1 broadphase. - // This means we can generate contacts across different broadphases. - // If you have a better way of allowing for broadphases attached to grids then by all means code it yourself. + // Find any entities being driven over that might need to be considered + FindGridContacts(movedGrids); - // FindNewContacts is inherently going to be a lot slower than Box2D's normal version so we need - // to cache a bunch of stuff to make up for it. + // There is some mariana trench levels of bullshit going on. + // We essentially need to re-create Box2D's FindNewContacts but in a way that allows us to check every + // broadphase intersecting a particular proxy instead of just on the 1 broadphase. + // This means we can generate contacts across different broadphases. + // If you have a better way of allowing for broadphases attached to grids then by all means code it yourself. - // Handle grids first as they're not stored on map broadphase at all. - HandleGridCollisions(movedGrids); + // FindNewContacts is inherently going to be a lot slower than Box2D's normal version so we need + // to cache a bunch of stuff to make up for it. - // EZ - if (moveBuffer.Count == 0) - return; + // Handle grids first as they're not stored on map broadphase at all. + HandleGridCollisions(movedGrids); - _contactJob.MoveBuffer.Clear(); + // EZ + if (moveBuffer.Count == 0) + return; - foreach (var proxy in moveBuffer) - { - DebugTools.Assert(_xformQuery.GetComponent(proxy.Entity).Broadphase?.Uid != null); - _contactJob.MoveBuffer.Add(proxy); - } + _contactJob.MoveBuffer.Clear(); - var count = moveBuffer.Count; + foreach (var proxy in moveBuffer) + { + DebugTools.Assert(_xformQuery.GetComponent(proxy.Entity).Broadphase?.Uid != null); + _contactJob.MoveBuffer.Add(proxy); + } - _parallel.ProcessNow(_contactJob, count); + var count = moveBuffer.Count; - foreach (var (proxyA, proxyB, flags) in _contactJob.Pairs) - { - var otherBody = proxyB.Body; - var contactFlags = ContactFlags.None; - - // Because we may be colliding with something asleep (due to the way grid movement works) need - // to make sure the contact doesn't fail. - // This is because we generate a contact across 2 different broadphases where both bodies aren't - // moving locally but are moving in world-terms. - if ((flags & PairFlag.Wake) == PairFlag.Wake) - { - _physicsSystem.WakeBody(proxyA.Entity, force: true, body: proxyA.Body); - _physicsSystem.WakeBody(proxyB.Entity, force: true, body: otherBody); - } + _parallel.ProcessNow(_contactJob, count); - // TODO: Actually implement this for grids, atm they have their own skrungly fixture handling which prevents this. - if ((PairFlag.Grid & flags) == PairFlag.Grid) - { - contactFlags |= ContactFlags.Grid; - } + foreach (var (proxyA, proxyB, flags) in _contactJob.Pairs) + { + var otherBody = proxyB.Body; + var contactFlags = ContactFlags.None; + + // Because we may be colliding with something asleep (due to the way grid movement works) need + // to make sure the contact doesn't fail. + // This is because we generate a contact across 2 different broadphases where both bodies aren't + // moving locally but are moving in world-terms. + if ((flags & PairFlag.Wake) == PairFlag.Wake) + { + _physicsSystem.WakeBody(proxyA.Entity, force: true, body: proxyA.Body); + _physicsSystem.WakeBody(proxyB.Entity, force: true, body: otherBody); + } - _physicsSystem.AddPair(proxyA.FixtureId, proxyB.FixtureId, proxyA, proxyB, flags: contactFlags); + // TODO: Actually implement this for grids, atm they have their own skrungly fixture handling which prevents this. + if ((PairFlag.Grid & flags) == PairFlag.Grid) + { + contactFlags |= ContactFlags.Grid; } - moveBuffer.Clear(); - movedGrids.Clear(); + _physicsSystem.AddPair(proxyA.FixtureId, proxyB.FixtureId, proxyA, proxyB, flags: contactFlags); } - private void HandleGridCollisions(HashSet movedGrids) + moveBuffer.Clear(); + movedGrids.Clear(); + } + + private void HandleGridCollisions(HashSet movedGrids) + { + // TODO: Could move this into its own job. + // Ideally we'd just have some way to flag an entity as "AABB moves not proxy" into its own movebuffer. + foreach (var gridAUid in movedGrids) { - // TODO: Could move this into its own job. - // Ideally we'd just have some way to flag an entity as "AABB moves not proxy" into its own movebuffer. - foreach (var gridAUid in movedGrids) - { - var gridAMapComp = _gridQuery.GetComponent(gridAUid); - var xform = _xformQuery.GetComponent(gridAUid); - - if (xform.MapID == MapId.Nullspace) - continue; - - var (worldPos, worldRot, gridAToWorld, worldToGridA) = _transform.GetWorldPositionRotationMatrixWithInv(xform); - - var aabb = new Box2Rotated(gridAMapComp.LocalAABB, worldRot).CalcBoundingBox().Translated(worldPos); - - // TODO: Need to handle grids colliding with non-grid entities with the same layer - // (nothing in SS14 does this yet). - var fixture = _fixturesQuery.Comp(gridAUid); - var physics = _physicsQuery.Comp(gridAUid); - - // This is just a rigid transform, exactly the same as gridAToWorld //TODO: Really shouldn't need this! - var gridAToWorldRigid = _physicsSystem.GetPhysicsTransform(gridAUid); - var state = ( - new Entity(gridAUid, fixture, gridAMapComp, physics, xform), - gridAToWorldRigid, - gridAToWorld, - worldToGridA, - _map, - _physicsSystem, - _transform, - _fixturesQuery, - _physicsQuery, - _xformQuery); - - _mapManager.FindGridsIntersecting(xform.MapID, aabb, ref state, - static (EntityUid gridBUid, MapGridComponent gridBMapComp, - ref (Entity gridA, - Transform gridAToWorldRigid, - Matrix3x2 gridAToWorld, - Matrix3x2 worldToGridA, - SharedMapSystem _map, - SharedPhysicsSystem _physicsSystem, - SharedTransformSystem xformSystem, - EntityQuery fixturesQuery, - EntityQuery physicsQuery, - EntityQuery xformQuery) tuple) => - { - if (tuple.gridA.Owner == gridBUid || + var gridAMapComp = _gridQuery.GetComponent(gridAUid); + var xform = _xformQuery.GetComponent(gridAUid); + + if (xform.MapID == MapId.Nullspace) + continue; + + var (worldPos, worldRot, gridAToWorld, worldToGridA) = _transform.GetWorldPositionRotationMatrixWithInv(xform); + + var aabb = new Box2Rotated(gridAMapComp.LocalAABB, worldRot).CalcBoundingBox().Translated(worldPos); + + // TODO: Need to handle grids colliding with non-grid entities with the same layer + // (nothing in SS14 does this yet). + var fixture = _fixturesQuery.Comp(gridAUid); + var physics = _physicsQuery.Comp(gridAUid); + + // This is just a rigid transform, exactly the same as gridAToWorld //TODO: Really shouldn't need this! + var gridAToWorldRigid = _physicsSystem.GetPhysicsTransform(gridAUid); + var state = ( + new Entity(gridAUid, fixture, gridAMapComp, physics, xform), + gridAToWorldRigid, + gridAToWorld, + worldToGridA, + _map, + _physicsSystem, + _transform, + _fixturesQuery, + _physicsQuery, + _xformQuery); + + _mapManager.FindGridsIntersecting(xform.MapID, aabb, ref state, + static (EntityUid gridBUid, MapGridComponent gridBMapComp, + ref (Entity gridA, + Transform gridAToWorldRigid, + Matrix3x2 gridAToWorld, + Matrix3x2 worldToGridA, + SharedMapSystem _map, + SharedPhysicsSystem _physicsSystem, + SharedTransformSystem xformSystem, + EntityQuery fixturesQuery, + EntityQuery physicsQuery, + EntityQuery xformQuery) tuple) => + { + if (tuple.gridA.Owner == gridBUid || !tuple.xformQuery.TryGetComponent(gridBUid, out var collidingXform)) - { - return true; - } + { + return true; + } - // If the other entity is lower ID and also moved then let that handle the collision. - if (tuple.gridA.Owner.Id > gridBUid.Id && tuple._physicsSystem.MovedGrids.Contains(gridBUid)) - { - return true; - } + // If the other entity is lower ID and also moved then let that handle the collision. + if (tuple.gridA.Owner.Id > gridBUid.Id && tuple._physicsSystem.MovedGrids.Contains(gridBUid)) + { + return true; + } - var (_, _, gridBToWorld, worldToGridB) = tuple.xformSystem.GetWorldPositionRotationMatrixWithInv(collidingXform); - var gridBAabbInWorld = gridBToWorld.TransformBox(gridBMapComp.LocalAABB); - // Same as gridBToWorld, just a rigid transform: - var gridBToWorldRigid = tuple._physicsSystem.GetPhysicsTransform(gridBUid); + var (_, _, gridBToWorld, worldToGridB) = tuple.xformSystem.GetWorldPositionRotationMatrixWithInv(collidingXform); + var gridBAabbInWorld = gridBToWorld.TransformBox(gridBMapComp.LocalAABB); + // Same as gridBToWorld, just a rigid transform: + var gridBToWorldRigid = tuple._physicsSystem.GetPhysicsTransform(gridBUid); - // Get Grid2 AABB in grid1 ref - var intersectionAabbInGridA = tuple.gridA.Comp2.LocalAABB.Intersect(tuple.worldToGridA.TransformBox(gridBAabbInWorld)); + // Get Grid2 AABB in grid1 ref + var intersectionAabbInGridA = tuple.gridA.Comp2.LocalAABB.Intersect(tuple.worldToGridA.TransformBox(gridBAabbInWorld)); - // TODO: AddPair has a nasty check in there that's O(n) but that's also a general physics problem. - var gridAChunks = tuple._map.GetLocalMapChunks(tuple.gridA.Owner, tuple.gridA, intersectionAabbInGridA); - var physicsA = tuple.gridA.Comp3; - var physicsB = tuple.physicsQuery.GetComponent(gridBUid); - var fixturesB = tuple.fixturesQuery.Comp(gridBUid); + // TODO: AddPair has a nasty check in there that's O(n) but that's also a general physics problem. + var gridAChunks = tuple._map.GetLocalMapChunks(tuple.gridA.Owner, tuple.gridA, intersectionAabbInGridA); + var physicsA = tuple.gridA.Comp3; + var physicsB = tuple.physicsQuery.GetComponent(gridBUid); + var fixturesB = tuple.fixturesQuery.Comp(gridBUid); - // Only care about chunks on other grid overlapping us. - while (gridAChunks.MoveNext(out var curChunkA)) - { - var chunkAAabbInWorld = - tuple.gridAToWorld.TransformBox( - curChunkA.CachedBounds.Translated(curChunkA.Indices * tuple.gridA.Comp2.ChunkSize)); - var chunkAAabbInGridB = worldToGridB.TransformBox(chunkAAabbInWorld); - var gridBOverlaps = tuple._map.GetLocalMapChunks(gridBUid, gridBMapComp, chunkAAabbInGridB); + // Only care about chunks on other grid overlapping us. + while (gridAChunks.MoveNext(out var curChunkA)) + { + var chunkAAabbInWorld = + tuple.gridAToWorld.TransformBox( + curChunkA.CachedBounds.Translated(curChunkA.Indices * tuple.gridA.Comp2.ChunkSize)); + var chunkAAabbInGridB = worldToGridB.TransformBox(chunkAAabbInWorld); + var gridBOverlaps = tuple._map.GetLocalMapChunks(gridBUid, gridBMapComp, chunkAAabbInGridB); - while (gridBOverlaps.MoveNext(out var curChunkB)) + while (gridBOverlaps.MoveNext(out var curChunkB)) + { + foreach (var fixAId in curChunkA.Fixtures) { - foreach (var fixAId in curChunkA.Fixtures) + var fixtureA = tuple.gridA.Comp1.Fixtures[fixAId]; + + foreach (var fixBId in curChunkB.Fixtures) { - var fixtureA = tuple.gridA.Comp1.Fixtures[fixAId]; + var fixtureB = fixturesB.Fixtures[fixBId]; - foreach (var fixBId in curChunkB.Fixtures) - { - var fixtureB = fixturesB.Fixtures[fixBId]; + // There's already a contact so ignore it. + if (fixtureA.Contacts.ContainsKey(fixtureB)) + continue; - // There's already a contact so ignore it. - if (fixtureA.Contacts.ContainsKey(fixtureB)) - continue; + bool addedPair = false; + for (var i = 0; i < fixtureA.Shape.ChildCount && !addedPair; i++) + { + var shapeAAabbInWorld = fixtureA.Shape.ComputeAABB(tuple.gridAToWorldRigid, i); - bool addedPair = false; - for (var i = 0; i < fixtureA.Shape.ChildCount && !addedPair; i++) + for (var j = 0; j < fixtureB.Shape.ChildCount && !addedPair; j++) { - var shapeAAabbInWorld = fixtureA.Shape.ComputeAABB(tuple.gridAToWorldRigid, i); - - for (var j = 0; j < fixtureB.Shape.ChildCount && !addedPair; j++) - { - var shapeBAabbInWorld = fixtureB.Shape.ComputeAABB(gridBToWorldRigid, j); - - if (!shapeAAabbInWorld.Intersects(shapeBAabbInWorld)) continue; - - tuple._physicsSystem.AddPair( - (tuple.gridA.Owner, tuple.gridA.Comp3, tuple.gridA.Comp4), - (gridBUid, physicsB, collidingXform), - fixAId, fixBId, - fixtureA, i, - fixtureB, j, - physicsA, physicsB, - ContactFlags.Grid); - - // Early out now and ignore every other i/j pair - addedPair = true; - // TODO: AddPair only handles a *single* contact between multiple - // fixtures, even if the contact is added for distinct child shapes i - // and j. Right now, there is only one child shape per fixture, but - // this code will fail to detect some collisions if this assumption is - // ever changed! - } + var shapeBAabbInWorld = fixtureB.Shape.ComputeAABB(gridBToWorldRigid, j); + + if (!shapeAAabbInWorld.Intersects(shapeBAabbInWorld)) continue; + + tuple._physicsSystem.AddPair( + (tuple.gridA.Owner, tuple.gridA.Comp3, tuple.gridA.Comp4), + (gridBUid, physicsB, collidingXform), + fixAId, fixBId, + fixtureA, i, + fixtureB, j, + physicsA, physicsB, + ContactFlags.Grid); + + // Early out now and ignore every other i/j pair + addedPair = true; + // TODO: AddPair only handles a *single* contact between multiple + // fixtures, even if the contact is added for distinct child shapes i + // and j. Right now, there is only one child shape per fixture, but + // this code will fail to detect some collisions if this assumption is + // ever changed! } } } } } + } - return true; - }, approx: true, includeMap: false); - } + return true; + }, approx: true, includeMap: false); } + } - #endregion + #endregion - private void FindPairs( - FixtureProxy proxy, - Box2 worldAABB, - EntityUid broadphase, - List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) - { - DebugTools.Assert(proxy.Body.CanCollide); + private void FindPairs( + FixtureProxy proxy, + Box2 worldAABB, + EntityUid broadphase, + List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) + { + DebugTools.Assert(proxy.Body.CanCollide); - // Broadphase can't intersect with entities on itself so skip. - if (proxy.Entity == broadphase || !_xformQuery.TryGetComponent(proxy.Entity, out var xform)) - { - return; - } + // Broadphase can't intersect with entities on itself so skip. + if (proxy.Entity == broadphase || !_xformQuery.TryGetComponent(proxy.Entity, out var xform)) + { + return; + } - // Logger.DebugS("physics", $"Checking proxy for {proxy.Entity} on {broadphase.Owner}"); - Box2 aabb; - DebugTools.AssertNotNull(xform.Broadphase); - if (!_lookup.TryGetCurrentBroadphase(xform, out var proxyBroad)) - { - Log.Error($"Found null broadphase for {ToPrettyString(proxy.Entity)}"); - DebugTools.Assert(false); - return; - } + // Logger.DebugS("physics", $"Checking proxy for {proxy.Entity} on {broadphase.Owner}"); + Box2 aabb; + DebugTools.AssertNotNull(xform.Broadphase); + if (!_lookup.TryGetCurrentBroadphase(xform, out var proxyBroad)) + { + Log.Error($"Found null broadphase for {ToPrettyString(proxy.Entity)}"); + DebugTools.Assert(false); + return; + } - // If it's the same broadphase as our body's one then don't need to translate the AABB. - if (proxyBroad.Owner == broadphase) - { - aabb = proxy.AABB; - } - else - { - aabb = _transform.GetInvWorldMatrix(broadphase).TransformBox(worldAABB); - } + // If it's the same broadphase as our body's one then don't need to translate the AABB. + if (proxyBroad.Owner == broadphase) + { + aabb = proxy.AABB; + } + else + { + aabb = _transform.GetInvWorldMatrix(broadphase).TransformBox(worldAABB); + } - var broadphaseComp = _broadphaseQuery.GetComponent(broadphase); - var state = (pairBuffer, _physicsSystem.MoveBuffer, this, _physicsSystem, proxy); + var broadphaseComp = _broadphaseQuery.GetComponent(broadphase); + var state = (pairBuffer, _physicsSystem.MoveBuffer, this, _physicsSystem, proxy); - QueryBroadphase(broadphaseComp.DynamicTree, state, aabb); + QueryBroadphase(broadphaseComp.DynamicTree, state, aabb); - if ((proxy.Body.BodyType & BodyType.Static) != 0x0) - return; + if ((proxy.Body.BodyType & BodyType.Static) != 0x0) + return; - QueryBroadphase(broadphaseComp.StaticTree, state, aabb); - } + QueryBroadphase(broadphaseComp.StaticTree, state, aabb); + } - private void QueryBroadphase(IBroadPhase broadPhase, (List<(FixtureProxy, FixtureProxy, PairFlag)>, HashSet MoveBuffer, SharedBroadphaseSystem Broadphase, SharedPhysicsSystem PhysicsSystem, FixtureProxy) state, Box2 aabb) + private void QueryBroadphase(IBroadPhase broadPhase, (List<(FixtureProxy, FixtureProxy, PairFlag)>, HashSet MoveBuffer, SharedBroadphaseSystem Broadphase, SharedPhysicsSystem PhysicsSystem, FixtureProxy) state, Box2 aabb) + { + broadPhase.QueryAabb(ref state, static ( + ref (List<(FixtureProxy, FixtureProxy, PairFlag)> pairs, HashSet moveBuffer, SharedBroadphaseSystem broadphase, SharedPhysicsSystem physicsSystem, FixtureProxy proxy) tuple, + in FixtureProxy other) => { - broadPhase.QueryAabb(ref state, static ( - ref (List<(FixtureProxy, FixtureProxy, PairFlag)> pairs, HashSet moveBuffer, SharedBroadphaseSystem broadphase, SharedPhysicsSystem physicsSystem, FixtureProxy proxy) tuple, - in FixtureProxy other) => - { - DebugTools.Assert(other.Body.CanCollide); - // Logger.DebugS("physics", $"Checking {proxy.Entity} against {other.Fixture.Body.Owner} at {aabb}"); - - if (tuple.proxy.Entity == other.Entity || - !SharedPhysicsSystem.ShouldCollide(tuple.proxy.Fixture, other.Fixture)) - { - return true; - } + DebugTools.Assert(other.Body.CanCollide); + // Logger.DebugS("physics", $"Checking {proxy.Entity} against {other.Fixture.Body.Owner} at {aabb}"); - // Avoid creating duplicate pairs. - // We give priority to whoever has the lower entity ID. - if (tuple.proxy.Entity.Id > other.Entity.Id) - { - // Let the other fixture handle it. - if (tuple.moveBuffer.Contains(other)) - return true; - } + if (tuple.proxy.Entity == other.Entity || + !SharedPhysicsSystem.ShouldCollide(tuple.proxy.Fixture, other.Fixture)) + return true; - // Check if contact already exists. - if (tuple.proxy.Fixture.Contacts.ContainsKey(other.Fixture)) + // Avoid creating duplicate pairs. + // We give priority to whoever has the lower entity ID. + if (tuple.proxy.Entity.Id > other.Entity.Id) + { + // Let the other fixture handle it. + if (tuple.moveBuffer.Contains(other)) return true; + } - // TODO: Add in the slow path check here but turnstiles currently explodes this on content so. - if (!tuple.physicsSystem.ShouldCollideJoints(tuple.proxy.Entity, other.Entity)) - return true; + // Check if contact already exists. + if (tuple.proxy.Fixture.Contacts.ContainsKey(other.Fixture)) + return true; - // TODO: Sensors handled elsewhere when we do v3 port. - //if (!tuple.proxy.Fixture.Hard || !other.Fixture.Hard) - // return true; + // TODO: Add in the slow path check here but turnstiles currently explodes this on content so. + if (!tuple.physicsSystem.ShouldCollideJoints(tuple.proxy.Entity, other.Entity)) + return true; - // TODO: Check if interlocked + array is better here which is what box2d does - // It then just heap allocates anything over the array size. - var flags = PairFlag.None; - if (tuple.proxy.Fixture.Hard && - other.Fixture.Hard && - (tuple.broadphase._gridMoveBuffer.Contains(tuple.proxy) || tuple.broadphase._gridMoveBuffer.Contains(other))) - { - flags |= PairFlag.Wake; - } + // TODO: Sensors handled elsewhere when we do v3 port. + //if (!tuple.proxy.Fixture.Hard || !other.Fixture.Hard) + // return true; - lock (tuple.pairs) - { - tuple.pairs.Add((tuple.proxy, other, flags)); - } + // TODO: Check if interlocked + array is better here which is what box2d does + // It then just heap allocates anything over the array size. + var flags = PairFlag.None; + if (tuple.proxy.Fixture.Hard && + other.Fixture.Hard && + (tuple.broadphase._gridMoveBuffer.Contains(tuple.proxy) || tuple.broadphase._gridMoveBuffer.Contains(other))) + flags |= PairFlag.Wake; - return true; - }, aabb, true); - } + lock (tuple.pairs) + { + tuple.pairs.Add((tuple.proxy, other, flags)); + } - [Obsolete("Use Entity variant")] - public void RegenerateContacts(EntityUid uid, PhysicsComponent body, FixturesComponent? fixtures = null, TransformComponent? xform = null) - { - RegenerateContacts((uid, body, fixtures, xform)); - } + return true; + }, aabb, true); + } - public void RegenerateContacts(Entity entity) - { - if (!Resolve(entity.Owner, ref entity.Comp1)) - return; + [Obsolete("Use Entity variant")] + public void RegenerateContacts(EntityUid uid, PhysicsComponent body, FixturesComponent? fixtures = null, TransformComponent? xform = null) + { + RegenerateContacts((uid, body, fixtures, xform)); + } - _physicsSystem.DestroyContacts(entity.Comp1); + public void RegenerateContacts(Entity entity) + { + if (!Resolve(entity.Owner, ref entity.Comp1)) + return; - if (!Resolve(entity.Owner, ref entity.Comp2 , ref entity.Comp3)) - return; + _physicsSystem.DestroyContacts(entity.Comp1); - if (entity.Comp3.MapUid == null) - return; + if (!Resolve(entity.Owner, ref entity.Comp2 , ref entity.Comp3)) + return; - if (!_xformQuery.TryGetComponent(entity.Comp3.Broadphase?.Uid, out var broadphase)) - return; + if (entity.Comp3.MapUid == null) + return; - _physicsSystem.SetAwake(entity!, true); + if (!_xformQuery.TryGetComponent(entity.Comp3.Broadphase?.Uid, out var broadphase)) + return; - foreach (var fixture in entity.Comp2.Fixtures.Values) - { - TouchProxies(fixture); - } - } + _physicsSystem.SetAwake(entity!, true); - internal void TouchProxies(Fixture fixture) + foreach (var fixture in entity.Comp2.Fixtures.Values) { - foreach (var proxy in fixture.Proxies) - { - AddToMoveBuffer(proxy); - } + TouchProxies(fixture); } + } - private void AddToMoveBuffer(FixtureProxy proxy) + internal void TouchProxies(Fixture fixture) + { + foreach (var proxy in fixture.Proxies) { - DebugTools.Assert(proxy.Body.CanCollide); - _physicsSystem.MoveBuffer.Add(proxy); + AddToMoveBuffer(proxy); } + } - public void Refilter(EntityUid uid, Fixture fixture, TransformComponent? xform = null) + private void AddToMoveBuffer(FixtureProxy proxy) + { + DebugTools.Assert(proxy.Body.CanCollide); + _physicsSystem.MoveBuffer.Add(proxy); + } + + public void Refilter(EntityUid uid, Fixture fixture, TransformComponent? xform = null) + { + foreach (var contact in fixture.Contacts.Values) { - foreach (var contact in fixture.Contacts.Values) - { - contact.Flags |= ContactFlags.Filter; - } + contact.Flags |= ContactFlags.Filter; + } - if (!Resolve(uid, ref xform)) - return; + if (!Resolve(uid, ref xform)) + return; - if (xform.MapUid == null) - return; + if (xform.MapUid == null) + return; - if (!_xformQuery.TryGetComponent(xform.Broadphase?.Uid, out var broadphase)) - return; + if (!_xformQuery.TryGetComponent(xform.Broadphase?.Uid, out var broadphase)) + return; - TouchProxies(fixture); - } + TouchProxies(fixture); + } - internal void GetBroadphases(MapId mapId, Box2 aabb, BroadphaseCallback callback) - { - var internalState = (callback, _broadphaseQuery); + internal void GetBroadphases(MapId mapId, Box2 aabb, BroadphaseCallback callback) + { + var internalState = (callback, _broadphaseQuery); - if (!_map.TryGetMap(mapId, out var map)) - return; + if (!_map.TryGetMap(mapId, out var map)) + return; - if (_broadphaseQuery.TryGetComponent(map.Value, out var mapBroadphase)) - callback((map.Value, mapBroadphase)); + if (_broadphaseQuery.TryGetComponent(map.Value, out var mapBroadphase)) + callback((map.Value, mapBroadphase)); - _mapManager.FindGridsIntersecting(map.Value, - aabb, - ref internalState, - static ( - EntityUid uid, - MapGridComponent _, - ref (BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => - { - if (tuple._broadphaseQuery.TryComp(uid, out var broadphase)) - tuple.callback((uid, broadphase)); + _mapManager.FindGridsIntersecting(map.Value, + aabb, + ref internalState, + static ( + EntityUid uid, + MapGridComponent _, + ref (BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => + { + if (tuple._broadphaseQuery.TryComp(uid, out var broadphase)) + tuple.callback((uid, broadphase)); - return true; - }, - // Approx because we don't really need accurate checks for these most of the time. - approx: true, - includeMap: false); - } + return true; + }, + // Approx because we don't really need accurate checks for these most of the time. + approx: true, + includeMap: false); + } - internal void GetBroadphases(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback callback) - { - var internalState = (state, callback, _broadphaseQuery); + internal void GetBroadphases(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback callback) + { + var internalState = (state, callback, _broadphaseQuery); - if (!_map.TryGetMap(mapId, out var map)) - return; + if (!_map.TryGetMap(mapId, out var map)) + return; - if (_broadphaseQuery.TryGetComponent(map.Value, out var mapBroadphase)) - callback((map.Value, mapBroadphase), ref state); + if (_broadphaseQuery.TryGetComponent(map.Value, out var mapBroadphase)) + callback((map.Value, mapBroadphase), ref state); - _mapManager.FindGridsIntersecting(map.Value, - aabb, - ref internalState, - static ( - EntityUid uid, - MapGridComponent _, - ref (TState state, BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => - { - if (tuple._broadphaseQuery.TryComp(uid, out var broadphase)) - tuple.callback((uid, broadphase), ref tuple.state); - return true; - }, - // Approx because we don't really need accurate checks for these most of the time. - approx: true, - includeMap: false); + _mapManager.FindGridsIntersecting(map.Value, + aabb, + ref internalState, + static ( + EntityUid uid, + MapGridComponent _, + ref (TState state, BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => + { + if (tuple._broadphaseQuery.TryComp(uid, out var broadphase)) + tuple.callback((uid, broadphase), ref tuple.state); + return true; + }, + // Approx because we don't really need accurate checks for these most of the time. + approx: true, + includeMap: false); - state = internalState.state; - } + state = internalState.state; + } - internal delegate void BroadphaseCallback(Entity entity); + internal delegate void BroadphaseCallback(Entity entity); - internal delegate void BroadphaseCallback(Entity entity, ref TState state); + internal delegate void BroadphaseCallback(Entity entity, ref TState state); - private record struct BroadphaseContactJob() : IParallelRobustJob - { - public SharedBroadphaseSystem System = default!; - public SharedTransformSystem TransformSys = default!; - public IMapManager MapManager = default!; + private record struct BroadphaseContactJob() : IParallelRobustJob + { + public SharedBroadphaseSystem System = default!; + public SharedTransformSystem TransformSys = default!; + public IMapManager MapManager = default!; - public EntityQuery XformQuery; + public EntityQuery XformQuery; - public readonly List MoveBuffer = new(); + public readonly List MoveBuffer = new(); - public List<(FixtureProxy, FixtureProxy, PairFlag)> Pairs = new(64); + public List<(FixtureProxy, FixtureProxy, PairFlag)> Pairs = new(64); - public float FrameTime; + public float FrameTime; - // Box2D uses 64 but we have to do grid queries for each fixtureproxy which will add a fair bit of overhead. - // Plus we also run events + trycomp for joints on top. - public int BatchSize => 16; + // Box2D uses 64 but we have to do grid queries for each fixtureproxy which will add a fair bit of overhead. + // Plus we also run events + trycomp for joints on top. + public int BatchSize => 16; - public void Execute(int index) - { - var proxy = MoveBuffer[index]; - var broadphaseUid = XformQuery.GetComponent(proxy.Entity).Broadphase?.Uid; - var worldAABB = TransformSys.GetWorldMatrix(broadphaseUid!.Value).TransformBox(proxy.AABB); + public void Execute(int index) + { + var proxy = MoveBuffer[index]; + var broadphaseUid = XformQuery.GetComponent(proxy.Entity).Broadphase?.Uid; + var worldAABB = TransformSys.GetWorldMatrix(broadphaseUid!.Value).TransformBox(proxy.AABB); - var mapUid = XformQuery.GetComponent(proxy.Entity).MapUid ?? EntityUid.Invalid; + var mapUid = XformQuery.GetComponent(proxy.Entity).MapUid ?? EntityUid.Invalid; - var broadphaseExpand = System.GetBroadphaseExpand(proxy.Body, FrameTime); + var broadphaseExpand = System.GetBroadphaseExpand(proxy.Body, FrameTime); - var proxyBody = proxy.Body; - DebugTools.Assert(!proxyBody.Deleted); + var proxyBody = proxy.Body; + DebugTools.Assert(!proxyBody.Deleted); - var state = (System, proxy, worldAABB, Pairs); + var state = (System, proxy, worldAABB, Pairs); - // Get every broadphase we may be intersecting. - MapManager.FindGridsIntersecting(mapUid, worldAABB.Enlarged(broadphaseExpand), ref state, - static (EntityUid uid, MapGridComponent _, ref ( - SharedBroadphaseSystem system, - FixtureProxy proxy, - Box2 worldAABB, - List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) tuple) => - { - ref var buffer = ref tuple.pairBuffer; - tuple.system.FindPairs(tuple.proxy, tuple.worldAABB, uid, buffer); - return true; - }, - approx: true, - includeMap: false); + // Get every broadphase we may be intersecting. + MapManager.FindGridsIntersecting(mapUid, worldAABB.Enlarged(broadphaseExpand), ref state, + static (EntityUid uid, MapGridComponent _, ref ( + SharedBroadphaseSystem system, + FixtureProxy proxy, + Box2 worldAABB, + List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) tuple) => + { + ref var buffer = ref tuple.pairBuffer; + tuple.system.FindPairs(tuple.proxy, tuple.worldAABB, uid, buffer); + return true; + }, + approx: true, + includeMap: false); - // Struct ref moment, I have no idea what's fastest. - System.FindPairs(proxy, worldAABB, mapUid, Pairs); - } + // Struct ref moment, I have no idea what's fastest. + System.FindPairs(proxy, worldAABB, mapUid, Pairs); } + } - [Flags] - private enum PairFlag : byte - { - None = 0, + [Flags] + private enum PairFlag : byte + { + None = 0, - /// - /// Should we wake the contacting entities. - /// - Wake = 1 << 0, + /// + /// Should we wake the contacting entities. + /// + Wake = 1 << 0, - /// - /// Is it a grid collision. - /// - Grid = 1 << 1, - } + /// + /// Is it a grid collision. + /// + Grid = 1 << 1, } }