From 32bdc19fe9cbf365dee06faefeec94e625c380f4 Mon Sep 17 00:00:00 2001 From: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com> Date: Sat, 14 May 2022 14:54:31 +1000 Subject: [PATCH] Final grid movement optimisation (#2711) --- Robust.Client/Debugging/DebugPhysicsSystem.cs | 33 +++-- .../GridChunkBoundsDebugSystem.cs | 2 +- .../Graphics/Clyde/Clyde.GridRendering.cs | 2 +- Robust.Client/Physics/GridFixtureSystem.cs | 2 +- Robust.Server/Maps/MapLoader.cs | 2 - .../Collidable/PhysicsComponent.Physics.cs | 8 +- Robust.Shared/Map/CoordinatesExtensions.cs | 2 +- Robust.Shared/Map/FindGridsEnumerator.cs | 4 +- Robust.Shared/Map/IMapGrid.cs | 6 +- Robust.Shared/Map/IMapGridInternal.cs | 4 +- Robust.Shared/Map/IMapManagerInternal.cs | 1 + Robust.Shared/Map/MapGrid.cs | 53 ++++--- Robust.Shared/Map/MapManager.GridTrees.cs | 9 +- Robust.Shared/Map/MapManager.Queries.cs | 2 +- .../Physics/Dynamics/ContactManager.cs | 59 +++++--- .../Physics/Dynamics/Contacts/Contact.cs | 27 +++- .../Dynamics/SharedPhysicsMapComponent.cs | 6 +- .../Physics/SharedBroadphaseSystem.cs | 136 ++++++++++++++---- Robust.Shared/Physics/SharedJointSystem.cs | 3 +- .../Shared/Map/GridCollision_Test.cs | 4 +- .../Shared/Map/MapGrid_Tests.cs | 4 +- 21 files changed, 257 insertions(+), 112 deletions(-) diff --git a/Robust.Client/Debugging/DebugPhysicsSystem.cs b/Robust.Client/Debugging/DebugPhysicsSystem.cs index 299156eba..8f63da0e6 100644 --- a/Robust.Client/Debugging/DebugPhysicsSystem.cs +++ b/Robust.Client/Debugging/DebugPhysicsSystem.cs @@ -52,6 +52,7 @@ using Robust.Client.ResourceManagement; using Robust.Shared.Enums; using Robust.Shared.GameObjects; using Robust.Shared.IoC; +using Robust.Shared.Map; using Robust.Shared.Maths; using Robust.Shared.Physics; using Robust.Shared.Physics.Collision; @@ -88,6 +89,7 @@ namespace Robust.Client.Debugging EntityManager, IoCManager.Resolve(), IoCManager.Resolve(), + IoCManager.Resolve(), IoCManager.Resolve(), this, Get())); @@ -176,6 +178,7 @@ namespace Robust.Client.Debugging private IEntityManager _entityManager = default!; private IEyeManager _eyeManager = default!; private IInputManager _inputManager = default!; + private IMapManager _mapManager = default!; private DebugPhysicsSystem _debugPhysicsSystem = default!; private SharedPhysicsSystem _physicsSystem = default!; @@ -187,11 +190,12 @@ namespace Robust.Client.Debugging private HashSet _drawnJoints = new(); - public PhysicsDebugOverlay(IEntityManager entityManager, IEyeManager eyeManager, IInputManager inputManager, IResourceCache cache, DebugPhysicsSystem system, SharedPhysicsSystem physicsSystem) + public PhysicsDebugOverlay(IEntityManager entityManager, IEyeManager eyeManager, IInputManager inputManager, IMapManager mapManager, IResourceCache cache, DebugPhysicsSystem system, SharedPhysicsSystem physicsSystem) { _entityManager = entityManager; _eyeManager = eyeManager; _inputManager = inputManager; + _mapManager = mapManager; _debugPhysicsSystem = system; _physicsSystem = physicsSystem; _font = new VectorFont(cache.GetResource("/Fonts/NotoSans/NotoSans-Regular.ttf"), 10); @@ -246,26 +250,21 @@ namespace Robust.Client.Debugging if ((_debugPhysicsSystem.Flags & PhysicsDebugFlags.COM) != 0) { + const float Alpha = 0.25f; + foreach (var physBody in _physicsSystem.GetCollidingEntities(mapId, viewBounds)) { - Color color; - const float Alpha = 0.25f; - float size; - - if (_entityManager.HasComponent(physBody.Owner)) - { - color = Color.Orange.WithAlpha(Alpha); - size = 1f; - } - else - { - color = Color.Purple.WithAlpha(Alpha); - size = 0.2f; - } - + var color = Color.Purple.WithAlpha(Alpha); var transform = physBody.GetTransform(); + worldHandle.DrawCircle(Transform.Mul(transform, physBody.LocalCenter), 0.2f, color); + } - worldHandle.DrawCircle(Transform.Mul(transform, physBody.LocalCenter), size, color); + foreach (var grid in _mapManager.FindGridsIntersecting(mapId, viewBounds)) + { + var physBody = _entityManager.GetComponent(grid.GridEntityId); + var color = Color.Orange.WithAlpha(Alpha); + var transform = physBody.GetTransform(); + worldHandle.DrawCircle(Transform.Mul(transform, physBody.LocalCenter), 1f, color); } } diff --git a/Robust.Client/GameObjects/EntitySystems/GridChunkBoundsDebugSystem.cs b/Robust.Client/GameObjects/EntitySystems/GridChunkBoundsDebugSystem.cs index 13348642e..45850c106 100644 --- a/Robust.Client/GameObjects/EntitySystems/GridChunkBoundsDebugSystem.cs +++ b/Robust.Client/GameObjects/EntitySystems/GridChunkBoundsDebugSystem.cs @@ -76,7 +76,7 @@ namespace Robust.Client.GameObjects worldHandle.SetTransform(worldMatrix); var transform = new Transform(Vector2.Zero, Angle.Zero); - gridInternal.GetMapChunks(viewport, out var chunkEnumerator); + var chunkEnumerator = gridInternal.GetMapChunks(viewport); while (chunkEnumerator.MoveNext(out var chunk)) { diff --git a/Robust.Client/Graphics/Clyde/Clyde.GridRendering.cs b/Robust.Client/Graphics/Clyde/Clyde.GridRendering.cs index 04504f11a..b2dd5eab7 100644 --- a/Robust.Client/Graphics/Clyde/Clyde.GridRendering.cs +++ b/Robust.Client/Graphics/Clyde/Clyde.GridRendering.cs @@ -50,7 +50,7 @@ namespace Robust.Client.Graphics.Clyde var transform = _entityManager.GetComponent(grid.GridEntityId); gridProgram.SetUniform(UniIModelMatrix, transform.WorldMatrix); - grid.GetMapChunks(worldBounds, out var enumerator); + var enumerator = grid.GetMapChunks(worldBounds); while (enumerator.MoveNext(out var chunk)) { diff --git a/Robust.Client/Physics/GridFixtureSystem.cs b/Robust.Client/Physics/GridFixtureSystem.cs index 4a22ae29f..9120264c3 100644 --- a/Robust.Client/Physics/GridFixtureSystem.cs +++ b/Robust.Client/Physics/GridFixtureSystem.cs @@ -89,7 +89,7 @@ namespace Robust.Client.Physics var gridXform = xformQuery.GetComponent(iGrid.GridEntityId); worldHandle.SetTransform(gridXform.WorldMatrix); var grid = (MapGrid)iGrid; - grid.GetMapChunks(args.WorldBounds, out var chunkEnumerator); + var chunkEnumerator = grid.GetMapChunks(args.WorldBounds); while (chunkEnumerator.MoveNext(out var chunk)) { diff --git a/Robust.Server/Maps/MapLoader.cs b/Robust.Server/Maps/MapLoader.cs index f3ad0854b..45ab5eb78 100644 --- a/Robust.Server/Maps/MapLoader.cs +++ b/Robust.Server/Maps/MapLoader.cs @@ -469,8 +469,6 @@ namespace Robust.Server.Maps { var gridInternal = (IMapGridInternal) grid; var body = entManager.EnsureComponent(grid.GridEntityId); - var mapUid = _mapManager.GetMapEntityIdOrThrow(grid.ParentMapId); - body.Broadphase = entManager.GetComponent(mapUid); var fixtures = entManager.EnsureComponent(grid.GridEntityId); // Regenerate grid collision. gridFixtures.EnsureGrid(grid.GridEntityId); diff --git a/Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs b/Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs index bf68a6959..a75c53cfc 100644 --- a/Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs +++ b/Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs @@ -54,6 +54,7 @@ namespace Robust.Shared.GameObjects /// public bool Island { get; set; } + [ViewVariables] internal BroadphaseComponent? Broadphase { get; set; } /// @@ -659,7 +660,12 @@ namespace Robust.Shared.GameObjects public Transform GetTransform() { - var (worldPos, worldRot) = _entMan.GetComponent(Owner).GetWorldPositionRotation(); + return GetTransform(_entMan.GetComponent(Owner)); + } + + public Transform GetTransform(TransformComponent xform) + { + var (worldPos, worldRot) = xform.GetWorldPositionRotation(); var xf = new Transform(worldPos, (float) worldRot.Theta); // xf.Position -= Transform.Mul(xf.Quaternion2D, LocalCenter); diff --git a/Robust.Shared/Map/CoordinatesExtensions.cs b/Robust.Shared/Map/CoordinatesExtensions.cs index 8b495cb51..f7c9e5672 100644 --- a/Robust.Shared/Map/CoordinatesExtensions.cs +++ b/Robust.Shared/Map/CoordinatesExtensions.cs @@ -42,7 +42,7 @@ namespace Robust.Shared.Map // TODO: Use CollisionManager to get nearest edge. // figure out closest intersect - var gridIntersect = gridSearchBox.Intersect(grid.WorldBounds); + var gridIntersect = gridSearchBox.Intersect(grid.WorldAABB); var gridDist = (gridIntersect.Center - mapCoords.Position).LengthSquared; if (gridDist >= distance) diff --git a/Robust.Shared/Map/FindGridsEnumerator.cs b/Robust.Shared/Map/FindGridsEnumerator.cs index c7dfbc14a..ded905e15 100644 --- a/Robust.Shared/Map/FindGridsEnumerator.cs +++ b/Robust.Shared/Map/FindGridsEnumerator.cs @@ -48,13 +48,13 @@ namespace Robust.Shared.Map var invMatrix3 = xformComp.InvWorldMatrix; var localAABB = invMatrix3.TransformBox(_worldAABB); - if (!localAABB.Intersects(nextGrid.LocalBounds)) continue; + if (!localAABB.Intersects(nextGrid.LocalAABB)) continue; var intersects = false; if (_entityManager.HasComponent(nextGrid.GridEntityId)) { - nextGrid.GetLocalMapChunks(localAABB, out var enumerator); + var enumerator = nextGrid.GetLocalMapChunks(localAABB); if (!_approx) { diff --git a/Robust.Shared/Map/IMapGrid.cs b/Robust.Shared/Map/IMapGrid.cs index 0ab8b1e7a..478d2c30e 100644 --- a/Robust.Shared/Map/IMapGrid.cs +++ b/Robust.Shared/Map/IMapGrid.cs @@ -32,15 +32,17 @@ namespace Robust.Shared.Map /// ushort TileSize { get; } + Box2Rotated WorldBounds { get; } + /// /// The bounding box of the grid in world coordinates. /// - Box2 WorldBounds { get; } + Box2 WorldAABB { get; } /// /// The bounding box of the grid in local coordinates. /// - Box2 LocalBounds { get; } + Box2 LocalAABB { get; } /// /// The length of a side of the square chunk in number of tiles. diff --git a/Robust.Shared/Map/IMapGridInternal.cs b/Robust.Shared/Map/IMapGridInternal.cs index 1c1ec6f46..f45e73e38 100644 --- a/Robust.Shared/Map/IMapGridInternal.cs +++ b/Robust.Shared/Map/IMapGridInternal.cs @@ -54,12 +54,12 @@ namespace Robust.Shared.Map /// /// Returns all the intersecting the worldAABB. /// - void GetMapChunks(Box2 worldAABB, out MapGrid.ChunkEnumerator enumerator); + MapGrid.ChunkEnumerator GetMapChunks(Box2 worldAABB); /// /// Returns all the intersecting the rotated world box. /// - void GetMapChunks(Box2Rotated worldArea, out MapGrid.ChunkEnumerator enumerator); + MapGrid.ChunkEnumerator GetMapChunks(Box2Rotated worldArea); /// /// Regenerates the chunk local bounds of this chunk. diff --git a/Robust.Shared/Map/IMapManagerInternal.cs b/Robust.Shared/Map/IMapManagerInternal.cs index 6dfb0c012..70888ad67 100644 --- a/Robust.Shared/Map/IMapManagerInternal.cs +++ b/Robust.Shared/Map/IMapManagerInternal.cs @@ -51,5 +51,6 @@ namespace Robust.Shared.Map void TrueDeleteMap(MapId mapId); GridId GenerateGridId(GridId? forcedGridId); void OnGridAllocated(MapGridComponent gridComponent, MapGrid mapGrid); + void OnGridBoundsChange(EntityUid uid, MapGrid grid); } } diff --git a/Robust.Shared/Map/MapGrid.cs b/Robust.Shared/Map/MapGrid.cs index cb109f900..69e289987 100644 --- a/Robust.Shared/Map/MapGrid.cs +++ b/Robust.Shared/Map/MapGrid.cs @@ -83,14 +83,25 @@ namespace Robust.Shared.Map /// [ViewVariables] - public Box2 WorldBounds => - new Box2Rotated(LocalBounds, WorldRotation, Vector2.Zero) + public Box2Rotated WorldBounds + { + get + { + var worldAABB = LocalAABB.Translated(WorldPosition); + return new Box2Rotated(worldAABB, WorldRotation, worldAABB.Center); + } + } + + /// + [ViewVariables] + public Box2 WorldAABB => + new Box2Rotated(LocalAABB, WorldRotation, Vector2.Zero) .CalcBoundingBox() .Translated(WorldPosition); /// [ViewVariables] - public Box2 LocalBounds { get; private set; } + public Box2 LocalAABB { get; private set; } /// [ViewVariables] @@ -479,24 +490,23 @@ namespace Robust.Shared.Map } /// - public void GetMapChunks(Box2 worldAABB, out ChunkEnumerator enumerator) + public ChunkEnumerator GetMapChunks(Box2 worldAABB) { - var localArea = InvWorldMatrix.TransformBox(worldAABB); - enumerator = new ChunkEnumerator(_chunks, localArea, ChunkSize); + var localAABB = InvWorldMatrix.TransformBox(worldAABB); + return new ChunkEnumerator(_chunks, localAABB, ChunkSize); } /// - public void GetMapChunks(Box2Rotated worldArea, out ChunkEnumerator enumerator) + public ChunkEnumerator GetMapChunks(Box2Rotated worldArea) { var matrix = InvWorldMatrix; var localArea = matrix.TransformBox(worldArea); - - enumerator = new ChunkEnumerator(_chunks, localArea, ChunkSize); + return new ChunkEnumerator(_chunks, localArea, ChunkSize); } - public void GetLocalMapChunks(Box2 localAABB, out ChunkEnumerator enumerator) + public ChunkEnumerator GetLocalMapChunks(Box2 localAABB) { - enumerator = new ChunkEnumerator(_chunks, localAABB, ChunkSize); + return new ChunkEnumerator(_chunks, localAABB, ChunkSize); } #endregion ChunkAccess @@ -914,7 +924,7 @@ namespace Robust.Shared.Map } } - LocalBounds = new Box2(); + LocalAABB = new Box2(); foreach (var chunk in _chunks.Values) { var chunkBounds = chunk.CachedBounds; @@ -922,18 +932,20 @@ namespace Robust.Shared.Map if(chunkBounds.Size.Equals(Vector2i.Zero)) continue; - if (LocalBounds.Size == Vector2.Zero) + if (LocalAABB.Size == Vector2.Zero) { var gridBounds = chunkBounds.Translated(chunk.Indices * chunk.ChunkSize); - LocalBounds = gridBounds; + LocalAABB = gridBounds; } else { var gridBounds = chunkBounds.Translated(chunk.Indices * chunk.ChunkSize); - LocalBounds = LocalBounds.Union(gridBounds); + LocalAABB = LocalAABB.Union(gridBounds); } } + _mapManager.OnGridBoundsChange(GridEntityId, this); + if (chunkRectangles.Count == 0) { // May have been deleted from the bulk update above! @@ -965,7 +977,7 @@ namespace Robust.Shared.Map GridChunkPartition.PartitionChunk(mapChunk, out var localBounds, out var rectangles); mapChunk.CachedBounds = localBounds; - LocalBounds = new Box2(); + LocalAABB = new Box2(); foreach (var chunk in _chunks.Values) { var chunkBounds = chunk.CachedBounds; @@ -973,20 +985,23 @@ namespace Robust.Shared.Map if(chunkBounds.Size.Equals(Vector2i.Zero)) continue; - if (LocalBounds.Size == Vector2.Zero) + if (LocalAABB.Size == Vector2.Zero) { var gridBounds = chunkBounds.Translated(chunk.Indices * chunk.ChunkSize); - LocalBounds = gridBounds; + LocalAABB = gridBounds; } else { var gridBounds = chunkBounds.Translated(chunk.Indices * chunk.ChunkSize); - LocalBounds = LocalBounds.Union(gridBounds); + LocalAABB = LocalAABB.Union(gridBounds); } } if (!_entityManager.EntitySysManager.TryGetEntitySystem(out SharedGridFixtureSystem? system) || _entityManager.Deleted(GridEntityId)) return; + + // TODO: Move this to the component when we combine. + _mapManager.OnGridBoundsChange(GridEntityId, this); // TryGet because unit tests YAY if (mapChunk.FilledTiles > 0) diff --git a/Robust.Shared/Map/MapManager.GridTrees.cs b/Robust.Shared/Map/MapManager.GridTrees.cs index 94d719968..889adcd58 100644 --- a/Robust.Shared/Map/MapManager.GridTrees.cs +++ b/Robust.Shared/Map/MapManager.GridTrees.cs @@ -33,7 +33,6 @@ internal partial class MapManager EntityManager.EventBus.SubscribeLocalEvent(OnGridMove); EntityManager.EventBus.SubscribeLocalEvent(OnGridRotate); EntityManager.EventBus.SubscribeLocalEvent(OnGridParentChange); - EntityManager.EventBus.SubscribeLocalEvent(OnGridBoundsChange); } private void ShutdownGridTrees() @@ -43,7 +42,6 @@ internal partial class MapManager EntityManager.EventBus.UnsubscribeLocalEvent(); EntityManager.EventBus.UnsubscribeLocalEvent(); EntityManager.EventBus.UnsubscribeLocalEvent(); - EntityManager.EventBus.UnsubscribeLocalEvent(); DebugTools.Assert(_gridTrees.Count == 0); DebugTools.Assert(_movedGrids.Count == 0); @@ -71,7 +69,7 @@ internal partial class MapManager var (worldPos, worldRot) = xform.GetWorldPositionRotation(); - return new Box2Rotated(grid.LocalBounds.Translated(worldPos), worldRot, worldPos).CalcBoundingBox(); + return new Box2Rotated(grid.LocalAABB, worldRot).CalcBoundingBox().Translated(worldPos); } private void OnGridInit(GridInitializeEvent args) @@ -171,15 +169,14 @@ internal partial class MapManager } } - private void OnGridBoundsChange(EntityUid uid, MapGridComponent component, GridFixtureChangeEvent args) + public void OnGridBoundsChange(EntityUid uid, MapGrid grid) { - var grid = (MapGrid) component.Grid; - // Just MapLoader things. if (grid.MapProxy == DynamicTree.Proxy.Free) return; var xform = EntityManager.GetComponent(uid); var aabb = GetWorldAABB(grid); _gridTrees[xform.MapID].MoveProxy(grid.MapProxy, in aabb, Vector2.Zero); + _movedGrids[xform.MapID].Add(grid); } } diff --git a/Robust.Shared/Map/MapManager.Queries.cs b/Robust.Shared/Map/MapManager.Queries.cs index d13738865..0d2e83cd6 100644 --- a/Robust.Shared/Map/MapManager.Queries.cs +++ b/Robust.Shared/Map/MapManager.Queries.cs @@ -67,7 +67,7 @@ internal partial class MapManager if (physicsQuery.HasComponent(grid.GridEntityId)) { - grid.GetLocalMapChunks(localAABB, out var enumerator); + var enumerator = grid.GetLocalMapChunks(localAABB); var transform = new Transform(worldPos, worldRot); diff --git a/Robust.Shared/Physics/Dynamics/ContactManager.cs b/Robust.Shared/Physics/Dynamics/ContactManager.cs index afa7d5734..d60121f9a 100644 --- a/Robust.Shared/Physics/Dynamics/ContactManager.cs +++ b/Robust.Shared/Physics/Dynamics/ContactManager.cs @@ -48,6 +48,7 @@ namespace Robust.Shared.Physics.Dynamics { [Dependency] private readonly IEntityManager _entityManager = default!; [Dependency] private readonly IPhysicsManager _physicsManager = default!; + private SharedTransformSystem _transform = default!; internal MapId MapId { get; set; } @@ -134,8 +135,7 @@ namespace Robust.Shared.Physics.Dynamics { contact.Enabled = true; contact.IsTouching = false; - contact.IslandFlag = false; - contact.FilterFlag = false; + contact.Flags = ContactFlags.None; // TOIFlag = false; contact.FixtureA = fixtureA; @@ -159,6 +159,7 @@ namespace Robust.Shared.Physics.Dynamics public void Initialize() { IoCManager.InjectDependencies(this); + _transform = IoCManager.Resolve().GetEntitySystem(); var configManager = IoCManager.Resolve(); configManager.OnValueChanged(CVars.ContactMultithreadThreshold, OnContactMultithreadThreshold, true); configManager.OnValueChanged(CVars.ContactMinimumThreads, OnContactMinimumThreads, true); @@ -225,16 +226,10 @@ namespace Robust.Shared.Physics.Dynamics } /// - /// Go through the cached broadphase movement and update contacts. + /// Try to create a contact between these 2 fixtures. /// - internal void AddPair(in FixtureProxy proxyA, in FixtureProxy proxyB) + internal void AddPair(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB, ContactFlags flags = ContactFlags.None) { - Fixture fixtureA = proxyA.Fixture; - Fixture fixtureB = proxyB.Fixture; - - var indexA = proxyA.ChildIndex; - var indexB = proxyB.ChildIndex; - PhysicsComponent bodyA = fixtureA.Body; PhysicsComponent bodyB = fixtureB.Body; @@ -253,6 +248,7 @@ namespace Robust.Shared.Physics.Dynamics // Call the factory. var contact = CreateContact(fixtureA, indexA, fixtureB, indexB); + contact.Flags = flags; // Contact creation may swap fixtures. fixtureA = contact.FixtureA!; @@ -274,6 +270,14 @@ namespace Robust.Shared.Physics.Dynamics contact.BodyBNode = bodyB.Contacts.AddLast(contact); } + /// + /// Go through the cached broadphase movement and update contacts. + /// + internal void AddPair(in FixtureProxy proxyA, in FixtureProxy proxyB) + { + AddPair(proxyA.Fixture, proxyA.ChildIndex, proxyB.Fixture, proxyB.ChildIndex); + } + internal static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) { return !((fixtureA.CollisionMask & fixtureB.CollisionLayer) == 0x0 && @@ -335,6 +339,7 @@ namespace Robust.Shared.Physics.Dynamics // TODO: check for null instead? // Work out which contacts are still valid before we decide to update manifolds. var node = _activeContacts.First; + var xformQuery = _entityManager.GetEntityQuery(); while (node != null) { @@ -356,7 +361,7 @@ namespace Robust.Shared.Physics.Dynamics } // Is this contact flagged for filtering? - if (contact.FilterFlag) + if ((contact.Flags & ContactFlags.Filter) != 0x0) { // Should these bodies collide? if (bodyB.ShouldCollide(bodyA) == false) @@ -386,7 +391,7 @@ namespace Robust.Shared.Physics.Dynamics */ // Clear the filtering flag. - contact.FilterFlag = false; + contact.Flags &= ~ContactFlags.Filter; } bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static; @@ -399,12 +404,32 @@ namespace Robust.Shared.Physics.Dynamics continue; } + // Special-case grid contacts. + if ((contact.Flags & ContactFlags.Grid) != 0x0) + { + var xformA = xformQuery.GetComponent(bodyA.Owner); + var xformB = xformQuery.GetComponent(bodyB.Owner); + + var gridABounds = fixtureA.Shape.ComputeAABB(bodyA.GetTransform(xformA), 0); + var gridBBounds = fixtureB.Shape.ComputeAABB(bodyB.GetTransform(xformB), 0); + + if (!gridABounds.Intersects(gridBBounds)) + { + Destroy(contact); + } + else + { + contacts[index++] = contact; + } + + node = node.Next; + continue; + } + var proxyA = fixtureA.Proxies[indexA]; var proxyB = fixtureB.Proxies[indexB]; var broadphaseA = bodyA.Broadphase; var broadphaseB = bodyB.Broadphase; - - // TODO: IT MIGHT BE THE FATAABB STUFF FOR MOVEPROXY SO TRY THAT var overlap = false; // We can have cross-broadphase proxies hence need to change them to worldspace @@ -417,10 +442,10 @@ namespace Robust.Shared.Physics.Dynamics else { // These should really be destroyed before map changes. - DebugTools.Assert(_entityManager.GetComponent(broadphaseA.Owner).MapID == _entityManager.GetComponent(broadphaseB.Owner).MapID); + DebugTools.Assert(xformQuery.GetComponent(broadphaseA.Owner).MapID == xformQuery.GetComponent(broadphaseB.Owner).MapID); - var proxyAWorldAABB = _entityManager.GetComponent(broadphaseA.Owner).WorldMatrix.TransformBox(proxyA.AABB); - var proxyBWorldAABB = _entityManager.GetComponent(broadphaseB.Owner).WorldMatrix.TransformBox(proxyB.AABB); + var proxyAWorldAABB = _transform.GetWorldMatrix(broadphaseA.Owner, xformQuery).TransformBox(proxyA.AABB); + var proxyBWorldAABB = _transform.GetWorldMatrix(broadphaseB.Owner, xformQuery).TransformBox(proxyB.AABB); overlap = proxyAWorldAABB.Intersects(proxyBWorldAABB); } } diff --git a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs index 5dc93b14c..2afb7bb52 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs @@ -69,12 +69,7 @@ namespace Robust.Shared.Physics.Dynamics.Contacts internal ContactType Type; - /// - /// Has this contact already been added to an island? - /// - public bool IslandFlag { get; set; } - - public bool FilterFlag { get; set; } + internal ContactFlags Flags = ContactFlags.None; /// /// Determines whether the contact is touching. @@ -318,4 +313,24 @@ namespace Robust.Shared.Physics.Dynamics.Contacts return HashCode.Combine((FixtureA != null ? FixtureA.Body.Owner : EntityUid.Invalid), (FixtureB != null ? FixtureB.Body.Owner : EntityUid.Invalid)); } } + + [Flags] + internal enum ContactFlags : byte + { + None = 0, + /// + /// Has this contact already been added to an island? + /// + Island = 1 << 0, + + /// + /// Does this contact need re-filtering? + /// + Filter = 1 << 1, + + /// + /// Is this a special contact for grid-grid collisions + /// + Grid = 1 << 2, + } } diff --git a/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs b/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs index 0551f5f38..ee8513b57 100644 --- a/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs +++ b/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs @@ -221,7 +221,7 @@ namespace Robust.Shared.Physics.Dynamics { var contact = contactNode.Value; contactNode = contactNode.Next; - contact.IslandFlag = false; + contact.Flags &= ~ContactFlags.Island; } // Build and simulated islands from awake bodies. @@ -295,7 +295,7 @@ namespace Robust.Shared.Physics.Dynamics node = node.Next; // Has this contact already been added to an island? - if (contact.IslandFlag) continue; + if ((contact.Flags & ContactFlags.Island) != 0x0) continue; // Is this contact solid and touching? if (!contact.Enabled || !contact.IsTouching) continue; @@ -304,7 +304,7 @@ namespace Robust.Shared.Physics.Dynamics if (contact.FixtureA?.Hard != true || contact.FixtureB?.Hard != true) continue; _islandContacts.Add(contact); - contact.IslandFlag = true; + contact.Flags |= ContactFlags.Island; var bodyA = contact.FixtureA!.Body; var bodyB = contact.FixtureB!.Body; diff --git a/Robust.Shared/Physics/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/SharedBroadphaseSystem.cs index 2b3ddea26..62c3a7e05 100644 --- a/Robust.Shared/Physics/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/SharedBroadphaseSystem.cs @@ -10,6 +10,7 @@ using Robust.Shared.Map; using Robust.Shared.Maths; using Robust.Shared.Physics.Broadphase; using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Dynamics.Contacts; using Robust.Shared.Utility; namespace Robust.Shared.Physics @@ -111,14 +112,16 @@ namespace Robust.Shared.Physics /// /// Check the AABB for each moved broadphase fixture and add any colliding entities to the movebuffer in case. /// - private void FindGridContacts(MapId mapId) + private void FindGridContacts( + MapId mapId, + HashSet movedGrids, + EntityQuery bodyQuery, + EntityQuery broadQuery) { - var movedGrids = _mapManager.GetMovedGrids(mapId); - // None moved this tick if (movedGrids.Count == 0) return; - var mapBroadphase = EntityManager.GetComponent(_mapManager.GetMapEntityId(mapId)); + var mapBroadphase = broadQuery.GetComponent(_mapManager.GetMapEntityId(mapId)); // 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. @@ -127,19 +130,20 @@ namespace Robust.Shared.Physics foreach (var grid in movedGrids) { DebugTools.Assert(grid.ParentMapId == mapId); - var worldAABB = grid.WorldBounds; + var worldAABB = grid.WorldAABB; var enlargedAABB = worldAABB.Enlarged(_broadphaseExpand); - var gridBody = EntityManager.GetComponent(grid.GridEntityId); + var gridBody = bodyQuery.GetComponent(grid.GridEntityId); // TODO: Use the callback for this you ape. // Easier to just not go over each proxy as we already unioned the fixture's worldaabb. foreach (var other in mapBroadphase.Tree.QueryAabb(_queryBuffer, enlargedAABB)) { + DebugTools.Assert(other.Fixture.Body != gridBody); // 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 (other.Fixture.Body == gridBody || moveBuffer.ContainsKey(other)) continue; + if (moveBuffer.ContainsKey(other)) continue; // To avoid updating during iteration. // Don't need to transform as it's already in map terms. @@ -153,8 +157,6 @@ namespace Robust.Shared.Physics { moveBuffer[proxy] = worldAABB; } - - movedGrids.Clear(); } /// @@ -163,11 +165,14 @@ namespace Robust.Shared.Physics internal void FindNewContacts(MapId mapId) { var moveBuffer = _moveBuffer[mapId]; + var movedGrids = _mapManager.GetMovedGrids(mapId); - if (moveBuffer.Count == 0) return; + var broadphaseQuery = GetEntityQuery(); + var physicsQuery = GetEntityQuery(); + var xformQuery = GetEntityQuery(); // Find any entities being driven over that might need to be considered - FindGridContacts(mapId); + FindGridContacts(mapId, movedGrids, physicsQuery, broadphaseQuery); // 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 @@ -177,10 +182,12 @@ namespace Robust.Shared.Physics // 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. - var contactManager = EntityManager.GetComponent(_mapManager.GetMapEntityIdOrThrow(mapId)).ContactManager; - var broadphaseQuery = EntityManager.GetEntityQuery(); - var physicsQuery = EntityManager.GetEntityQuery(); - var xformQuery = EntityManager.GetEntityQuery(); + var contactManager = Comp(_mapManager.GetMapEntityIdOrThrow(mapId)).ContactManager; + + // Handle grids first as they're not stored on map broadphase at all. + HandleGridCollisions(mapId, contactManager, movedGrids, physicsQuery, xformQuery); + + DebugTools.Assert(moveBuffer.Count > 0 || _pairBuffer.Count == 0); // TODO: Could store fixtures by broadphase for more perf? foreach (var (proxy, worldAABB) in moveBuffer) @@ -234,12 +241,88 @@ namespace Robust.Shared.Physics } } + movedGrids.Clear(); _pairBuffer.Clear(); moveBuffer.Clear(); _gridMoveBuffer.Clear(); _mapManager.ClearMovedGrids(mapId); } + private void HandleGridCollisions( + MapId mapId, + ContactManager contactManager, + HashSet movedGrids, + EntityQuery bodyQuery, + EntityQuery xformQuery) + { + foreach (var grid in movedGrids) + { + DebugTools.Assert(grid.ParentMapId == mapId); + + var mapGrid = (MapGrid)grid; + var xform = xformQuery.GetComponent(grid.GridEntityId); + + var (worldPos, worldRot, worldMatrix, invWorldMatrix) = xform.GetWorldPositionRotationMatrixWithInv(xformQuery); + + var aabb = new Box2Rotated(grid.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 transform = bodyQuery.GetComponent(grid.GridEntityId).GetTransform(xform); + _gridsPool.Clear(); + + foreach (var colliding in _mapManager.FindGridsIntersecting(mapId, aabb, _gridsPool, xformQuery, bodyQuery, true)) + { + if (grid == colliding) continue; + + var otherGrid = (MapGrid)colliding; + var otherGridBounds = colliding.WorldAABB; + var otherGridInvMatrix = colliding.InvWorldMatrix; + var otherTransform = bodyQuery.GetComponent(colliding.GridEntityId).GetTransform(xformQuery.GetComponent(colliding.GridEntityId)); + + // Get Grid2 AABB in grid1 ref + var aabb1 = grid.LocalAABB.Union(invWorldMatrix.TransformBox(otherGridBounds)); + + // TODO: AddPair has a nasty check in there that's O(n) but that's also a general physics problem. + var ourChunks = mapGrid.GetLocalMapChunks(aabb1); + + // Only care about chunks on other grid overlapping us. + while (ourChunks.MoveNext(out var ourChunk)) + { + var ourChunkWorld = worldMatrix.TransformBox(ourChunk.CachedBounds.Translated(ourChunk.Indices * grid.ChunkSize)); + var ourChunkOtherRef = otherGridInvMatrix.TransformBox(ourChunkWorld); + var collidingChunks = otherGrid.GetLocalMapChunks(ourChunkOtherRef); + + while (collidingChunks.MoveNext(out var collidingChunk)) + { + foreach (var fixture in ourChunk.Fixtures) + { + for (var i = 0; i < fixture.Shape.ChildCount; i++) + { + var fixAABB = fixture.Shape.ComputeAABB(transform, i); + + foreach (var otherFixture in collidingChunk.Fixtures) + { + for (var j = 0; j < otherFixture.Shape.ChildCount; j++) + { + var otherAABB = otherFixture.Shape.ComputeAABB(otherTransform, j); + + if (!fixAABB.Intersects(otherAABB)) continue; + contactManager.AddPair(fixture, i, otherFixture, j, ContactFlags.Grid); + break; + } + } + } + } + } + } + } + } + } + + #endregion + private void FindPairs( FixtureProxy proxy, Box2 worldAABB, @@ -300,14 +383,13 @@ namespace Robust.Shared.Physics _queryBuffer.Clear(); } - #endregion - /// /// If our broadphase has changed then remove us from our old one and add to our new one. /// internal void UpdateBroadphase(PhysicsComponent body, FixturesComponent? manager = null, TransformComponent? xform = null) { - if (!Resolve(body.Owner, ref manager, ref xform)) return; + if (!Resolve(body.Owner, ref manager, ref xform) || + _mapManager.IsGrid(body.Owner)) return; var oldBroadphase = body.Broadphase; var newBroadphase = GetBroadphase(xform); @@ -384,8 +466,9 @@ namespace Robust.Shared.Physics // TODO: Good idea? Ehhhhhhhhhhhh // The problem is there's some fuckery with events while an entity is initializing. // Can probably just bypass this by doing stuff in Update / FrameUpdate again but future problem - // - if (body.Broadphase != null) return; + // Also grids are special-cased due to their high fixture count. + if (body.Broadphase != null || + _mapManager.IsGrid(body.Owner)) return; if (!Resolve(body.Owner, ref manager)) { @@ -451,7 +534,7 @@ namespace Robust.Shared.Physics foreach (var (_, contact) in fixture.Contacts) { - contact.FilterFlag = true; + contact.Flags |= ContactFlags.Filter; } var broadphase = body.Broadphase; @@ -474,7 +557,9 @@ namespace Robust.Shared.Physics private void OnMove(EntityUid uid, PhysicsComponent component, ref MoveEvent args) { - if (!component.CanCollide || !EntityManager.TryGetComponent(uid, out FixturesComponent? manager)) return; + if (!component.CanCollide || + !TryComp(uid, out var manager) || + _mapManager.IsGrid(uid)) return; var worldRot = Transform(uid).WorldRotation; @@ -483,9 +568,10 @@ namespace Robust.Shared.Physics private void OnRotate(EntityUid uid, PhysicsComponent component, ref RotateEvent args) { - if (!component.CanCollide) return; + if (!component.CanCollide || + _mapManager.IsGrid(uid)) return; - var xform = EntityManager.GetComponent(uid); + var xform = Transform(uid); var (worldPos, worldRot) = xform.GetWorldPositionRotation(); DebugTools.Assert(xform.LocalRotation.Equals(args.NewRotation)); @@ -790,7 +876,7 @@ namespace Robust.Shared.Physics var grid = (IMapGridInternal) _mapManager.GetGrid(mapGrid.GridIndex); // Won't worry about accurate bounds checks as it's probably slower in most use cases. - grid.GetMapChunks(aabb, out var chunkEnumerator); + var chunkEnumerator = grid.GetMapChunks(aabb); if (chunkEnumerator.MoveNext(out _)) { diff --git a/Robust.Shared/Physics/SharedJointSystem.cs b/Robust.Shared/Physics/SharedJointSystem.cs index d9da2d2fb..be6c568e9 100644 --- a/Robust.Shared/Physics/SharedJointSystem.cs +++ b/Robust.Shared/Physics/SharedJointSystem.cs @@ -9,6 +9,7 @@ using Robust.Shared.IoC; using Robust.Shared.Log; using Robust.Shared.Map; using Robust.Shared.Maths; +using Robust.Shared.Physics.Dynamics.Contacts; using Robust.Shared.Physics.Dynamics.Joints; using Robust.Shared.Timing; using Robust.Shared.Utility; @@ -470,7 +471,7 @@ namespace Robust.Shared.Physics { // Flag the contact for filtering at the next time step (where either // body is awake). - contact.FilterFlag = true; + contact.Flags |= ContactFlags.Filter; } } } diff --git a/Robust.UnitTesting/Shared/Map/GridCollision_Test.cs b/Robust.UnitTesting/Shared/Map/GridCollision_Test.cs index 77a452209..018ad128b 100644 --- a/Robust.UnitTesting/Shared/Map/GridCollision_Test.cs +++ b/Robust.UnitTesting/Shared/Map/GridCollision_Test.cs @@ -35,8 +35,8 @@ namespace Robust.UnitTesting.Shared.Map gridId2 = mapManager.CreateGrid(mapId); gridEnt1 = gridId1.GridEntityId; gridEnt2 = gridId2.GridEntityId; - physics1 = IoCManager.Resolve().GetComponent(gridEnt1.Value); - physics2 = IoCManager.Resolve().GetComponent(gridEnt2.Value); + physics1 = entManager.GetComponent(gridEnt1.Value); + physics2 = entManager.GetComponent(gridEnt2.Value); // Can't collide static bodies and grids (at time of this writing) start as static // (given most other games would probably prefer them as static) hence we need to make them dynamic. physics1.BodyType = BodyType.Dynamic; diff --git a/Robust.UnitTesting/Shared/Map/MapGrid_Tests.cs b/Robust.UnitTesting/Shared/Map/MapGrid_Tests.cs index eb7a67638..2ade64b85 100644 --- a/Robust.UnitTesting/Shared/Map/MapGrid_Tests.cs +++ b/Robust.UnitTesting/Shared/Map/MapGrid_Tests.cs @@ -56,7 +56,7 @@ namespace Robust.UnitTesting.Shared.Map grid.SetTile(new Vector2i(-1, -2), new Tile(1)); grid.SetTile(new Vector2i(1, 2), new Tile(1)); - var bounds = grid.WorldBounds; + var bounds = grid.WorldAABB; // this is world, so add the grid world pos Assert.That(bounds.Bottom, Is.EqualTo(-2+5)); @@ -82,7 +82,7 @@ namespace Robust.UnitTesting.Shared.Map grid.SetTile(new Vector2i(1, 2), Tile.Empty); - var bounds = grid.WorldBounds; + var bounds = grid.WorldAABB; // this is world, so add the grid world pos Assert.That(bounds.Bottom, Is.EqualTo(-2+5));