space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-14 19:29:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

Reduce lookup allocs significantly (#5639)

Browse Source
This commit is contained in:
metalgearsloth
2025-01-29 23:43:20 +11:00
committed by GitHub
parent 033c52751a
commit 15f81751f7
10 changed files with 273 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
Robust.Shared/GameObjects/Systems/EntityLookup.Queries.cs
View File

@@ -1,8 +1,6 @@
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Runtime.CompilerServices;
using JetBrains.Annotations;
using Robust.Shared.Collections;
using Robust.Shared.Map;
using Robust.Shared.Map.Components;
@@ -11,7 +9,6 @@ using Robust.Shared.Physics;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Shapes;
using Robust.Shared.Physics.Systems;
using Robust.Shared.Utility;
@@ -77,14 +74,14 @@ public sealed partial class EntityLookupSystem
/// <summary>
/// Wrapper around the per-grid version.
/// </summary>
private void AddEntitiesIntersecting(MapId mapId,
private void AddEntitiesIntersecting<T>(MapId mapId,
HashSet<EntityUid> intersecting,
IPhysShape shape,
T shape,
Transform shapeTransform,
LookupFlags flags)
LookupFlags flags) where T : IPhysShape
{
var worldAABB = shape.ComputeAABB(shapeTransform, 0);
var state = new EntityQueryState(intersecting,
var state = new EntityQueryState<T>(intersecting,
shape,
shapeTransform,
_fixtures,
@@ -96,7 +93,7 @@ public sealed partial class EntityLookupSystem
// Need to include maps
_mapManager.FindGridsIntersecting(mapId, worldAABB, ref state,
static (EntityUid uid, MapGridComponent _, ref EntityQueryState state) =>
static (EntityUid uid, MapGridComponent _, ref EntityQueryState<T> state) =>
{
var localTransform = state.Physics.GetRelativePhysicsTransform(state.Transform, uid);
var localAabb = state.Shape.ComputeAABB(localTransform, 0);
@@ -112,19 +109,19 @@ public sealed partial class EntityLookupSystem
AddContained(intersecting, flags);
}
private void AddEntitiesIntersecting(
private void AddEntitiesIntersecting<T>(
EntityUid lookupUid,
HashSet<EntityUid> intersecting,
IPhysShape shape,
T shape,
Box2 localAABB,
Transform localShapeTransform,
LookupFlags flags,
BroadphaseComponent? lookup = null)
BroadphaseComponent? lookup = null) where T : IPhysShape
{
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return;
var state = new EntityQueryState(
var state = new EntityQueryState<T>(
intersecting,
shape,
localShapeTransform,
@@ -157,7 +154,7 @@ public sealed partial class EntityLookupSystem
return;
static bool PhysicsQuery(ref EntityQueryState state, in FixtureProxy value)
static bool PhysicsQuery(ref EntityQueryState<T> state, in FixtureProxy value)
{
var sensors = (state.Flags & LookupFlags.Sensors) != 0x0;
@@ -179,7 +176,7 @@ public sealed partial class EntityLookupSystem
return true;
}
static bool SundriesQuery(ref EntityQueryState state, in EntityUid value)
static bool SundriesQuery(ref EntityQueryState<T> state, in EntityUid value)
{
var approx = (state.Flags & LookupFlags.Approximate) != 0x0;
@@ -227,14 +224,14 @@ public sealed partial class EntityLookupSystem
/// <summary>
/// Wrapper around the per-grid version.
/// </summary>
private bool AnyEntitiesIntersecting(MapId mapId,
IPhysShape shape,
private bool AnyEntitiesIntersecting<T>(MapId mapId,
T shape,
Transform shapeTransform,
LookupFlags flags,
EntityUid? ignored = null)
EntityUid? ignored = null) where T : IPhysShape
{
var worldAABB = shape.ComputeAABB(shapeTransform, 0);
var state = new AnyEntityQueryState(false,
var state = new AnyEntityQueryState<T>(false,
ignored,
shape,
shapeTransform,
@@ -247,7 +244,7 @@ public sealed partial class EntityLookupSystem
// Need to include maps
_mapManager.FindGridsIntersecting(mapId, worldAABB, ref state,
static (EntityUid uid, MapGridComponent _, ref AnyEntityQueryState state) =>
static (EntityUid uid, MapGridComponent _, ref AnyEntityQueryState<T> state) =>
{
var localTransform = state.Physics.GetRelativePhysicsTransform(state.Transform, uid);
var localAabb = state.Shape.ComputeAABB(localTransform, 0);
@@ -272,18 +269,18 @@ public sealed partial class EntityLookupSystem
return state.Found;
}
private bool AnyEntitiesIntersecting(EntityUid lookupUid,
IPhysShape shape,
private bool AnyEntitiesIntersecting<T>(EntityUid lookupUid,
T shape,
Box2 localAABB,
Transform shapeTransform,
LookupFlags flags,
EntityUid? ignored = null,
BroadphaseComponent? lookup = null)
BroadphaseComponent? lookup = null) where T : IPhysShape
{
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return false;
var state = new AnyEntityQueryState(false,
var state = new AnyEntityQueryState<T>(false,
ignored,
shape,
shapeTransform,
@@ -325,7 +322,7 @@ public sealed partial class EntityLookupSystem
return state.Found;
static bool PhysicsQuery(ref AnyEntityQueryState state, in FixtureProxy value)
static bool PhysicsQuery(ref AnyEntityQueryState<T> state, in FixtureProxy value)
{
if (state.Ignored == value.Entity)
return true;
@@ -350,7 +347,7 @@ public sealed partial class EntityLookupSystem
return false;
}
static bool SundriesQuery(ref AnyEntityQueryState state, in EntityUid value)
static bool SundriesQuery(ref AnyEntityQueryState<T> state, in EntityUid value)
{
if (state.Ignored == value)
return true;
@@ -409,9 +406,16 @@ public sealed partial class EntityLookupSystem
var broadphaseInv = _transform.GetInvWorldMatrix(lookupUid);
var localBounds = broadphaseInv.TransformBounds(worldBounds);
var shape = new Polygon(localBounds);
var polygon = _physics.GetPooled(localBounds);
var result = AnyEntitiesIntersecting(lookupUid,
polygon,
localBounds.CalcBoundingBox(),
Physics.Transform.Empty,
flags,
ignored);
return AnyEntitiesIntersecting(lookupUid, shape, localBounds.CalcBoundingBox(), Physics.Transform.Empty, flags, ignored);
_physics.ReturnPooled(polygon);
return result;
}
#endregion
@@ -454,8 +458,10 @@ public sealed partial class EntityLookupSystem
{
if (mapId == MapId.Nullspace) return false;
var shape = new Polygon(worldAABB);
return AnyEntitiesIntersecting(mapId, shape, Physics.Transform.Empty, flags);
var polygon = _physics.GetPooled(worldAABB);
var result = AnyEntitiesIntersecting(mapId, polygon, Physics.Transform.Empty, flags);
_physics.ReturnPooled(polygon);
return result;
}
public HashSet<EntityUid> GetEntitiesIntersecting(MapId mapId, Box2 worldAABB, LookupFlags flags = DefaultFlags)
@@ -469,8 +475,9 @@ public sealed partial class EntityLookupSystem
{
if (mapId == MapId.Nullspace) return;
var shape = new Polygon(worldAABB);
AddEntitiesIntersecting(mapId, intersecting, shape, Physics.Transform.Empty, flags);
var polygon = _physics.GetPooled(worldAABB);
AddEntitiesIntersecting(mapId, intersecting, polygon, Physics.Transform.Empty, flags);
_physics.ReturnPooled(polygon);
}
#endregion
@@ -480,15 +487,18 @@ public sealed partial class EntityLookupSystem
public bool AnyEntitiesIntersecting(MapId mapId, Box2Rotated worldBounds, LookupFlags flags = DefaultFlags)
{
// Don't need to check contained entities as they have the same bounds as the parent.
var shape = new Polygon(worldBounds);
return AnyEntitiesIntersecting(mapId, shape, Physics.Transform.Empty, flags);
var polygon = _physics.GetPooled(worldBounds);
var result = AnyEntitiesIntersecting(mapId, polygon, Physics.Transform.Empty, flags);
_physics.ReturnPooled(polygon);
return result;
}
public HashSet<EntityUid> GetEntitiesIntersecting(MapId mapId, Box2Rotated worldBounds, LookupFlags flags = DefaultFlags)
{
var intersecting = new HashSet<EntityUid>();
var shape = new Polygon(worldBounds);
AddEntitiesIntersecting(mapId, intersecting, shape, Physics.Transform.Empty, flags);
var polygon = _physics.GetPooled(worldBounds);
AddEntitiesIntersecting(mapId, intersecting, polygon, Physics.Transform.Empty, flags);
_physics.ReturnPooled(polygon);
return intersecting;
}
@@ -703,12 +713,12 @@ public sealed partial class EntityLookupSystem
return entities;
}
public void GetEntitiesIntersecting(
public void GetEntitiesIntersecting<T>(
MapId mapId,
IPhysShape shape,
T shape,
Transform transform,
HashSet<EntityUid> entities,
LookupFlags flags = LookupFlags.All)
LookupFlags flags = LookupFlags.All) where T : IPhysShape
{
if (mapId == MapId.Nullspace)
return;
@@ -751,10 +761,11 @@ public sealed partial class EntityLookupSystem
return;
var localAABB = _transform.GetInvWorldMatrix(gridId).TransformBox(worldAABB);
var shape = new Polygon(localAABB);
var polygon = _physics.GetPooled(localAABB);
AddEntitiesIntersecting(gridId, intersecting, shape, localAABB, Physics.Transform.Empty, flags, lookup);
AddEntitiesIntersecting(gridId, intersecting, polygon, localAABB, Physics.Transform.Empty, flags, lookup);
AddContained(intersecting, flags);
_physics.ReturnPooled(polygon);
}
public void GetEntitiesIntersecting(EntityUid gridId, Box2Rotated worldBounds, HashSet<EntityUid> intersecting, LookupFlags flags = DefaultFlags)
@@ -763,10 +774,11 @@ public sealed partial class EntityLookupSystem
return;
var localBounds = _transform.GetInvWorldMatrix(gridId).TransformBounds(worldBounds);
var shape = new Polygon(localBounds);
var polygon = _physics.GetPooled(localBounds);
AddEntitiesIntersecting(gridId, intersecting, shape, localBounds.CalcBoundingBox(), Physics.Transform.Empty, flags, lookup);
AddEntitiesIntersecting(gridId, intersecting, polygon, localBounds.CalcBoundingBox(), Physics.Transform.Empty, flags, lookup);
AddContained(intersecting, flags);
_physics.ReturnPooled(polygon);
}
#endregion
@@ -832,10 +844,10 @@ public sealed partial class EntityLookupSystem
#endregion
private record struct AnyEntityQueryState(
private record struct AnyEntityQueryState<T>(
bool Found,
EntityUid? Ignored,
IPhysShape Shape,
T Shape,
Transform Transform,
FixtureSystem Fixtures,
EntityLookupSystem Lookup,
@@ -843,11 +855,11 @@ public sealed partial class EntityLookupSystem
IManifoldManager Manifolds,
EntityQuery<FixturesComponent> FixturesQuery,
LookupFlags Flags
);
) where T : IPhysShape;
private readonly record struct EntityQueryState(
private readonly record struct EntityQueryState<T>(
HashSet<EntityUid> Intersecting,
IPhysShape Shape,
T Shape,
Transform Transform,
FixtureSystem Fixtures,
EntityLookupSystem Lookup,
@@ -855,5 +867,5 @@ public sealed partial class EntityLookupSystem
IManifoldManager Manifolds,
EntityQuery<FixturesComponent> FixturesQuery,
LookupFlags Flags
);
) where T : IPhysShape;
}

105
Robust.Shared/GameObjects/Systems/EntityLookupSystem.ComponentQueries.cs
View File

@@ -67,7 +67,7 @@ public sealed partial class EntityLookupSystem
/// <summary>
/// Common method to determine if an entity overlaps the specified shape.
/// </summary>
private bool IsIntersecting(MapId mapId, EntityUid uid, TransformComponent xform, IPhysShape shape, Transform shapeTransform, Box2 worldAABB, LookupFlags flags)
private bool IsIntersecting<TShape>(MapId mapId, EntityUid uid, TransformComponent xform, TShape shape, Transform shapeTransform, Box2 worldAABB, LookupFlags flags) where TShape : IPhysShape
{
var (entPos, entRot) = _transform.GetWorldPositionRotation(xform);
@@ -121,25 +121,27 @@ public sealed partial class EntityLookupSystem
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return;
var lookupPoly = new Polygon(localAABB);
AddEntitiesIntersecting(lookupUid, intersecting, lookupPoly, localAABB, Physics.Transform.Empty, flags, query, lookup);
var polygon = _physics.GetPooled(localAABB);
AddEntitiesIntersecting(lookupUid, intersecting, polygon, localAABB, Physics.Transform.Empty, flags, query, lookup);
_physics.ReturnPooled(polygon);
}
private void AddEntitiesIntersecting<T>(
private void AddEntitiesIntersecting<T, TShape>(
EntityUid lookupUid,
HashSet<Entity<T>> intersecting,
IPhysShape shape,
TShape shape,
Box2 localAABB,
Transform localTransform,
LookupFlags flags,
EntityQuery<T> query,
BroadphaseComponent? lookup = null) where T : IComponent
BroadphaseComponent? lookup = null)
where T : IComponent
where TShape : IPhysShape
{
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return;
var state = new QueryState<T>(
var state = new QueryState<T, TShape>(
intersecting,
shape,
localTransform,
@@ -174,7 +176,7 @@ public sealed partial class EntityLookupSystem
return;
static bool PhysicsQuery(ref QueryState<T> state, in FixtureProxy value)
static bool PhysicsQuery(ref QueryState<T, TShape> state, in FixtureProxy value)
{
if (!state.Sensors && !value.Fixture.Hard)
return true;
@@ -195,7 +197,7 @@ public sealed partial class EntityLookupSystem
return true;
}
static bool SundriesQuery(ref QueryState<T> state, in EntityUid value)
static bool SundriesQuery(ref QueryState<T, TShape> state, in EntityUid value)
{
if (!state.Query.TryGetComponent(value, out var comp))
return true;
@@ -250,22 +252,26 @@ public sealed partial class EntityLookupSystem
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return false;
var shape = new Polygon(localAABB);
var polygon = _physics.GetPooled(localAABB);
var (lookupPos, lookupRot) = _transform.GetWorldPositionRotation(lookupUid);
var transform = new Transform(lookupPos, lookupRot);
var result = AnyComponentsIntersecting(lookupUid, polygon, localAABB, transform, flags, query, ignored, lookup);
_physics.ReturnPooled(polygon);
return AnyComponentsIntersecting(lookupUid, shape, localAABB, transform, flags, query, ignored, lookup);
return result;
}
private bool AnyComponentsIntersecting<T>(
private bool AnyComponentsIntersecting<T, TShape>(
EntityUid lookupUid,
IPhysShape shape,
TShape shape,
Box2 localAABB,
Transform shapeTransform,
LookupFlags flags,
EntityQuery<T> query,
EntityUid? ignored = null,
BroadphaseComponent? lookup = null) where T : IComponent
BroadphaseComponent? lookup = null)
where T : IComponent
where TShape : IPhysShape
{
/*
* Unfortunately this is split from the other query as we can short-circuit here, hence the code duplication.
@@ -275,7 +281,7 @@ public sealed partial class EntityLookupSystem
if (!_broadQuery.Resolve(lookupUid, ref lookup))
return false;
var state = new AnyQueryState<T>(false,
var state = new AnyQueryState<T, TShape>(false,
ignored,
shape,
shapeTransform,
@@ -317,7 +323,7 @@ public sealed partial class EntityLookupSystem
return state.Found;
static bool PhysicsQuery(ref AnyQueryState<T> state, in FixtureProxy value)
static bool PhysicsQuery(ref AnyQueryState<T, TShape> state, in FixtureProxy value)
{
if (value.Entity == state.Ignored)
return true;
@@ -342,7 +348,7 @@ public sealed partial class EntityLookupSystem
return false;
}
static bool SundriesQuery(ref AnyQueryState<T> state, in EntityUid value)
static bool SundriesQuery(ref AnyQueryState<T, TShape> state, in EntityUid value)
{
if (state.Ignored == value)
return true;
@@ -421,13 +427,14 @@ public sealed partial class EntityLookupSystem
public bool AnyComponentsIntersecting(Type type, MapId mapId, Box2 worldAABB, EntityUid? ignored = null, LookupFlags flags = DefaultFlags)
{
var shape = new Polygon(worldAABB);
var polygon = _physics.GetPooled(worldAABB);
var transform = Physics.Transform.Empty;
return AnyComponentsIntersecting(type, mapId, shape, transform, ignored, flags);
var result = AnyComponentsIntersecting(type, mapId, polygon, transform, ignored, flags);
_physics.ReturnPooled(polygon);
return result;
}
public bool AnyComponentsIntersecting(Type type, MapId mapId, IPhysShape shape, Transform shapeTransform, EntityUid? ignored = null, LookupFlags flags = DefaultFlags)
public bool AnyComponentsIntersecting<T>(Type type, MapId mapId, T shape, Transform shapeTransform, EntityUid? ignored = null, LookupFlags flags = DefaultFlags) where T : IPhysShape
{
DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type));
if (mapId == MapId.Nullspace)
@@ -489,37 +496,40 @@ public sealed partial class EntityLookupSystem
if (mapId == MapId.Nullspace)
return;
var shape = new Polygon(worldAABB);
var polygon = _physics.GetPooled(worldAABB);
var transform = Physics.Transform.Empty;
GetEntitiesIntersecting(type, mapId, shape, transform, intersecting, flags);
GetEntitiesIntersecting(type, mapId, polygon, transform, intersecting, flags);
_physics.ReturnPooled(polygon);
}
public void GetEntitiesIntersecting<T>(MapId mapId, Box2Rotated worldBounds, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags) where T : IComponent
{
if (mapId == MapId.Nullspace) return;
var shape = new Polygon(worldBounds);
var polygon = _physics.GetPooled(worldBounds);
var shapeTransform = Physics.Transform.Empty;
GetEntitiesIntersecting(mapId, shape, shapeTransform, entities, flags);
GetEntitiesIntersecting(mapId, polygon, shapeTransform, entities, flags);
_physics.ReturnPooled(polygon);
}
public void GetEntitiesIntersecting<T>(MapId mapId, Box2 worldAABB, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags) where T : IComponent
{
if (mapId == MapId.Nullspace) return;
var shape = new Polygon(worldAABB);
var polygon = _physics.GetPooled(worldAABB);
var shapeTransform = Physics.Transform.Empty;
GetEntitiesIntersecting(mapId, shape, shapeTransform, entities, flags);
GetEntitiesIntersecting(mapId, polygon, shapeTransform, entities, flags);
_physics.ReturnPooled(polygon);
}
#endregion
#region IPhysShape
public void GetEntitiesIntersecting(Type type, MapId mapId, IPhysShape shape, Transform shapeTransform, HashSet<Entity<IComponent>> intersecting, LookupFlags flags = DefaultFlags)
public void GetEntitiesIntersecting<T>(Type type, MapId mapId, T shape, Transform shapeTransform, HashSet<Entity<IComponent>> intersecting, LookupFlags flags = DefaultFlags) where T : IPhysShape
{
DebugTools.Assert(typeof(IComponent).IsAssignableFrom(type));
if (mapId == MapId.Nullspace)
@@ -544,10 +554,10 @@ public sealed partial class EntityLookupSystem
var query = EntityManager.GetEntityQuery(type);
// Get grid entities
var state = new GridQueryState<IComponent>(intersecting, shape, shapeTransform, this, _physics, flags, query);
var state = new GridQueryState<IComponent, T>(intersecting, shape, shapeTransform, this, _physics, flags, query);
_mapManager.FindGridsIntersecting(mapId, worldAABB, ref state,
static (EntityUid uid, MapGridComponent grid, ref GridQueryState<IComponent> state) =>
static (EntityUid uid, MapGridComponent grid, ref GridQueryState<IComponent, T> state) =>
{
var localTransform = state.Physics.GetRelativePhysicsTransform(state.Transform, uid);
var localAabb = state.Shape.ComputeAABB(localTransform, 0);
@@ -565,7 +575,9 @@ public sealed partial class EntityLookupSystem
}
}
public void GetEntitiesIntersecting<T>(MapId mapId, IPhysShape shape, Transform shapeTransform, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags) where T : IComponent
public void GetEntitiesIntersecting<T, TShape>(MapId mapId, TShape shape, Transform shapeTransform, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags)
where T : IComponent
where TShape : IPhysShape
{
if (mapId == MapId.Nullspace) return;
@@ -588,10 +600,10 @@ public sealed partial class EntityLookupSystem
var query = GetEntityQuery<T>();
// Get grid entities
var state = new GridQueryState<T>(entities, shape, shapeTransform, this, _physics, flags, query);
var state = new GridQueryState<T, TShape>(entities, shape, shapeTransform, this, _physics, flags, query);
_mapManager.FindGridsIntersecting(mapId, worldAABB, ref state,
static (EntityUid uid, MapGridComponent grid, ref GridQueryState<T> state) =>
static (EntityUid uid, MapGridComponent grid, ref GridQueryState<T, TShape> state) =>
{
var localTransform = state.Physics.GetRelativePhysicsTransform(state.Transform, uid);
var localAabb = state.Shape.ComputeAABB(localTransform, 0);
@@ -679,7 +691,9 @@ public sealed partial class EntityLookupSystem
GetEntitiesInRange(mapId, shape, transform, entities, flags);
}
public void GetEntitiesInRange<T>(MapId mapId, IPhysShape shape, Transform transform, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags) where T : IComponent
public void GetEntitiesInRange<T, TShape>(MapId mapId, TShape shape, Transform transform, HashSet<Entity<T>> entities, LookupFlags flags = DefaultFlags)
where T : IComponent
where TShape : IPhysShape
{
DebugTools.Assert(shape.Radius > 0, "Range must be a positive float");
@@ -829,20 +843,21 @@ public sealed partial class EntityLookupSystem
}
}
private readonly record struct GridQueryState<T>(
private readonly record struct GridQueryState<T, TShape>(
HashSet<Entity<T>> Intersecting,
IPhysShape Shape,
TShape Shape,
Transform Transform,
EntityLookupSystem Lookup,
SharedPhysicsSystem Physics,
LookupFlags Flags,
EntityQuery<T> Query
) where T : IComponent;
) where T : IComponent
where TShape : IPhysShape;
private record struct AnyQueryState<T>(
private record struct AnyQueryState<T, TShape>(
bool Found,
EntityUid? Ignored,
IPhysShape Shape,
TShape Shape,
Transform Transform,
FixtureSystem Fixtures,
SharedPhysicsSystem Physics,
@@ -850,11 +865,12 @@ public sealed partial class EntityLookupSystem
EntityQuery<T> Query,
EntityQuery<FixturesComponent> FixturesQuery,
LookupFlags Flags
) where T : IComponent;
) where T : IComponent
where TShape : IPhysShape;
private readonly record struct QueryState<T>(
private readonly record struct QueryState<T, TShape>(
HashSet<Entity<T>> Intersecting,
IPhysShape Shape,
TShape Shape,
Transform Transform,
FixtureSystem Fixtures,
SharedPhysicsSystem Physics,
@@ -863,5 +879,6 @@ public sealed partial class EntityLookupSystem
EntityQuery<FixturesComponent> FixturesQuery,
bool Sensors,
bool Approximate
) where T : IComponent;
) where T : IComponent
where TShape : IPhysShape;
}

20
Robust.Shared/Map/IMapManager.cs
View File

@@ -77,11 +77,11 @@ namespace Robust.Shared.Map
#region MapId
public void FindGridsIntersecting(MapId mapId, IPhysShape shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting<T>(MapId mapId, T shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = Approximate, bool includeMap = IncludeMap) where T : IPhysShape;
public void FindGridsIntersecting(MapId mapId, IPhysShape shape, Transform transform, GridCallback callback,
bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting<T>(MapId mapId, T shape, Transform transform, GridCallback callback,
bool approx = Approximate, bool includeMap = IncludeMap) where T : IPhysShape;
public void FindGridsIntersecting(MapId mapId, Box2 worldAABB, GridCallback callback, bool approx = Approximate,
bool includeMap = IncludeMap);
@@ -107,11 +107,11 @@ namespace Robust.Shared.Map
#region MapEnt
public void FindGridsIntersecting(EntityUid mapEnt, IPhysShape shape, Transform transform, GridCallback callback,
bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting<T>(EntityUid mapEnt, T shape, Transform transform, GridCallback callback,
bool approx = Approximate, bool includeMap = IncludeMap) where T : IPhysShape;
public void FindGridsIntersecting<TState>(EntityUid mapEnt, IPhysShape shape, Transform transform,
ref TState state, GridCallback<TState> callback, bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting<T, TState>(EntityUid mapEnt, T shape, Transform transform,
ref TState state, GridCallback<TState> callback, bool approx = Approximate, bool includeMap = IncludeMap) where T : IPhysShape;
/// <summary>
/// Returns true if any grids overlap the specified shapes.
@@ -119,8 +119,8 @@ namespace Robust.Shared.Map
public void FindGridsIntersecting(EntityUid mapEnt, List<IPhysShape> shapes, Transform transform,
ref List<Entity<MapGridComponent>> entities, bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting(EntityUid mapEnt, IPhysShape shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = Approximate, bool includeMap = IncludeMap);
public void FindGridsIntersecting<T>(EntityUid mapEnt, T shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = Approximate, bool includeMap = IncludeMap) where T : IPhysShape;
public void FindGridsIntersecting(EntityUid mapEnt, Box2 worldAABB, GridCallback callback,
bool approx = Approximate, bool includeMap = IncludeMap);

93
Robust.Shared/Map/MapManager.Queries.cs
View File

@@ -8,17 +8,16 @@ using Robust.Shared.Map.Enumerators;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Shapes;
namespace Robust.Shared.Map;
internal partial class MapManager
{
private bool IsIntersecting(
private bool IsIntersecting<T>(
ChunkEnumerator enumerator,
IPhysShape shape,
T shape,
Transform shapeTransform,
Entity<FixturesComponent> grid)
Entity<FixturesComponent> grid) where T : IPhysShape
{
var gridTransform = _physics.GetPhysicsTransform(grid);
@@ -43,14 +42,14 @@ internal partial class MapManager
#region MapId
public void FindGridsIntersecting(MapId mapId, IPhysShape shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
public void FindGridsIntersecting<T>(MapId mapId, T shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
if (_mapEntities.TryGetValue(mapId, out var mapEnt))
FindGridsIntersecting(mapEnt, shape, transform, ref grids, approx, includeMap);
}
public void FindGridsIntersecting(MapId mapId, IPhysShape shape, Transform transform, GridCallback callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
public void FindGridsIntersecting<T>(MapId mapId, T shape, Transform transform, GridCallback callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
if (_mapEntities.TryGetValue(mapId, out var mapEnt))
FindGridsIntersecting(mapEnt, shape, transform, callback, includeMap, approx);
@@ -100,18 +99,18 @@ internal partial class MapManager
#region MapEnt
public void FindGridsIntersecting(
public void FindGridsIntersecting<T>(
EntityUid mapEnt,
IPhysShape shape,
T shape,
Transform transform,
GridCallback callback,
bool approx = IMapManager.Approximate,
bool includeMap = IMapManager.IncludeMap)
bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
FindGridsIntersecting(mapEnt, shape, shape.ComputeAABB(transform, 0), transform, callback, approx, includeMap);
}
private void FindGridsIntersecting(EntityUid mapEnt, IPhysShape shape, Box2 worldAABB, Transform transform, GridCallback callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
private void FindGridsIntersecting<T>(EntityUid mapEnt, T shape, Box2 worldAABB, Transform transform, GridCallback callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
// This is here so we don't double up on code.
var state = callback;
@@ -121,20 +120,27 @@ internal partial class MapManager
approx: approx, includeMap: includeMap);
}
public void FindGridsIntersecting<TState>(
public void FindGridsIntersecting<T, TState>(
EntityUid mapEnt,
IPhysShape shape,
T shape,
Transform transform,
ref TState state,
GridCallback<TState> callback,
bool approx = IMapManager.Approximate,
bool includeMap = IMapManager.IncludeMap)
bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
FindGridsIntersecting(mapEnt, shape, shape.ComputeAABB(transform, 0), transform, ref state, callback, approx, includeMap);
}
private void FindGridsIntersecting<TState>(EntityUid mapEnt, IPhysShape shape, Box2 worldAABB, Transform transform,
ref TState state, GridCallback<TState> callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
private void FindGridsIntersecting<T, TState>(
EntityUid mapEnt,
T shape,
Box2 worldAABB,
Transform transform,
ref TState state,
GridCallback<TState> callback,
bool approx = IMapManager.Approximate,
bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
if (!_gridTreeQuery.TryGetComponent(mapEnt, out var gridTree))
return;
@@ -144,7 +150,7 @@ internal partial class MapManager
callback(mapEnt, mapGrid, ref state);
}
var gridState = new GridQueryState<TState>(
var gridState = new GridQueryState<T, TState>(
callback,
state,
worldAABB,
@@ -156,8 +162,7 @@ internal partial class MapManager
_transformSystem,
approx);
gridTree.Tree.Query(ref gridState, static (ref GridQueryState<TState> state, DynamicTree.Proxy proxy) =>
gridTree.Tree.Query(ref gridState, static (ref GridQueryState<T, TState> state, DynamicTree.Proxy proxy) =>
{
// Even for approximate we'll check if any chunks roughly overlap.
var data = state.Tree.GetUserData(proxy);
@@ -198,14 +203,14 @@ internal partial class MapManager
}
}
public void FindGridsIntersecting(EntityUid mapEnt, IPhysShape shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
public void FindGridsIntersecting<T>(EntityUid mapEnt, T shape, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
FindGridsIntersecting(mapEnt, shape, shape.ComputeAABB(transform, 0), transform, ref grids, approx, includeMap);
}
public void FindGridsIntersecting(EntityUid mapEnt, IPhysShape shape, Box2 worldAABB, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
public void FindGridsIntersecting<T>(EntityUid mapEnt, T shape, Box2 worldAABB, Transform transform,
ref List<Entity<MapGridComponent>> grids, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap) where T : IPhysShape
{
var state = grids;
@@ -219,39 +224,48 @@ internal partial class MapManager
public void FindGridsIntersecting(EntityUid mapEnt, Box2 worldAABB, GridCallback callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
{
FindGridsIntersecting(mapEnt, new Polygon(worldAABB), worldAABB, Transform.Empty, callback, approx, includeMap);
var polygon = _physics.GetPooled(worldAABB);
FindGridsIntersecting(mapEnt, polygon, worldAABB, Transform.Empty, callback, approx, includeMap);
_physics.ReturnPooled(polygon);
}
public void FindGridsIntersecting<TState>(EntityUid mapEnt, Box2 worldAABB, ref TState state, GridCallback<TState> callback, bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
{
FindGridsIntersecting(mapEnt, new Polygon(worldAABB), worldAABB, Transform.Empty, ref state, callback, approx, includeMap);
var polygon = _physics.GetPooled(worldAABB);
FindGridsIntersecting(mapEnt, polygon, worldAABB, Transform.Empty, ref state, callback, approx, includeMap);
_physics.ReturnPooled(polygon);
}
public void FindGridsIntersecting(EntityUid mapEnt, Box2 worldAABB, ref List<Entity<MapGridComponent>> grids,
bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
{
FindGridsIntersecting(mapEnt, new Polygon(worldAABB), worldAABB, Transform.Empty, ref grids, approx, includeMap);
var polygon = _physics.GetPooled(worldAABB);
FindGridsIntersecting(mapEnt, polygon, worldAABB, Transform.Empty, ref grids, approx, includeMap);
_physics.ReturnPooled(polygon);
}
public void FindGridsIntersecting(EntityUid mapEnt, Box2Rotated worldBounds, GridCallback callback, bool approx = IMapManager.Approximate,
bool includeMap = IMapManager.IncludeMap)
{
var shape = new Polygon(worldBounds);
FindGridsIntersecting(mapEnt, shape, worldBounds.CalcBoundingBox(), Transform.Empty, callback, approx, includeMap);
var polygon = _physics.GetPooled(worldBounds);
FindGridsIntersecting(mapEnt, polygon, worldBounds.CalcBoundingBox(), Transform.Empty, callback, approx, includeMap);
_physics.ReturnPooled(polygon);
}
public void FindGridsIntersecting<TState>(EntityUid mapEnt, Box2Rotated worldBounds, ref TState state, GridCallback<TState> callback,
bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
{
var shape = new Polygon(worldBounds);
FindGridsIntersecting(mapEnt, shape, worldBounds.CalcBoundingBox(), Transform.Empty, ref state, callback, approx, includeMap);
var polygon = _physics.GetPooled(worldBounds);
FindGridsIntersecting(mapEnt, polygon, worldBounds.CalcBoundingBox(), Transform.Empty, ref state, callback, approx, includeMap);
_physics.ReturnPooled(polygon);
}
public void FindGridsIntersecting(EntityUid mapEnt, Box2Rotated worldBounds, ref List<Entity<MapGridComponent>> grids,
bool approx = IMapManager.Approximate, bool includeMap = IMapManager.IncludeMap)
{
var shape = new Polygon(worldBounds);
FindGridsIntersecting(mapEnt, shape, worldBounds.CalcBoundingBox(), Transform.Empty, ref grids, approx, includeMap);
var polygon = _physics.GetPooled(worldBounds);
FindGridsIntersecting(mapEnt, polygon, worldBounds.CalcBoundingBox(), Transform.Empty, ref grids, approx, includeMap);
_physics.ReturnPooled(polygon);
}
#endregion
@@ -342,22 +356,11 @@ internal partial class MapManager
#endregion
private readonly record struct GridQueryState(
GridCallback Callback,
Box2 WorldAABB,
IPhysShape Shape,
Transform Transform,
B2DynamicTree<(EntityUid Uid, FixturesComponent Fixtures, MapGridComponent Grid)> Tree,
SharedMapSystem MapSystem,
MapManager MapManager,
SharedTransformSystem TransformSystem,
bool Approximate);
private record struct GridQueryState<TState>(
private record struct GridQueryState<T, TState>(
GridCallback<TState> Callback,
TState State,
Box2 WorldAABB,
IPhysShape Shape,
T Shape,
Transform Transform,
B2DynamicTree<(EntityUid Uid, FixturesComponent Fixtures, MapGridComponent Grid)> Tree,
SharedMapSystem MapSystem,

6
Robust.Shared/Physics/Collision/CollisionManager.Overlap.cs
View File

@@ -12,12 +12,12 @@ internal sealed partial class CollisionManager
/// <param name="xfA">The transform for the first shape.</param>
/// <param name="xfB">The transform for the seconds shape.</param>
/// <returns></returns>
public bool TestOverlap(IPhysShape shapeA, int childIndexA, IPhysShape shapeB, int childIndexB, in Transform xfA, in Transform xfB)
public bool TestOverlap<T, U>(T shapeA, int indexA, U shapeB, int indexB, in Transform xfA, in Transform xfB) where T : IPhysShape where U : IPhysShape
{
var input = new DistanceInput();
input.ProxyA.Set(shapeA, childIndexA);
input.ProxyB.Set(shapeB, childIndexB);
input.ProxyA.Set(shapeA, indexA);
input.ProxyB.Set(shapeB, indexB);
input.TransformA = xfA;
input.TransformB = xfB;
input.UseRadii = true;

11
Robust.Shared/Physics/Collision/DistanceProxy.cs
View File

@@ -24,6 +24,7 @@ using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using Robust.Shared.Map;
using Robust.Shared.Maths;
@@ -56,12 +57,12 @@ internal ref struct DistanceProxy
/// must remain in scope while the proxy is in use.
/// </summary>
/// <param name="shape">The shape.</param>
public void Set(IPhysShape shape, int index)
internal void Set<T>(T shape, int index) where T : IPhysShape
{
switch (shape.ShapeType)
{
case ShapeType.Circle:
PhysShapeCircle circle = (PhysShapeCircle) shape;
var circle = Unsafe.As<PhysShapeCircle>(shape);
Buffer._00 = circle.Position;
Vertices = Buffer.AsSpan[..1];
Radius = circle.Radius;
@@ -75,7 +76,7 @@ internal ref struct DistanceProxy
}
else
{
var polyShape = (PolygonShape) shape;
var polyShape = Unsafe.As<PolygonShape>(shape);
Vertices = polyShape.Vertices;
Radius = polyShape.Radius;
}
@@ -83,7 +84,7 @@ internal ref struct DistanceProxy
break;
case ShapeType.Chain:
ChainShape chain = (ChainShape) shape;
var chain = Unsafe.As<ChainShape>(shape);
Debug.Assert(0 <= index && index < chain.Vertices.Length);
Buffer._00 = chain.Vertices[index];
@@ -93,7 +94,7 @@ internal ref struct DistanceProxy
Radius = chain.Radius;
break;
case ShapeType.Edge:
EdgeShape edge = (EdgeShape) shape;
var edge = Unsafe.As<EdgeShape>(shape);
Buffer._00 = edge.Vertex1;
Buffer._01 = edge.Vertex2;

4
Robust.Shared/Physics/Collision/IManifoldManager.cs
View File

@@ -12,7 +12,9 @@ internal interface IManifoldManager
void ReturnEdge(EdgeShape edge);
bool TestOverlap(IPhysShape shapeA, int indexA, IPhysShape shapeB, int indexB, in Transform xfA, in Transform xfB);
bool TestOverlap<T, U>(T shapeA, int indexA, U shapeB, int indexB, in Transform xfA, in Transform xfB)
where T : IPhysShape
where U : IPhysShape;
void CollideCircles(ref Manifold manifold, PhysShapeCircle circleA, in Transform xfA,
PhysShapeCircle circleB, in Transform xfB);

35
Robust.Shared/Physics/Shapes/Polygon.cs
View File

@@ -44,6 +44,17 @@ internal record struct Polygon : IPhysShape
Array.Copy(polyShape.Normals, Normals, Vertices.Length);
}
/// <summary>
/// Manually constructed polygon for internal use to take advantage of pooling.
/// </summary>
internal Polygon(Vector2[] vertices, Vector2[] normals, Vector2 centroid)
{
Unsafe.SkipInit(out this);
Vertices = vertices;
Normals = normals;
Centroid = centroid;
}
public Polygon(Box2 aabb)
{
Unsafe.SkipInit(out this);
@@ -67,15 +78,16 @@ internal record struct Polygon : IPhysShape
public Polygon(Box2Rotated bounds)
{
Unsafe.SkipInit(out this);
Vertices = new Vector2[4];
Normals = new Vector2[4];
Radius = 0f;
Span<Vector2> verts = stackalloc Vector2[4];
verts[0] = bounds.BottomLeft;
verts[1] = bounds.BottomRight;
verts[2] = bounds.TopRight;
verts[3] = bounds.TopLeft;
var hull = new PhysicsHull(verts, 4);
Set(hull);
Vertices[0] = bounds.BottomLeft;
Vertices[1] = bounds.BottomRight;
Vertices[2] = bounds.TopRight;
Vertices[3] = bounds.TopLeft;
CalculateNormals(Normals, 4);
Centroid = bounds.Center;
}
@@ -116,9 +128,14 @@ internal record struct Polygon : IPhysShape
}
// Compute normals. Ensure the edges have non-zero length.
for (var i = 0; i < vertexCount; i++)
CalculateNormals(Normals, vertexCount);
}
internal void CalculateNormals(Span<Vector2> normals, int count)
{
for (var i = 0; i < count; i++)
{
var next = i + 1 < vertexCount ? i + 1 : 0;
var next = i + 1 < count ? i + 1 : 0;
var edge = Vertices[next] - Vertices[i];
DebugTools.Assert(edge.LengthSquared() > float.Epsilon * float.Epsilon);

56
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Pool.cs Normal file
View File

@@ -0,0 +1,56 @@
using System.Buffers;
using System.Numerics;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Shapes;
namespace Robust.Shared.Physics.Systems;
public abstract partial class SharedPhysicsSystem
{
private ArrayPool<Vector2> _vectorPool = ArrayPool<Vector2>.Create(4, 128);
/// <summary>
/// Gets a polygon with pooled arrays backing it.
/// </summary>
internal Polygon GetPooled(Box2 box)
{
var vertices = _vectorPool.Rent(4);
var normals = _vectorPool.Rent(4);
var centroid = box.Center;
vertices[0] = box.BottomLeft;
vertices[1] = box.BottomRight;
vertices[2] = box.TopRight;
vertices[3] = box.TopLeft;
normals[0] = new Vector2(0.0f, -1.0f);
normals[1] = new Vector2(1.0f, 0.0f);
normals[2] = new Vector2(0.0f, 1.0f);
normals[3] = new Vector2(-1.0f, 0.0f);
return new Polygon(vertices, normals, centroid);
}
internal Polygon GetPooled(Box2Rotated box)
{
var vertices = _vectorPool.Rent(4);
var normals = _vectorPool.Rent(4);
var centroid = box.Center;
vertices[0] = box.BottomLeft;
vertices[1] = box.BottomRight;
vertices[2] = box.TopRight;
vertices[3] = box.TopLeft;
var polygon = new Polygon(vertices, normals, centroid);
polygon.CalculateNormals(normals, 4);
return polygon;
}
internal void ReturnPooled(Polygon polygon)
{
_vectorPool.Return(polygon.Vertices);
_vectorPool.Return(polygon.Normals);
}
}

1
Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs
View File

@@ -24,7 +24,6 @@ namespace Robust.Shared.Physics.Systems
/*
* TODO:
* Raycasts for non-box shapes.
* TOI Solver (continuous collision detection)
* Poly cutting
*/