Re-implement DynamicTree<T> on top of B2DynamicTree<T>.

Robust.Client/GameObjects/EntitySystems/SpriteSystem.cs

@@ -31,17 +31,16 @@ namespace Robust.Client.GameObjects.EntitySystems
 
             var mapTree = renderTreeSystem.GetSpriteTreeForMap(currentMap);
 
-            var pvsEntities = mapTree.Query(pvsBounds, true);
-
-            foreach (var sprite in pvsEntities)
+            mapTree.QueryAabb(ref frameTime, (ref float state, in SpriteComponent value) =>
             {
-                if (sprite.IsInert)
+                if (value.IsInert)
                 {
-                    continue;
+                    return true;
                 }
 
-                sprite.FrameUpdate(frameTime);
-            }
+                value.FrameUpdate(state);
+                return true;
+            }, pvsBounds, approx: true);
         }
     }
 }

Robust.Client/Graphics/Clyde/Clyde.HLR.cs

@@ -10,6 +10,7 @@ using Robust.Client.Interfaces.Graphics;
 using Robust.Client.ResourceManagement;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics;
 using Robust.Shared.Utility;
 
 namespace Robust.Client.Graphics.Clyde
@@ -209,25 +210,27 @@ namespace Robust.Client.Graphics.Clyde
 
             var tree = spriteSystem.GetSpriteTreeForMap(map);
 
-            var sprites = tree.Query(worldBounds, true);
-
-            foreach (var sprite in sprites)
+            tree.QueryAabb(ref list, ((
+                ref RefList<(SpriteComponent sprite, Matrix3 matrix, Angle worldRot, float yWorldPos)> state,
+                in SpriteComponent value) =>
             {
-                if (sprite.ContainerOccluded || !sprite.Visible)
+                if (value.ContainerOccluded || !value.Visible)
                 {
-                    continue;
+                    return true;
                 }
 
-                var entity = sprite.Owner;
+                var entity = value.Owner;
                 var transform = entity.Transform;
 
-                ref var entry = ref list.AllocAdd();
-                entry.sprite = sprite;
+                ref var entry = ref state.AllocAdd();
+                entry.sprite = value;
                 entry.worldRot = transform.WorldRotation;
                 entry.matrix = transform.WorldMatrix;
                 var worldPos = entry.matrix.Transform(transform.LocalPosition);
                 entry.yWorldPos = worldPos.Y;
-            }
+                return true;
+
+            }), worldBounds, approx: true);
         }
 
         private void DrawSplash(IRenderHandle handle)

Robust.Client/Graphics/Clyde/Clyde.LightRendering.cs

@@ -486,8 +486,6 @@ namespace Robust.Client.Graphics.Clyde
         private ((PointLightComponent light, Vector2 pos)[] lights, int count, Box2 expandedBounds)
             GetLightsToRender(MapId map, in Box2 worldBounds)
         {
-            var count = 0;
-
             // When culling occluders later, we can't just remove any occluders outside the worldBounds.
             // As they could still affect the shadows of (large) light sources.
             // We expand the world bounds so that it encompasses the center of every light source.
@@ -498,37 +496,41 @@ namespace Robust.Client.Graphics.Clyde
             var renderingTreeSystem = _entitySystemManager.GetEntitySystem<RenderingTreeSystem>();
             var lightTree = renderingTreeSystem.GetLightTreeForMap(map);
 
-            foreach (var component in lightTree.Query(worldBounds))
+            var state = (this, expandedBounds, count: 0);
+
+            lightTree.QueryAabb(ref state, (ref (Clyde clyde, Box2 expandedBounds, int count) state, in PointLightComponent light) =>
             {
-                var transform = component.Owner.Transform;
+                var transform = light.Owner.Transform;
 
-                if (!component.Enabled || component.ContainerOccluded)
+                if (!light.Enabled || light.ContainerOccluded)
                 {
-                    continue;
+                    return true;
                 }
 
-                var lightPos = transform.WorldMatrix.Transform(component.Offset);
+                var lightPos = transform.WorldMatrix.Transform(light.Offset);
 
-                var circle = new Circle(lightPos, component.Radius);
+                var circle = new Circle(lightPos, light.Radius);
 
-                if (!circle.Intersects(worldBounds))
+                if (!circle.Intersects(state.expandedBounds))
                 {
-                    continue;
+                    return true;
                 }
 
-                _lightsToRenderList[count] = (component, lightPos);
-                count += 1;
+                state.clyde._lightsToRenderList[state.count] = (light, lightPos);
+                state.count += 1;
 
-                expandedBounds = expandedBounds.ExtendToContain(lightPos);
+                state.expandedBounds = state.expandedBounds.ExtendToContain(lightPos);
 
-                if (count == MaxLightsPerScene)
+                if (state.count == MaxLightsPerScene)
                 {
                     // TODO: Allow more than MaxLightsPerScene lights.
-                    break;
+                    return false;
                 }
-            }
 
-            return (_lightsToRenderList, count, expandedBounds);
+                return true;
+            }, expandedBounds);
+
+            return (_lightsToRenderList, state.count, state.expandedBounds);
         }
 
         private void BlurOntoWalls(Viewport viewport, IEye eye)
