From 4a50bc2154a55bc0ff88890da5a6943611d9a952 Mon Sep 17 00:00:00 2001 From: DrSmugleaf Date: Thu, 16 Nov 2023 01:44:21 -0800 Subject: [PATCH] Add AddEntitiesIntersecting for phys shapes, change float range overload to use circles, remove obsolete methods (#4572) --- .../EntityLookupSystem.ComponentQueries.cs | 426 ++++++++++++++---- .../GameObjects/Systems/EntityLookupSystem.cs | 5 + 2 files changed, 352 insertions(+), 79 deletions(-) diff --git a/Robust.Shared/GameObjects/Systems/EntityLookupSystem.ComponentQueries.cs b/Robust.Shared/GameObjects/Systems/EntityLookupSystem.ComponentQueries.cs index 6a028f134..ecf4930e9 100644 --- a/Robust.Shared/GameObjects/Systems/EntityLookupSystem.ComponentQueries.cs +++ b/Robust.Shared/GameObjects/Systems/EntityLookupSystem.ComponentQueries.cs @@ -6,7 +6,11 @@ using Robust.Shared.Collections; using Robust.Shared.Map; using Robust.Shared.Map.Components; using Robust.Shared.Maths; +using Robust.Shared.Physics; +using Robust.Shared.Physics.Collision; +using Robust.Shared.Physics.Collision.Shapes; using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Systems; using Robust.Shared.Utility; namespace Robust.Shared.GameObjects; @@ -15,18 +19,6 @@ public sealed partial class EntityLookupSystem { #region Private - private void AddComponentsIntersecting( - EntityUid lookupUid, - HashSet intersecting, - Box2 worldAABB, - LookupFlags flags, - EntityQuery query) where T : IComponent - { - var intersectingEntities = new HashSet>(); - AddEntitiesIntersecting(lookupUid, intersectingEntities, worldAABB, flags, query); - intersecting.UnionWith(intersectingEntities.Select(e => e.Comp)); - } - private void AddEntitiesIntersecting( EntityUid lookupUid, HashSet> intersecting, @@ -88,6 +80,153 @@ public sealed partial class EntityLookupSystem } } + private void AddEntitiesIntersecting( + EntityUid lookupUid, + HashSet> intersecting, + IPhysShape shape, + Box2 worldAABB, + LookupFlags flags, + EntityQuery query) where T : IComponent + { + var lookup = _broadQuery.GetComponent(lookupUid); + var invMatrix = _transform.GetInvWorldMatrix(lookupUid); + var localAABB = invMatrix.TransformBox(worldAABB); + var transform = new Transform(0); + var state = new QueryState( + intersecting, + shape, + transform, + _fixtures, + _physics, + _transform, + _manifoldManager, + query, + _fixturesQuery, + (flags & LookupFlags.Sensors) != 0 + ); + + if ((flags & LookupFlags.Dynamic) != 0x0) + { + lookup.DynamicTree.QueryAabb(ref state, static (ref QueryState state, in FixtureProxy value) => + { + if (!state.Sensors && !value.Fixture.Hard) + return true; + + if (!state.Query.TryGetComponent(value.Entity, out var comp)) + return true; + + var intersectingTransform = state.Physics.GetPhysicsTransform(value.Entity); + if (!state.Manifolds.TestOverlap(state.Shape, 0, value.Fixture.Shape, value.ChildIndex, state.Transform, intersectingTransform)) + { + return true; + } + + state.Intersecting.Add((value.Entity, comp)); + return true; + }, localAABB, (flags & LookupFlags.Approximate) != 0x0); + } + + if ((flags & (LookupFlags.Static)) != 0x0) + { + lookup.StaticTree.QueryAabb(ref state, static (ref QueryState state, in FixtureProxy value) => + { + if (!state.Sensors && !value.Fixture.Hard) + return true; + + if (!state.Query.TryGetComponent(value.Entity, out var comp)) + return true; + + var intersectingTransform = state.Physics.GetPhysicsTransform(value.Entity); + if (!state.Manifolds.TestOverlap(state.Shape, 0, value.Fixture.Shape, value.ChildIndex, state.Transform, intersectingTransform)) + { + return true; + } + + state.Intersecting.Add((value.Entity, comp)); + return true; + }, localAABB, (flags & LookupFlags.Approximate) != 0x0); + } + + if ((flags & LookupFlags.StaticSundries) == LookupFlags.StaticSundries) + { + lookup.StaticSundriesTree.QueryAabb(ref state, static (ref QueryState state, in EntityUid value) => + { + if (!state.Query.TryGetComponent(value, out var comp)) + return true; + + if (!state.FixturesQuery.TryGetComponent(value, out var fixtures)) + { + var position = state.TransformSystem.GetWorldPosition(value); + if (state.Fixtures.TestPoint(state.Shape, state.Transform, position)) + goto found; + + return true; + } + + var intersectingTransform = state.Physics.GetPhysicsTransform(value); + foreach (var fixture in fixtures.Fixtures.Values) + { + if (!state.Sensors && !fixture.Hard) + continue; + + for (var i = 0; i < fixture.Shape.ChildCount; i++) + { + if (state.Manifolds.TestOverlap(state.Shape, 0, fixture.Shape, i, state.Transform, intersectingTransform)) + { + goto found; + } + } + } + + return true; + + found: + state.Intersecting.Add((value, comp)); + return true; + }, localAABB, (flags & LookupFlags.Approximate) != 0x0); + } + + if ((flags & LookupFlags.Sundries) != 0x0) + { + lookup.SundriesTree.QueryAabb(ref state, static (ref QueryState state, + in EntityUid value) => + { + if (!state.Query.TryGetComponent(value, out var comp)) + return true; + + if (!state.FixturesQuery.TryGetComponent(value, out var fixtures)) + { + var position = state.TransformSystem.GetWorldPosition(value); + if (state.Fixtures.TestPoint(state.Shape, state.Transform, position)) + goto found; + + return true; + } + + var intersectingTransform = state.Physics.GetPhysicsTransform(value); + foreach (var fixture in fixtures.Fixtures.Values) + { + if (!state.Sensors && !fixture.Hard) + continue; + + for (var i = 0; i < fixture.Shape.ChildCount; i++) + { + if (!state.Manifolds.TestOverlap(state.Shape, 0, fixture.Shape, i, state.Transform, intersectingTransform)) + { + goto found; + } + } + } + + return true; + + found: + state.Intersecting.Add((value, comp)); + return true; + }, localAABB, (flags & LookupFlags.Approximate) != 0x0); + } + } + private bool AnyComponentsIntersecting( EntityUid lookupUid, Box2 worldAABB, @@ -308,15 +447,6 @@ public sealed partial class EntityLookupSystem return false; } - [Obsolete] - public HashSet GetComponentsIntersecting(Type type, MapId mapId, Box2 worldAABB, LookupFlags flags = DefaultFlags) - { - var intersectingEntities = new HashSet>(); - GetEntitiesIntersecting(type, mapId, worldAABB, intersectingEntities, flags); - var intersecting = new HashSet(intersectingEntities.Select(e => e.Comp)); - return intersecting; - } - public void GetEntitiesIntersecting(Type type, MapId mapId, Box2 worldAABB, HashSet> intersecting, LookupFlags flags = DefaultFlags) { DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type)); @@ -366,14 +496,6 @@ public sealed partial class EntityLookupSystem } } - [Obsolete] - public HashSet GetComponentsIntersecting(MapId mapId, Box2 worldAABB, LookupFlags flags = DefaultFlags) where T : IComponent - { - var intersectingEntities = new HashSet>(); - GetEntitiesIntersecting(mapId, worldAABB, intersectingEntities, flags); - return new HashSet(intersectingEntities.Select(e => e.Comp)); - } - public void GetEntitiesIntersecting(MapId mapId, Box2 worldAABB, HashSet> entities, LookupFlags flags = DefaultFlags) where T : IComponent { if (mapId == MapId.Nullspace) return; @@ -416,14 +538,168 @@ public sealed partial class EntityLookupSystem #endregion - #region EntityCoordinates + #region IPhysShape - public HashSet GetComponentsInRange(EntityCoordinates coordinates, float range) where T : IComponent + public void GetEntitiesIntersecting(Type type, MapId mapId, IPhysShape shape, HashSet> intersecting, LookupFlags flags = DefaultFlags) { - var mapPos = coordinates.ToMap(EntityManager, _transform); - return GetComponentsInRange(mapPos, range); + DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type)); + if (mapId == MapId.Nullspace) + return; + + var shapeTransform = new Transform(0); + var worldAABB = shape.ComputeAABB(shapeTransform, 0); + var sensors = (flags & LookupFlags.Sensors) != 0; + if (!UseBoundsQuery(type, worldAABB.Height * worldAABB.Width)) + { + foreach (var (uid, comp) in EntityManager.GetAllComponents(type, true)) + { + var xform = _xformQuery.GetComponent(uid); + var xFormPosition = _transform.GetWorldPosition(xform); + + if (xform.MapID != mapId || + !worldAABB.Contains(xFormPosition) || + ((flags & LookupFlags.Contained) == 0x0 && + _container.IsEntityOrParentInContainer(uid, _metaQuery.GetComponent(uid), xform))) + { + continue; + } + + if (_fixturesQuery.TryGetComponent(uid, out var fixtures)) + { + var xFormRotation = _transform.GetWorldRotation(xform, _xformQuery); + var transform = new Transform(xFormPosition, xFormRotation); + + foreach (var fixture in fixtures.Fixtures.Values) + { + if (!sensors && !fixture.Hard) + continue; + + for (var i = 0; i < fixture.Shape.ChildCount; i++) + { + if (_manifoldManager.TestOverlap(shape, 0, fixture.Shape, i, shapeTransform, transform)) + { + goto found; + } + } + } + + continue; + } + else + { + if (!_fixtures.TestPoint(shape, shapeTransform, xFormPosition)) + continue; + } + + found: + + intersecting.Add((uid, comp)); + } + } + else + { + var query = EntityManager.GetEntityQuery(type); + + // Get grid entities + var state = new GridQueryState(intersecting, shape, worldAABB, this, flags, query); + + _mapManager.FindGridsIntersecting(mapId, worldAABB, ref state, + static (EntityUid uid, MapGridComponent grid, ref GridQueryState state) => + { + state.Lookup.AddEntitiesIntersecting(uid, state.Intersecting, state.Shape, state.WorldAABB, state.Flags, state.Query); + return true; + }, (flags & LookupFlags.Approximate) != 0x0); + + // Get map entities + var mapUid = _mapManager.GetMapEntityId(mapId); + AddEntitiesIntersecting(mapUid, intersecting, shape, worldAABB, flags, query); + AddContained(intersecting, flags, query); + } } + public void GetEntitiesIntersecting(MapId mapId, IPhysShape shape, HashSet> entities, LookupFlags flags = DefaultFlags) where T : IComponent + { + if (mapId == MapId.Nullspace) return; + + var shapeTransform = new Transform(0); + var worldAABB = shape.ComputeAABB(shapeTransform, 0); + var sensors = (flags & LookupFlags.Sensors) != 0; + if (!UseBoundsQuery(worldAABB.Height * worldAABB.Width)) + { + var query = AllEntityQuery(); + + while (query.MoveNext(out var uid, out var comp, out var xform)) + { + var xFormPosition = _transform.GetWorldPosition(xform); + + if (xform.MapID != mapId || + !worldAABB.Contains(xFormPosition)) + { + continue; + } + + if (_fixturesQuery.TryGetComponent(uid, out var fixtures)) + { + var xFormRotation = _transform.GetWorldRotation(xform, _xformQuery); + var transform = new Transform(xFormPosition, xFormRotation); + + foreach (var fixture in fixtures.Fixtures.Values) + { + if (!sensors && !fixture.Hard) + continue; + + for (var i = 0; i < fixture.Shape.ChildCount; i++) + { + if (_manifoldManager.TestOverlap(shape, 0, fixture.Shape, i, shapeTransform, transform)) + { + goto found; + } + } + } + + continue; + } + else + { + if (!_fixtures.TestPoint(shape, shapeTransform, xFormPosition)) + continue; + } + + found: + entities.Add((uid, comp)); + } + } + else + { + var query = GetEntityQuery(); + + // Get grid entities + var state = (this, shape, worldAABB, flags, query, entities); + + _mapManager.FindGridsIntersecting(mapId, worldAABB, ref state, + static (EntityUid uid, MapGridComponent grid, + ref (EntityLookupSystem system, + IPhysShape shape, + Box2 worldAABB, + LookupFlags flags, + EntityQuery query, + HashSet> intersecting) tuple) => + { + tuple.system.AddEntitiesIntersecting(uid, tuple.intersecting, tuple.shape, tuple.worldAABB, tuple.flags, tuple.query); + return true; + }, (flags & LookupFlags.Approximate) != 0x0); + + // Get map entities + var mapUid = _mapManager.GetMapEntityId(mapId); + AddEntitiesIntersecting(mapUid, entities, shape, worldAABB, flags, query); + AddContained(entities, flags, query); + } + } + + #endregion + + #region EntityCoordinates + public void GetEntitiesInRange(EntityCoordinates coordinates, float range, HashSet> entities) where T : IComponent { var mapPos = coordinates.ToMap(EntityManager, _transform); @@ -441,13 +717,6 @@ public sealed partial class EntityLookupSystem #region MapCoordinates - [Obsolete] - public HashSet GetComponentsInRange(Type type, MapCoordinates coordinates, float range) - { - DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type)); - return GetComponentsInRange(type, coordinates.MapId, coordinates.Position, range); - } - public HashSet> GetEntitiesInRange(Type type, MapCoordinates coordinates, float range) { var entities = new HashSet>(); @@ -461,20 +730,21 @@ public sealed partial class EntityLookupSystem GetEntitiesInRange(type, coordinates.MapId, coordinates.Position, range, entities); } + [Obsolete] public HashSet GetComponentsInRange(MapCoordinates coordinates, float range) where T : IComponent { return GetComponentsInRange(coordinates.MapId, coordinates.Position, range); } - public void GetEntitiesInRange(MapCoordinates coordinates, float range, HashSet> entities) where T : IComponent + public void GetEntitiesInRange(MapCoordinates coordinates, float range, HashSet> entities, LookupFlags flags = DefaultFlags) where T : IComponent { - GetEntitiesInRange(coordinates.MapId, coordinates.Position, range, entities); + GetEntitiesInRange(coordinates.MapId, coordinates.Position, range, entities, flags); } - public HashSet> GetEntitiesInRange(MapCoordinates coordinates, float range) where T : IComponent + public HashSet> GetEntitiesInRange(MapCoordinates coordinates, float range, LookupFlags flags = DefaultFlags) where T : IComponent { var entities = new HashSet>(); - GetEntitiesInRange(coordinates.MapId, coordinates.Position, range, entities); + GetEntitiesInRange(coordinates.MapId, coordinates.Position, range, entities, flags); return entities; } @@ -482,40 +752,15 @@ public sealed partial class EntityLookupSystem #region MapId - public bool AnyComponentsInRange(Type type, MapId mapId, Vector2 worldPos, float range) - { - DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type)); - DebugTools.Assert(range > 0, "Range must be a positive float"); - - if (mapId == MapId.Nullspace) return false; - - // TODO: Actual circles - var rangeVec = new Vector2(range, range); - - var worldAABB = new Box2(worldPos - rangeVec, worldPos + rangeVec); - return AnyComponentsIntersecting(type, mapId, worldAABB); - } - - [Obsolete] - public HashSet GetComponentsInRange(Type type, MapId mapId, Vector2 worldPos, float range) - { - var entities = new HashSet>(); - GetEntitiesInRange(type, mapId, worldPos, range, entities); - return new HashSet(entities.Select(e => e.Comp)); - } - - public void GetEntitiesInRange(Type type, MapId mapId, Vector2 worldPos, float range, HashSet> entities) + public void GetEntitiesInRange(Type type, MapId mapId, Vector2 worldPos, float range, HashSet> entities, LookupFlags flags = DefaultFlags) { DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type)); DebugTools.Assert(range > 0, "Range must be a positive float"); if (mapId == MapId.Nullspace) return; - // TODO: Actual circles - var rangeVec = new Vector2(range, range); - - var worldAABB = new Box2(worldPos - rangeVec, worldPos + rangeVec); - GetEntitiesIntersecting(type, mapId, worldAABB, entities); + var circle = new PhysShapeCircle(range, worldPos); + GetEntitiesIntersecting(type, mapId, circle, entities, flags); } [Obsolete] @@ -526,18 +771,41 @@ public sealed partial class EntityLookupSystem return new HashSet(entities.Select(e => e.Comp)); } - public void GetEntitiesInRange(MapId mapId, Vector2 worldPos, float range, HashSet> entities) where T : IComponent + public void GetEntitiesInRange(MapId mapId, Vector2 worldPos, float range, HashSet> entities, LookupFlags flags = DefaultFlags) where T : IComponent { - DebugTools.Assert(range > 0, "Range must be a positive float"); + GetEntitiesInRange(mapId, new PhysShapeCircle(range, worldPos), entities, flags); + } + + public void GetEntitiesInRange(MapId mapId, IPhysShape shape, HashSet> entities, LookupFlags flags = DefaultFlags) where T : IComponent + { + DebugTools.Assert(shape.Radius > 0, "Range must be a positive float"); if (mapId == MapId.Nullspace) return; - // TODO: Actual circles - var rangeVec = new Vector2(range, range); - - var worldAABB = new Box2(worldPos - rangeVec, worldPos + rangeVec); - GetEntitiesIntersecting(mapId, worldAABB, entities); + GetEntitiesIntersecting(mapId, shape, entities, flags); } #endregion + + private readonly record struct GridQueryState( + HashSet> Intersecting, + IPhysShape Shape, + Box2 WorldAABB, + EntityLookupSystem Lookup, + LookupFlags Flags, + EntityQuery Query + ) where T : IComponent; + + private readonly record struct QueryState( + HashSet> Intersecting, + IPhysShape Shape, + Transform Transform, + FixtureSystem Fixtures, + SharedPhysicsSystem Physics, + SharedTransformSystem TransformSystem, + IManifoldManager Manifolds, + EntityQuery Query, + EntityQuery FixturesQuery, + bool Sensors + ) where T : IComponent; } diff --git a/Robust.Shared/GameObjects/Systems/EntityLookupSystem.cs b/Robust.Shared/GameObjects/Systems/EntityLookupSystem.cs index bab1a2f98..6a858ad57 100644 --- a/Robust.Shared/GameObjects/Systems/EntityLookupSystem.cs +++ b/Robust.Shared/GameObjects/Systems/EntityLookupSystem.cs @@ -11,9 +11,11 @@ using Robust.Shared.Maths; using Robust.Shared.Network; using Robust.Shared.Physics; using Robust.Shared.Physics.BroadPhase; +using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Dynamics; using Robust.Shared.Physics.Events; +using Robust.Shared.Physics.Systems; using Robust.Shared.Timing; using Robust.Shared.Utility; @@ -72,11 +74,14 @@ public record struct WorldAABBEvent public sealed partial class EntityLookupSystem : EntitySystem { + [Dependency] private readonly IManifoldManager _manifoldManager = default!; [Dependency] private readonly IMapManager _mapManager = default!; [Dependency] private readonly IGameTiming _timing = default!; [Dependency] private readonly INetManager _netMan = default!; [Dependency] private readonly SharedContainerSystem _container = default!; + [Dependency] private readonly FixtureSystem _fixtures = default!; [Dependency] private readonly SharedMapSystem _map = default!; + [Dependency] private readonly SharedPhysicsSystem _physics = default!; [Dependency] private readonly SharedTransformSystem _transform = default!; private EntityQuery _broadQuery;