diff --git a/RELEASE-NOTES.md b/RELEASE-NOTES.md index 59651158dce..a44ab1a4e60 100644 --- a/RELEASE-NOTES.md +++ b/RELEASE-NOTES.md @@ -50,6 +50,7 @@ END TEMPLATE--> ### Other * Add pure to some SharedTransformSystem methods. +* Significantly optimised collision detection in SharedBroadphaseSystem. ### Internal diff --git a/Robust.Client/Physics/PhysicsSystem.Predict.cs b/Robust.Client/Physics/PhysicsSystem.Predict.cs index 90552c7c18b..ec9cdf732ae 100644 --- a/Robust.Client/Physics/PhysicsSystem.Predict.cs +++ b/Robust.Client/Physics/PhysicsSystem.Predict.cs @@ -141,7 +141,8 @@ internal void UpdateIsTouching(List toUpdate) if ((contact.Flags & ContactFlags.Filter) != 0x0) { if (!ShouldCollide(fixtureA, fixtureB) || - !ShouldCollide(uidA, uidB, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB)) + !ShouldCollideSlow(uidA, uidB, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB) || + !ShouldCollideJoints(uidA, uidB)) { contact.IsTouching = false; continue; diff --git a/Robust.Shared/CVars.cs b/Robust.Shared/CVars.cs index 1996c21ca09..5c87df81cb7 100644 --- a/Robust.Shared/CVars.cs +++ b/Robust.Shared/CVars.cs @@ -1280,13 +1280,6 @@ protected CVars() * PHYSICS */ - /// - /// How much to expand broadphase checking for. This is useful for cross-grid collisions. - /// Performance impact if additional broadphases are being checked. - /// - public static readonly CVarDef BroadphaseExpand = - CVarDef.Create("physics.broadphase_expand", 2f, CVar.ARCHIVE | CVar.REPLICATED); - /// /// The target minimum ticks per second on the server. /// This is used for substepping and will help with clipping/physics issues and such. diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index cce6cd4f322..dbd1eff5677 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -1,9 +1,6 @@ using System; using System.Collections.Generic; using System.Numerics; -using System.Threading.Tasks; -using Microsoft.Extensions.ObjectPool; -using Robust.Shared.Collections; using Robust.Shared.Configuration; using Robust.Shared.GameObjects; using Robust.Shared.IoC; @@ -35,10 +32,9 @@ public abstract class SharedBroadphaseSystem : EntitySystem private EntityQuery _physicsQuery; private EntityQuery _xformQuery; - private float _broadphaseExpand; + private readonly HashSet _gridMoveBuffer = new(); - private readonly Dictionary _broadMatrices = new(); - private HashSet _gridMoveBuffer = new(); + private float _frameTime; /* * Okay so Box2D has its own "MoveProxy" stuff so you can easily find new contacts when required. @@ -55,9 +51,9 @@ public override void Initialize() _contactJob = new() { - _mapManager = _mapManager, + MapManager = _mapManager, System = this, - BroadphaseExpand = _broadphaseExpand, + TransformSys = EntityManager.System(), // TODO: EntityManager one isn't ready yet? XformQuery = GetEntityQuery(), }; @@ -71,13 +67,13 @@ public override void Initialize() UpdatesOutsidePrediction = true; UpdatesAfter.Add(typeof(SharedTransformSystem)); - Subs.CVar(_cfg, CVars.BroadphaseExpand, SetBroadphaseExpand, true); - } - - private void SetBroadphaseExpand(float value) - { - _contactJob.BroadphaseExpand = value; - _broadphaseExpand = value; + Subs.CVar(_cfg, + CVars.TargetMinimumTickrate, + val => + { + _frameTime = 1f / val; + }, + true); } public void Rebuild(BroadphaseComponent component, bool fullBuild) @@ -109,6 +105,7 @@ private void FindGridContacts(HashSet movedGrids) // 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(); foreach (var gridUid in movedGrids) { @@ -120,7 +117,7 @@ private void FindGridContacts(HashSet movedGrids) continue; var worldAABB = _transform.GetWorldMatrix(xform).TransformBox(grid.LocalAABB); - var enlargedAABB = worldAABB.Enlarged(_broadphaseExpand); + var enlargedAABB = worldAABB.Enlarged(GetBroadphaseExpand(_physicsQuery.GetComponent(gridUid), _frameTime)); var state = (moveBuffer, _gridMoveBuffer); QueryMapBroadphase(mapBroadphase.DynamicTree, ref state, enlargedAABB); @@ -135,6 +132,11 @@ private void FindGridContacts(HashSet movedGrids) } } + private float GetBroadphaseExpand(PhysicsComponent body, float frameTime) + { + return body.LinearVelocity.Length() * 1.2f * frameTime; + } + private void QueryMapBroadphase(IBroadPhase broadPhase, ref (HashSet, HashSet) state, Box2 enlargedAABB) @@ -163,11 +165,12 @@ private void QueryMapBroadphase(IBroadPhase broadPhase, /// internal void FindNewContacts() { + _contactJob.FrameTime = _frameTime; + _contactJob.Pairs.Clear(); + var moveBuffer = _physicsSystem.MoveBuffer; var movedGrids = _physicsSystem.MovedGrids; - _gridMoveBuffer.Clear(); - // Find any entities being driven over that might need to be considered FindGridContacts(movedGrids); @@ -195,55 +198,32 @@ internal void FindNewContacts() _contactJob.MoveBuffer.Add(proxy); } - _broadMatrices.Clear(); - var broadQuery = AllEntityQuery(); - - // Cache broadphase matrices up front. - // We'll defer the proxy world AABBs until we get contacts rather than doing it on every single move. - // This is because contacts are run in parallel so we can spread the work a bit more and also don't duplicate it per tick. - while (broadQuery.MoveNext(out var bUid, out _)) - { - _broadMatrices[bUid] = _transform.GetWorldMatrix(bUid); - } - - for (var i = _contactJob.ContactBuffer.Count; i < _contactJob.MoveBuffer.Count; i++) - { - _contactJob.ContactBuffer.Add(new List()); - } - var count = moveBuffer.Count; _parallel.ProcessNow(_contactJob, count); - for (var i = 0; i < count; i++) + foreach (var (proxyA, proxyB, flags) in _contactJob.Pairs) { - var proxies = _contactJob.ContactBuffer[i]; - - if (proxies.Count == 0) - continue; - - var proxyA = _contactJob.MoveBuffer[i]; - var proxyABody = proxyA.Body; - - _fixturesQuery.TryGetComponent(proxyA.Entity, out var manager); - - foreach (var other in proxies) + 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) { - var otherBody = other.Body; - - // 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 (proxyA.Fixture.Hard && other.Fixture.Hard && - (_gridMoveBuffer.Contains(proxyA) || _gridMoveBuffer.Contains(other))) - { - _physicsSystem.WakeBody(proxyA.Entity, force: true, manager: manager, body: proxyABody); - _physicsSystem.WakeBody(other.Entity, force: true, body: otherBody); - } + _physicsSystem.WakeBody(proxyA.Entity, force: true, body: proxyA.Body); + _physicsSystem.WakeBody(proxyB.Entity, force: true, body: otherBody); + } - _physicsSystem.AddPair(proxyA.FixtureId, other.FixtureId, proxyA, other); + // 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; } + + _physicsSystem.AddPair(proxyA.FixtureId, proxyB.FixtureId, proxyA, proxyB, flags: contactFlags); } moveBuffer.Clear(); @@ -252,6 +232,8 @@ internal void FindNewContacts() 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 gridUid in movedGrids) { var grid = _gridQuery.GetComponent(gridUid); @@ -301,6 +283,12 @@ private void HandleGridCollisions(HashSet movedGrids) return true; } + // If the other entity is lower ID and also moved then let that handle the collision. + if (tuple.grid.Owner.Id > uid.Id && tuple._physicsSystem.MovedGrids.Contains(uid)) + { + return true; + } + var (_, _, otherGridMatrix, otherGridInvMatrix) = tuple.xformSystem.GetWorldPositionRotationMatrixWithInv(collidingXform); var otherGridBounds = otherGridMatrix.TransformBox(component.LocalAABB); var otherTransform = tuple._physicsSystem.GetPhysicsTransform(uid); @@ -337,6 +325,10 @@ private void HandleGridCollisions(HashSet movedGrids) { var otherFixture = fixturesB.Fixtures[otherId]; + // There's already a contact so ignore it. + if (fixture.Contacts.ContainsKey(otherFixture)) + break; + for (var j = 0; j < otherFixture.Shape.ChildCount; j++) { var otherAABB = otherFixture.Shape.ComputeAABB(otherTransform, j); @@ -370,7 +362,7 @@ private void FindPairs( FixtureProxy proxy, Box2 worldAABB, EntityUid broadphase, - List pairBuffer) + List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) { DebugTools.Assert(proxy.Body.CanCollide); @@ -401,7 +393,7 @@ private void FindPairs( } var broadphaseComp = _broadphaseQuery.GetComponent(broadphase); - var state = (pairBuffer, proxy); + var state = (pairBuffer, _physicsSystem.MoveBuffer, this, _physicsSystem, proxy); QueryBroadphase(broadphaseComp.DynamicTree, state, aabb); @@ -411,23 +403,57 @@ private void FindPairs( QueryBroadphase(broadphaseComp.StaticTree, state, aabb); } - private void QueryBroadphase(IBroadPhase broadPhase, (List, 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 pairBuffer, FixtureProxy proxy) tuple, + 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 == other || - !SharedPhysicsSystem.ShouldCollide(tuple.proxy.Fixture, other.Fixture) || - tuple.proxy.Entity == other.Entity) + if (tuple.proxy.Entity == other.Entity || + !SharedPhysicsSystem.ShouldCollide(tuple.proxy.Fixture, other.Fixture)) { return true; } - tuple.pairBuffer.Add(other); + // 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; + } + + // Check if contact already exists. + if (tuple.proxy.Fixture.Contacts.ContainsKey(other.Fixture)) + 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: Sensors handled elsewhere when we do v3 port. + //if (!tuple.proxy.Fixture.Hard || !other.Fixture.Hard) + // 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; + } + + lock (tuple.pairs) + { + tuple.pairs.Add((tuple.proxy, other, flags)); + } + return true; }, aabb, true); } @@ -560,39 +586,42 @@ private record struct BroadphaseContactJob() : IParallelRobustJob { public SharedBroadphaseSystem System = default!; public SharedTransformSystem TransformSys = default!; - public IMapManager _mapManager = default!; - - public float BroadphaseExpand; + public IMapManager MapManager = default!; public EntityQuery XformQuery; - public List> ContactBuffer = new(); - public List MoveBuffer = new(); + public readonly List MoveBuffer = new(); + + public List<(FixtureProxy, FixtureProxy, PairFlag)> Pairs = new(64); - public int BatchSize => 8; + 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; public void Execute(int index) { var proxy = MoveBuffer[index]; var broadphaseUid = XformQuery.GetComponent(proxy.Entity).Broadphase?.Uid; - var worldAABB = System._broadMatrices[broadphaseUid!.Value].TransformBox(proxy.AABB); - var buffer = ContactBuffer[index]; - buffer.Clear(); + var worldAABB = TransformSys.GetWorldMatrix(broadphaseUid!.Value).TransformBox(proxy.AABB); var mapUid = XformQuery.GetComponent(proxy.Entity).MapUid ?? EntityUid.Invalid; + var broadphaseExpand = System.GetBroadphaseExpand(proxy.Body, FrameTime); + var proxyBody = proxy.Body; DebugTools.Assert(!proxyBody.Deleted); - var state = (System, proxy, worldAABB, buffer); + var state = (System, proxy, worldAABB, Pairs); // Get every broadphase we may be intersecting. - _mapManager.FindGridsIntersecting(mapUid, worldAABB.Enlarged(BroadphaseExpand), ref state, + MapManager.FindGridsIntersecting(mapUid, worldAABB.Enlarged(broadphaseExpand), ref state, static (EntityUid uid, MapGridComponent _, ref ( SharedBroadphaseSystem system, FixtureProxy proxy, Box2 worldAABB, - List pairBuffer) tuple) => + List<(FixtureProxy, FixtureProxy, PairFlag)> pairBuffer) tuple) => { ref var buffer = ref tuple.pairBuffer; tuple.system.FindPairs(tuple.proxy, tuple.worldAABB, uid, buffer); @@ -602,9 +631,24 @@ public void Execute(int index) includeMap: false); // Struct ref moment, I have no idea what's fastest. - buffer = state.buffer; - System.FindPairs(proxy, worldAABB, mapUid, buffer); + System.FindPairs(proxy, worldAABB, mapUid, Pairs); } } + + [Flags] + private enum PairFlag : byte + { + None = 0, + + /// + /// Should we wake the contacting entities. + /// + Wake = 1 << 0, + + /// + /// Is it a grid collision. + /// + Grid = 1 << 1, + } } } diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs index 29646dc2fad..e25a5c2fbbc 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs @@ -263,16 +263,13 @@ internal void AddPair( // Broadphase has already done the faster check for collision mask / layers // so no point duplicating - // Does a contact already exist? - if (fixtureA.Contacts.ContainsKey(fixtureB)) - return; - + DebugTools.Assert(!fixtureA.Contacts.ContainsKey(fixtureB)); DebugTools.Assert(!fixtureB.Contacts.ContainsKey(fixtureA)); var xformA = entA.Comp2; var xformB = entB.Comp2; // Does a joint override collision? Is at least one body dynamic? - if (!ShouldCollide(entA.Owner, entB.Owner, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB)) + if (!ShouldCollideSlow(entA.Owner, entB.Owner, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB)) return; // Call the factory. @@ -310,14 +307,14 @@ internal void AddPair( /// /// Go through the cached broadphase movement and update contacts. /// - internal void AddPair(string fixtureAId, string fixtureBId, in FixtureProxy proxyA, in FixtureProxy proxyB) + internal void AddPair(string fixtureAId, string fixtureBId, in FixtureProxy proxyA, in FixtureProxy proxyB, ContactFlags flags = ContactFlags.None) { AddPair((proxyA.Entity, proxyA.Body, proxyA.Xform), (proxyB.Entity, proxyB.Body, proxyB.Xform), fixtureAId, fixtureBId, proxyA.Fixture, proxyA.ChildIndex, proxyB.Fixture, proxyB.ChildIndex, - proxyA.Body, proxyB.Body); + proxyA.Body, proxyB.Body, flags: flags); } internal static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) @@ -447,7 +444,8 @@ internal void CollideContacts() { // Check default filtering if (!ShouldCollide(fixtureA, fixtureB) || - !ShouldCollide(uidA, uidB, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB)) + !ShouldCollideSlow(uidA, uidB, bodyA, bodyB, fixtureA, fixtureB, xformA, xformB) || + !ShouldCollideJoints(uidA, uidB)) { DestroyContact(contact); continue; @@ -720,10 +718,31 @@ private void UpdateContact(Contact[] contacts, int index, ContactStatus[] status } } + /// + /// Is there a joint blocking collision between these bodies. + /// + internal bool ShouldCollideJoints(Entity entA, Entity entB) + { + // Does a joint prevent collision? + // if one of them doesn't have jointcomp then they can't share a common joint. + // otherwise, only need to iterate over the joints of one component as they both store the same joint. + if (JointQuery.Resolve(entA.Owner, ref entA.Comp, false) && JointQuery.HasComp(entB)) + { + foreach (var joint in entA.Comp.Joints.Values) + { + // Check if either: the joint even allows collisions OR the other body on the joint is actually the other body we're checking. + if (!joint.CollideConnected && (entB.Owner == joint.BodyAUid || entB.Owner == joint.BodyBUid)) + return false; + } + } + + return true; + } + /// /// Used to prevent bodies from colliding; may lie depending on joints. /// - protected bool ShouldCollide( + internal bool ShouldCollideSlow( EntityUid uid, EntityUid other, PhysicsComponent body, @@ -757,18 +776,7 @@ protected bool ShouldCollide( return false; } - // Does a joint prevent collision? - // if one of them doesn't have jointcomp then they can't share a common joint. - // otherwise, only need to iterate over the joints of one component as they both store the same joint. - if (TryComp(uid, out JointComponent? jointComponentA) && HasComp(other)) - { - foreach (var joint in jointComponentA.Joints.Values) - { - // Check if either: the joint even allows collisions OR the other body on the joint is actually the other body we're checking. - if (!joint.CollideConnected && (other == joint.BodyAUid || other == joint.BodyBUid)) - return false; - } - } + // Joints already handled before the contact pair is made. var preventCollideMessage = new PreventCollideEvent(uid, other, body, otherBody, fixture, otherFixture); RaiseLocalEvent(uid, ref preventCollideMessage);