@@ -728,12 +730,12 @@ namespace Robust.Client.Graphics.Clyde
                 var ii = 0;
                 var imi = 0;
 
-                foreach (var occluder in occluderTree.Query(expandedBounds))
+                occluderTree.QueryAabb((in ClientOccluderComponent occluder) =>
                 {
                     var transform = occluder.Owner.Transform;
                     if (!occluder.Enabled)
                     {
-                        continue;
+                        return true;
                     }
 
                     var worldTransform = transform.WorldMatrix;
@@ -854,7 +856,9 @@ namespace Robust.Client.Graphics.Clyde
 
                     ai += 8;
                     ami += 4;
-                }
+
+                    return true;
+                }, expandedBounds);
 
                 _occlusionDataLength = ii;
                 _occlusionMaskDataLength = imi;

Robust.Shared/GameObjects/EntityManager.cs

@@ -169,15 +169,23 @@ namespace Robust.Shared.GameObjects
         /// <inheritdoc />
         public IEnumerable<IEntity> GetEntitiesAt(MapId mapId, Vector2 position, bool approximate = false)
         {
-            foreach (var entity in _entityTreesPerMap[mapId].Query(position, approximate))
+            var list = new List<IEntity>();
+
+            var state = (list, position);
+
+            _entityTreesPerMap[mapId].QueryPoint(ref state, (ref (List<IEntity> list, Vector2 position) state, in IEntity ent) =>
             {
-                var transform = entity.Transform;
-                if (MathHelper.CloseTo(transform.Coordinates.X, position.X) &&
-                    MathHelper.CloseTo(transform.Coordinates.Y, position.Y))
+                var transform = ent.Transform;
+                if (MathHelper.CloseTo(transform.Coordinates.X, state.position.X) &&
+                    MathHelper.CloseTo(transform.Coordinates.Y, state.position.Y))
                 {
-                    yield return entity;
+                    state.list.Add(ent);
                 }
-            }
+
+                return true;
+            }, position, approximate);
+
+            return list;
         }
 
         public IEnumerable<IEntity> GetEntities()
@@ -386,28 +394,42 @@ namespace Robust.Shared.GameObjects
         #region Spatial Queries
 
         /// <inheritdoc />
-        public bool AnyEntitiesIntersecting(MapId mapId, Box2 box, bool approximate = false) =>
-            _entityTreesPerMap[mapId].Query(box, approximate).Any(ent => !ent.Deleted);
+        public bool AnyEntitiesIntersecting(MapId mapId, Box2 box, bool approximate = false)
+        {
+            var found = false;
+            _entityTreesPerMap[mapId].QueryAabb(ref found, (ref bool found, in IEntity ent) =>
+            {
+                if (!ent.Deleted)
+                {
+                    found = true;
+                    return false;
+                }
+
+                return true;
+            }, box, approximate);
+            return found;
+        }
 
         /// <inheritdoc />
         public IEnumerable<IEntity> GetEntitiesIntersecting(MapId mapId, Box2 position, bool approximate = false)
         {
             if (mapId == MapId.Nullspace)
             {
-                yield break;
+                return Enumerable.Empty<IEntity>();
             }
 
-            var newResults = _entityTreesPerMap[mapId].Query(position, approximate); // .ToArray();
+            var list = new List<IEntity>();
 
-            foreach (var entity in newResults)
+            _entityTreesPerMap[mapId].QueryAabb(ref list, (ref List<IEntity> list, in IEntity ent) =>
             {
-                if (entity.Deleted)
+                if (!ent.Deleted)
                 {
-                    continue;
+                    list.Add(ent);
                 }
+                return true;
+            }, position, approximate);
 
-                yield return entity;
-            }
+            return list;
         }
 
         /// <inheritdoc />
@@ -418,19 +440,22 @@ namespace Robust.Shared.GameObjects
 
             if (mapId == MapId.Nullspace)
             {
-                yield break;
+                return Enumerable.Empty<IEntity>();
             }
 
-            var newResults = _entityTreesPerMap[mapId].Query(aabb, approximate);
+            var list = new List<IEntity>();
+            var state = (list, position);
 
-
-            foreach (var entity in newResults)
+            _entityTreesPerMap[mapId].QueryAabb(ref state, (ref (List<IEntity> list, Vector2 position) state, in IEntity ent) =>
             {
-                if (Intersecting(entity, position))
+                if (Intersecting(ent, state.position))
                 {
-                    yield return entity;
+                    state.list.Add(ent);
                 }
-            }
+                return true;
+            }, aabb, approximate);
+
+            return list;
         }
 
         /// <inheritdoc />
@@ -464,7 +489,7 @@ namespace Robust.Shared.GameObjects
             return Intersecting(entityTwo, position);
         }
 
-        private bool Intersecting(IEntity entity, Vector2 mapPosition)
+        private static bool Intersecting(IEntity entity, Vector2 mapPosition)
         {
             if (entity.TryGetComponent(out ICollidableComponent? component))
             {

Robust.Shared/Physics/BroadPhase.cs

@@ -29,10 +29,7 @@ namespace Robust.Shared.Physics {
 
         public bool Remove(IPhysBody item) => _tree.Remove(item);
 
-        public int Capacity {
-            get => _tree.Capacity;
-            set => _tree.Capacity = value;
-        }
+        public int Capacity => _tree.Capacity;
 
         public int Height => _tree.Height;
 
@@ -48,15 +45,46 @@ namespace Robust.Shared.Physics {
 
         public bool Update(in IPhysBody item) => _tree.Update(in item);
 
-        public IEnumerable<IPhysBody> Query(Box2 aabb, bool approx = false) => _tree.Query(aabb, approx);
+        public void QueryAabb(DynamicTree<IPhysBody>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
+        {
+            _tree.QueryAabb(callback, aabb, approx);
+        }
 
-        public IEnumerable<IPhysBody> Query(Vector2 point, bool approx = false) => _tree.Query(point, approx);
+        public void QueryAabb<TState>(ref TState state, DynamicTree<IPhysBody>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx = false)
+        {
+            _tree.QueryAabb(ref state, callback, aabb, approx);
+        }
 
-        public bool Query(DynamicTree<IPhysBody>.RayQueryCallbackDelegate callback, in Vector2 start, in Vector2 dir, bool approx = false) =>
-            _tree.Query(callback, in start, in dir, approx);
+        public IEnumerable<IPhysBody> QueryAabb(Box2 aabb, bool approx = false)
+        {
+            return _tree.QueryAabb(aabb, approx);
+        }
 
-        public IEnumerable<(IPhysBody A, IPhysBody B)> GetCollisions(bool approx = false) =>
-            _tree.GetCollisions(approx);
+        public void QueryPoint(DynamicTree<IPhysBody>.QueryCallbackDelegate callback, Vector2 point,
+            bool approx = false)
+        {
+            _tree.QueryPoint(callback, point, approx);
+        }
+
+        public void QueryPoint<TState>(ref TState state, DynamicTree<IPhysBody>.QueryCallbackDelegate<TState> callback,
+            Vector2 point, bool approx = false)
+        {
+            _tree.QueryPoint(ref state, callback, point, approx);
+        }
+
+        public IEnumerable<IPhysBody> QueryPoint(Vector2 point, bool approx = false)
+        {
+            return _tree.QueryPoint(point, approx);
+        }
+
+        public void QueryRay(DynamicTree<IPhysBody>.RayQueryCallbackDelegate callback, in Vector2 start, in Vector2 dir, bool approx = false) =>
+            _tree.QueryRay(callback, in start, in dir, approx);
+
+        public void QueryRay<TState>(ref TState state, DynamicTree<IPhysBody>.RayQueryCallbackDelegate<TState> callback, in Vector2 start, in Vector2 dir,
+            bool approx = false)
+        {
+            _tree.QueryRay(ref state, callback, start, dir, approx);
+        }
 
         public bool IsReadOnly => _tree.IsReadOnly;
 

Robust.Shared/Physics/DynamicTree.Node.cs

@@ -1,70 +0,0 @@
-/*
- * Initially based on Box2D by Erin Catto, license follows;
- *
- * Copyright (c) 2009 Erin Catto http://www.box2d.org
- *
- * This software is provided 'as-is', without any express or implied
- * warranty.  In no event will the authors be held liable for any damages
- * arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it
- * freely, subject to the following restrictions:
- * 1. The origin of this software must not be misrepresented; you must not
- * claim that you wrote the original software. If you use this software
- * in a product, an acknowledgment in the product documentation would be
- * appreciated but is not required.
- * 2. Altered source versions must be plainly marked as such, and must not be
- * misrepresented as being the original software.
- * 3. This notice may not be removed or altered from any source distribution.
- */
-
-using System.Diagnostics;
-using System.Runtime.CompilerServices;
-using Robust.Shared.Maths;
-
-namespace Robust.Shared.Physics
-{
-
-    public partial class DynamicTree<T>
-    {
-
-        public struct Node
-        {
-
-            public Box2 Aabb;
-
-            public Proxy Parent;
-
-            public Proxy Child1, Child2;
-
-            public int Height;
-
-            public T Item;
-
-            public bool IsLeaf
-            {
-                [MethodImpl(MethodImplOptions.AggressiveInlining)]
-                get => Child2 == Proxy.Free;
-            }
-
-            public bool IsFree
-            {
-                [MethodImpl(MethodImplOptions.AggressiveInlining)]
-                get => Height == -1;
-            }
-
-            public override string ToString()
-                => $@"Parent: {(Parent == Proxy.Free ? "None" : Parent.ToString())}, {
-                    (IsLeaf
-                        ? Height == 0
-                            ? $"Leaf: {Item}"
-                            : $"Leaf (invalid height of {Height}): {Item}"
-                        : IsFree
-                            ? "Free"
-                            : $"Branch at height {Height}, children: {Child1} and {Child2}")}";
-
-        }
-
-    }
-
-}

Robust.Shared/Physics/IBroadPhase.cs

@@ -10,7 +10,7 @@ namespace Robust.Shared.Physics {
 
     public interface IBroadPhase<T> : ICollection<T> where T : notnull {
 
-        int Capacity { get; set; }
+        int Capacity { get; }
 
         int Height {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -33,14 +33,42 @@ namespace Robust.Shared.Physics {
 
         bool Update(in T item);
 
-        IEnumerable<T> Query(Box2 aabb, bool approx = false);
+        void QueryAabb(
+            DynamicTree<T>.QueryCallbackDelegate callback,
+            Box2 aabb,
+            bool approx = false);
 
-        IEnumerable<T> Query(Vector2 point, bool approx = false);
+        void QueryAabb<TState>(
+            ref TState state,
+            DynamicTree<T>.QueryCallbackDelegate<TState> callback,
+            Box2 aabb,
+            bool approx = false);
 
-        bool Query(DynamicTree<T>.RayQueryCallbackDelegate callback, in Vector2 start, in Vector2 dir, bool approx = false);
+        IEnumerable<T> QueryAabb(Box2 aabb, bool approx = false);
 
-        IEnumerable<(T A,T B)> GetCollisions(bool approx = false);
+        void QueryPoint(DynamicTree<T>.QueryCallbackDelegate callback,
+            Vector2 point,
+            bool approx = false);
 
+        void QueryPoint<TState>(
+            ref TState state,
+            DynamicTree<T>.QueryCallbackDelegate<TState> callback,
+            Vector2 point,
+            bool approx = false);
+
+        IEnumerable<T> QueryPoint(Vector2 point, bool approx = false);
+
+        void QueryRay(
+            DynamicTree<T>.RayQueryCallbackDelegate callback,
+            in Vector2 start,
+            in Vector2 dir,
+            bool approx = false);
+
+        void QueryRay<TState>(
+            ref TState state,
+            DynamicTree<T>.RayQueryCallbackDelegate<TState> callback,
+            in Vector2 start,
+            in Vector2 dir,
+            bool approx = false);
     }
-
 }

Robust.Shared/Physics/PhysicsManager.cs

@@ -32,17 +32,22 @@ namespace Robust.Shared.Physics
         /// <returns></returns>
         public bool TryCollideRect(Box2 collider, MapId map)
         {
-            foreach (var body in this[map].Query(collider))
+            var state = (collider, map, found: false);
+            this[map].QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in IPhysBody body) =>
             {
                 if (!body.CanCollide || body.CollisionLayer == 0x0)
-                    continue;
-
-                if (body.MapID == map &&
-                    body.WorldAABB.Intersects(collider))
                     return true;
-            }
 
-            return false;
+                if (body.MapID == state.map &&
+                    body.WorldAABB.Intersects(state.collider))
+                {
+                    state.found = true;
+                    return false;
+                }
+                return true;
+            }, collider, true);
+
+            return state.found;
         }
 
         public bool IsWeightless(EntityCoordinates coordinates)
@@ -115,35 +120,47 @@ namespace Robust.Shared.Physics
         public IEnumerable<IEntity> GetCollidingEntities(IPhysBody physBody, Vector2 offset, bool approximate = true)
         {
             var modifiers = physBody.Entity.GetAllComponents<ICollideSpecial>();
-            foreach ( var body in this[physBody.MapID].Query(physBody.WorldAABB, approximate))
+            var entities = new List<IEntity>();
+
+            var state = (physBody, modifiers, entities);
+
+            this[physBody.MapID].QueryAabb(ref state,
+                (ref (IPhysBody physBody, IEnumerable<ICollideSpecial> modifiers, List<IEntity> entities) state,
+                    in IPhysBody body) =>
             {
                 if (body.Entity.Deleted) {
-                    continue;
+                    return true;
                 }
 
-                if (CollidesOnMask(physBody, body))
+                if (CollidesOnMask(state.physBody, body))
                 {
                     var preventCollision = false;
                     var otherModifiers = body.Entity.GetAllComponents<ICollideSpecial>();
-                    foreach (var modifier in modifiers)
+                    foreach (var modifier in state.modifiers)
                     {
                         preventCollision |= modifier.PreventCollide(body);
                     }
                     foreach (var modifier in otherModifiers)
                     {
-                        preventCollision |= modifier.PreventCollide(physBody);
+                        preventCollision |= modifier.PreventCollide(state.physBody);
                     }
 
-                    if (preventCollision) continue;
-                    yield return body.Entity;
+                    if (preventCollision)
+                    {
+                        return true;
+                    }
+                    state.entities.Add(body.Entity);
                 }
-            }
+                return true;
+            }, physBody.WorldAABB, approximate);
+
+            return entities;
         }
 
         /// <inheritdoc />
         public IEnumerable<IPhysBody> GetCollidingEntities(MapId mapId, in Box2 worldBox)
         {
-            return this[mapId].Query(worldBox, false);
+            return this[mapId].QueryAabb(worldBox, false);
         }
 
         public bool IsColliding(IPhysBody body, Vector2 offset, bool approximate)
@@ -244,7 +261,7 @@ namespace Robust.Shared.Physics
         {
             List<RayCastResults> results = new List<RayCastResults>();
 
-            this[mapId].Query((ref IPhysBody body, in Vector2 point, float distFromOrigin) =>
+            this[mapId].QueryRay((in IPhysBody body, in Vector2 point, float distFromOrigin) =>
             {
 
                 if (returnOnFirstHit && results.Count > 0) return true;
@@ -292,7 +309,7 @@ namespace Robust.Shared.Physics
         {
             var penetration = 0f;
 
-            this[mapId].Query((ref IPhysBody body, in Vector2 point, float distFromOrigin) =>
+            this[mapId].QueryRay((in IPhysBody body, in Vector2 point, float distFromOrigin) =>
             {
                 if (distFromOrigin > maxLength)
                 {

Robust.UnitTesting/Shared/Physics/DynamicTree_Test.cs

@@ -280,7 +280,7 @@ namespace Robust.UnitTesting.Shared.Physics
                 .Where(x => aabbs1[x].Contains(point))
                 .OrderBy(x => x).ToArray();
 
-            var results = dt.Query(point)
+            var results = dt.QueryPoint(point)
                 .OrderBy(x => x).ToArray();
 
             Assert.Multiple(() =>