Physics (#1602)

* Physics worlds

* Paul's a good boy

* Build working

* Ingame and not lagging to hell

* Why didn't you commit ahhhhh

* Hard collisions working

* Solver parity

* Decent broadphase work done

* BroadPhase outline done

* BroadPhase working

* waiting for pvs

* Fix static PVS AABB

* Stop static bodies from awakening

* Optimise a bunch of stuff

* Even more broadphase stuff

* I'm fucking stupid

* Optimise fixture updates

* Collision solver start

* Building

* A is for Argumentative

* Fix contact caching island flags

* Circle shapes actually workeded

* Damping

* DS2 consumables only

* Slightly more stable

* Even slightlier more stablier

* VV your heart out

* Initial joint support

* 90% of joints I just wanted to push as I'd scream if I lost progress

* JOINT PURGATORY

* Joints barely functional lmao

* Okay these joints slightly more functional

* Remove station FrictionJoint

* Also that

* Some Box2D ports

* Cleanup mass

* Edge shape

* Active contacts

* Fix active contacts

* Optimise active contacts even more

* Boxes be stacking

* I would die for smug oh my fucking god

* In which everything is fixed

* Distance joints working LETS GO

* Remove frequency on distancejoint

* Fix some stuff and break joints

* Crashing fixed mehbeh

* ICollideSpecial and more resilience

* auto-clear

* showbb vera

* Slap that TODO in there

* Fix restartround crash

* Random fixes

* Fix fixture networking

* Add intersection method for broadphase

* Fix contacts

* Licenses done

* Optimisations

* Fix wall clips

* Config caching for island

* allocations optimisations

* Optimise casts

* Optimise events queue for physics

* Contact manager optimisations

* Optimise controllers

* Sloth joint or something idk

* Controller graph

* Remove content cvar

* Random cleanup

* Finally remove VirtualController

* Manifold structs again

* Optimise this absolute retardation

* Optimise

* fix license

* Cleanup physics interface

* AHHHHHHHHHHHHH

* Fix collisions again

* snivybus

* Fix potential nasty manifold bug

* Tests go snivy

Co-authored-by: Metal Gear Sloth <metalgearsloth@gmail.com>

.github/CODEOWNERS

@@ -1,14 +1,7 @@
-# Lines starting with '#' are comments.
-# Each line is a file pattern followed by one or more owners.
+# Last match in file takes precedence.
 
-# These owners will be the default owners for everything in the repo.
-# * @defunkt
-* @Acruid @PJB3005 @Silvertorch5
+# Be they Fluent translations or Freemarker templates, I know them both!
+*.ftl @RemieRichards
 
-# Order is important. The last matching pattern has the most precedence.
-# So if a pull request only touches javascript files, only these owners
-# will be requested to review.
-# *.js    @octocat @github/js
-
-# You can also use email addresses if you prefer.
-# docs/*  docs@example.com
+# Ping for all PRs
+* @Acruid @PJB3005 @Silvertorch5

Resources/Prototypes/dummy_entity.yml

@@ -0,0 +1,4 @@
+- type: entity
+  name: blank entity
+  id: BlankEntity
+  abstract: true

Robust.Client/Audio/Midi/MidiManager.cs

@@ -13,6 +13,7 @@ using Robust.Shared.Log;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
+using Robust.Shared.Physics.Broadphase;
 using Robust.Shared.Utility;
 using Logger = Robust.Shared.Log.Logger;
 
@@ -71,6 +72,8 @@ namespace Robust.Client.Audio.Midi
         [Dependency] private readonly IResourceManagerInternal _resourceManager = default!;
         [Dependency] private readonly IEntityManager _entityManager = default!;
 
+        private SharedBroadPhaseSystem _broadPhaseSystem = default!;
+
         public bool IsAvailable
         {
             get
@@ -154,6 +157,7 @@ namespace Robust.Client.Audio.Midi
             _midiThread = new Thread(ThreadUpdate);
             _midiThread.Start();
 
+            _broadPhaseSystem = EntitySystem.Get<SharedBroadPhaseSystem>();
             FluidsynthInitialized = true;
         }
 
@@ -298,7 +302,7 @@ namespace Robust.Client.Audio.Midi
                             var occlusion = 0f;
                             if (sourceRelative.Length > 0)
                             {
-                                occlusion = IoCManager.Resolve<IPhysicsManager>().IntersectRayPenetration(
+                                occlusion = _broadPhaseSystem.IntersectRayPenetration(
                                     pos.MapId,
                                     new CollisionRay(
                                         pos.Position,

Robust.Client/Console/Commands/PhysicsOverlayCommands.cs

@@ -0,0 +1,41 @@
+using Robust.Client.Debugging;
+using Robust.Shared.Console;
+using Robust.Shared.GameObjects;
+
+namespace Robust.Client.Console.Commands
+{
+    public sealed class PhysicsOverlayCommands : IConsoleCommand
+    {
+        public string Command => "physics";
+        public string Description => $"{Command} <contactnormals / contactpoints / shapes>";
+        public string Help => $"{Command} <overlay>";
+        public void Execute(IConsoleShell shell, string argStr, string[] args)
+        {
+            if (args.Length != 1)
+            {
+                shell.WriteLine($"Invalid number of args supplied");
+                return;
+            }
+
+            var system = EntitySystem.Get<DebugPhysicsSystem>();
+
+            switch (args[0])
+            {
+                case "contactnormals":
+                    system.Flags ^= PhysicsDebugFlags.ContactNormals;
+                    break;
+                case "contactpoints":
+                    system.Flags ^= PhysicsDebugFlags.ContactPoints;
+                    break;
+                case "shapes":
+                    system.Flags ^= PhysicsDebugFlags.Shapes;
+                    break;
+                default:
+                    shell.WriteLine($"{args[0]} is not a recognised overlay");
+                    return;
+            }
+
+            return;
+        }
+    }
+}

Robust.Client/Console/Commands/VelocitiesCommand.cs

@@ -0,0 +1,17 @@
+using Robust.Client.GameObjects;
+using Robust.Shared.Console;
+using Robust.Shared.GameObjects;
+
+namespace Robust.Client.Console.Commands
+{
+    public class VelocitiesCommand : IConsoleCommand
+    {
+        public string Command => "showvelocities";
+        public string Description => "Displays your angular and linear velocities";
+        public string Help => $"{Command}";
+        public void Execute(IConsoleShell shell, string argStr, string[] args)
+        {
+            EntitySystem.Get<VelocityDebugSystem>().Enabled ^= true;
+        }
+    }
+}

Robust.Client/Debugging/DebugDrawing.cs

@@ -3,10 +3,13 @@ using System.Collections.Generic;
 using Robust.Client.Graphics;
 using Robust.Client.Input;
 using Robust.Client.ResourceManagement;
+using Robust.Shared;
+using Robust.Shared.Configuration;
 using Robust.Shared.GameObjects;
 using Robust.Shared.IoC;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
+using Robust.Shared.Physics.Broadphase;
 using Robust.Shared.Prototypes;
 
 namespace Robust.Client.Debugging
@@ -136,7 +139,7 @@ namespace Robust.Client.Debugging
                     row++;
                     DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Mask: {Convert.ToString(body.CollisionMask, 2)}");
                     row++;
-                    DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Enabled: {body.CanCollide}, Hard: {body.Hard}, Anchored: {((IPhysicsComponent)body).Anchored}");
+                    DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Enabled: {body.CanCollide}, Hard: {body.Hard}, Anchored: {(body).BodyType == BodyType.Static}");
                     row++;
                 }
 
@@ -158,19 +161,22 @@ namespace Robust.Client.Debugging
 
                 var mapId = _eyeManager.CurrentMap;
 
-                foreach (var physBody in _physicsManager.GetCollidingEntities(mapId, viewport))
+                foreach (var physBody in EntitySystem.Get<SharedBroadPhaseSystem>().GetCollidingEntities(mapId, viewport))
                 {
                     // all entities have a TransformComponent
                     var transform = physBody.Entity.Transform;
 
-                    var worldBox = physBody.WorldAABB;
+                    var worldBox = physBody.GetWorldAABB();
                     if (worldBox.IsEmpty()) continue;
 
                     var colorEdge = Color.Red.WithAlpha(0.33f);
+                    var sleepThreshold = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.TimeToSleep);
 
-                    foreach (var shape in physBody.PhysicsShapes)
+                    foreach (var fixture in physBody.Fixtures)
                     {
-                        shape.DebugDraw(drawing, transform.WorldMatrix, in viewport, physBody.SleepAccumulator / (float) physBody.SleepThreshold);
+                        var shape = fixture.Shape;
+                        var sleepPercent = physBody.Awake ? physBody.SleepTime / sleepThreshold : 1.0f;
+                        shape.DebugDraw(drawing, transform.WorldMatrix, in viewport,  sleepPercent);
                     }
 
                     if (worldBox.Contains(mouseWorldPos))
@@ -233,6 +239,16 @@ namespace Robust.Client.Debugging
                     _handle.DrawCircle(origin, radius, color);
                 }
 
+                public override void DrawPolygonShape(Vector2[] vertices, in Color color)
+                {
+                    _handle.DrawPrimitives(DrawPrimitiveTopology.TriangleFan, vertices, color);
+                }
+
+                public override void DrawLine(Vector2 start, Vector2 end, in Color color)
+                {
+                    _handle.DrawLine(start, end, color);
+                }
+
                 public override void SetTransform(in Matrix3 transform)
                 {
                     _handle.SetTransform(transform);

Robust.Client/Debugging/DebugPhysicsSystem.cs

@@ -0,0 +1,149 @@
+using System;
+using Robust.Client.Graphics;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Collision;
+using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Physics.Dynamics.Contacts;
+
+// TODO: Copy farseer licence here coz it's heavily inspired by it.
+namespace Robust.Client.Debugging
+{
+    internal sealed class DebugPhysicsSystem : EntitySystem
+    {
+        /*
+         * Used for debugging shapes, controllers, joints, contacts
+         */
+
+        private const int MaxContactPoints = 2048;
+        internal int PointCount;
+
+        internal ContactPoint[] _points = new ContactPoint[MaxContactPoints];
+
+        public PhysicsDebugFlags Flags
+        {
+            get => _flags;
+            set
+            {
+                if (value == _flags) return;
+
+                if (_flags == PhysicsDebugFlags.None)
+                    IoCManager.Resolve<IOverlayManager>().AddOverlay(new PhysicsDebugOverlay(this));
+
+                if (value == PhysicsDebugFlags.None)
+                    IoCManager.Resolve<IOverlayManager>().RemoveOverlay(nameof(PhysicsDebugOverlay));
+
+                _flags = value;
+            }
+        }
+
+        private PhysicsDebugFlags _flags;
+
+        public override void Initialize()
+        {
+            base.Initialize();
+            SubscribeLocalEvent<PreSolveMessage>(HandlePreSolve);
+        }
+
+        private void HandlePreSolve(PreSolveMessage message)
+        {
+            Contact contact = message.Contact;
+            Manifold oldManifold = message.OldManifold;
+
+            if ((Flags & PhysicsDebugFlags.ContactPoints) != 0)
+            {
+                Manifold manifold = contact.Manifold;
+
+                if (manifold.PointCount == 0)
+                    return;
+
+                Fixture fixtureA = contact.FixtureA!;
+
+                PointState[] state1, state2;
+                CollisionManager.GetPointStates(out state1, out state2, oldManifold, manifold);
+
+                Vector2[] points;
+                Vector2 normal;
+                contact.GetWorldManifold(out normal, out points);
+
+                for (int i = 0; i < manifold.PointCount && PointCount < MaxContactPoints; ++i)
+                {
+                    if (fixtureA == null)
+                        _points[i] = new ContactPoint();
+
+                    ContactPoint cp = _points[PointCount];
+                    cp.Position = points[i];
+                    cp.Normal = normal;
+                    cp.State = state2[i];
+                    _points[PointCount] = cp;
+                    ++PointCount;
+                }
+            }
+        }
+
+        internal struct ContactPoint
+        {
+            public Vector2 Normal;
+            public Vector2 Position;
+            public PointState State;
+        }
+    }
+
+    [Flags]
+    internal enum PhysicsDebugFlags : byte
+    {
+        None = 0,
+        ContactPoints = 1 << 0,
+        ContactNormals = 1 << 1,
+        Shapes = 1 << 2,
+    }
+
+    internal sealed class PhysicsDebugOverlay : Overlay
+    {
+        private DebugPhysicsSystem _physics = default!;
+
+        public override OverlaySpace Space => OverlaySpace.WorldSpace;
+
+        public PhysicsDebugOverlay(DebugPhysicsSystem system) : base(nameof(PhysicsDebugOverlay))
+        {
+            _physics = system;
+        }
+
+        protected override void Draw(DrawingHandleBase handle, OverlaySpace currentSpace)
+        {
+            if (_physics.Flags == PhysicsDebugFlags.None) return;
+
+            var worldHandle = (DrawingHandleWorld) handle;
+
+            if ((_physics.Flags & PhysicsDebugFlags.Shapes) != 0)
+            {
+                // Port DebugDrawing over.
+            }
+
+            if ((_physics.Flags & PhysicsDebugFlags.ContactPoints) != 0)
+            {
+                const float axisScale = 0.3f;
+
+                for (int i = 0; i < _physics.PointCount; ++i)
+                {
+                    DebugPhysicsSystem.ContactPoint point = _physics._points[i];
+
+                    if (point.State == PointState.Add)
+                        worldHandle.DrawCircle(point.Position, 0.5f, new Color(255, 77, 243, 77));
+                    else if (point.State == PointState.Persist)
+                        worldHandle.DrawCircle(point.Position, 0.5f, new Color(255, 77, 77, 77));
+
+                    if ((_physics.Flags & PhysicsDebugFlags.ContactNormals) != 0)
+                    {
+                        Vector2 p1 = point.Position;
+                        Vector2 p2 = p1 + point.Normal * axisScale;
+                        worldHandle.DrawLine(p1, p2, new Color(255, 102, 230, 102));
+                    }
+                }
+
+                _physics.PointCount = 0;
+            }
+        }
+    }
+}

Robust.Client/GameObjects/ClientComponentFactory.cs

@@ -23,7 +23,7 @@ namespace Robust.Client.GameObjects
 
             Register<PhysicsComponent>();
             RegisterReference<PhysicsComponent, IPhysBody>();
-            RegisterReference<PhysicsComponent, IPhysicsComponent>();
+
             RegisterIgnore("KeyBindingInput");
 
             Register<InputComponent>();

Robust.Client/GameObjects/Components/Appearance/AppearanceComponent.cs

@@ -116,7 +116,7 @@ namespace Robust.Client.GameObjects
                 _didRegisterSerializer = true;
             }
 
-            serializer.DataField(ref Visualizers, "visuals", new List<AppearanceVisualizer>());
+            serializer.DataFieldCached(ref Visualizers, "visuals", new List<AppearanceVisualizer>());
         }
 
         public override void Initialize()

Robust.Client/GameObjects/Components/Containers/ContainerManagerComponent.ClientContainer.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using Robust.Server.GameObjects;
 using Robust.Shared.GameObjects;
 using Robust.Shared.ViewVariables;
 
@@ -62,14 +63,14 @@ namespace Robust.Client.GameObjects
             public void DoInsert(IEntity entity)
             {
                 Entities.Add(entity);
-
+                Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntInsertedIntoContainerMessage(entity, this));
                 Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new UpdateContainerOcclusionMessage(entity));
             }
 
             public void DoRemove(IEntity entity)
             {
                 Entities.Remove(entity);
-
+                Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntRemovedFromContainerMessage(entity, this));
                 Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new UpdateContainerOcclusionMessage(entity));
             }
 

Robust.Client/GameObjects/Components/Renderable/ISpriteComponent.cs

@@ -222,6 +222,12 @@ namespace Robust.Client.GameObjects
         ISpriteLayer this[object layerKey] { get; }
 
         IEnumerable<ISpriteLayer> AllLayers { get; }
+
         int GetLayerDirectionCount(ISpriteLayer layer);
+
+        /// <summary>
+        ///     Calculate sprite bounding box in world-space coordinates.
+        /// </summary>
+        Box2 CalculateBoundingBox();
     }
 }

Robust.Client/GameObjects/Components/Renderable/ISpriteLayer.cs

@@ -26,5 +26,17 @@ namespace Robust.Client.GameObjects
         RSI.State.Direction EffectiveDirection(Angle worldRotation);
 
         Vector2 LocalToLayer(Vector2 localPos);
+
+        /// <summary>
+        ///     Layer size in pixels.
+        ///     Don't account layer scale or sprite world transform.
+        /// </summary>
+        Vector2i PixelSize { get; }
+
+        /// <summary>
+        ///     Calculate layer bounding box in sprite local-space coordinates.
+        /// </summary>
+        /// <returns>Bounding box in sprite local-space coordinates.</returns>
+        Box2 CalculateBoundingBox();
     }
 }

Robust.Client/GameObjects/Components/Renderable/SpriteComponent.cs

@@ -1604,6 +1604,39 @@ namespace Robust.Client.GameObjects
             return builder.ToString();
         }
 
+        /// <inheritdoc/>
+        public Box2 CalculateBoundingBox()
+        {
+            // fast check for empty sprites
+            if (Layers.Count == 0)
+                return new Box2();
+
+            // we need to calculate bounding box taking into account all nested layers
+            // because layers can have offsets, scale or rotation we need to calculate a new BB
+            // based on lowest bottomLeft and hightest topRight points from all layers
+            var box = Layers[0].CalculateBoundingBox();
+
+            for (int i = 1; i < Layers.Count; i++)
+            {
+                var layer = Layers[i];
+                var layerBB = layer.CalculateBoundingBox();
+
+                box = box.Union(layerBB);
+            }
+
+            // apply sprite transformations and calculate sprite bounding box
+            // we can optimize it a bit, if sprite doesn't have rotation
+            var spriteBox = box.Scale(Scale);
+            var spriteHasRotation = !Rotation.EqualsApprox(Angle.Zero);
+            var spriteBB = spriteHasRotation ?
+                new Box2Rotated(spriteBox, Rotation).CalcBoundingBox() : spriteBox;
+
+            // move it all to world transform system (with sprite offset)
+            var worldPosition = Owner.Transform.WorldPosition;
+            var worldBB = spriteBB.Translated(Offset + worldPosition);
+            return worldBB;
+        }
+
         /// <summary>
         ///     Enum to "offset" a cardinal direction.
         /// </summary>
@@ -1889,6 +1922,33 @@ namespace Robust.Client.GameObjects
             {
                 Offset = offset;
             }
+
+            /// <inheritdoc/>
+            public Vector2i PixelSize
+            {
+                get
+                {
+                    var pixelSize = Vector2i.Zero;
+                    if (Texture != null)
+                    {
+                        pixelSize = Texture.Size;
+                    }
+                    else if (ActualRsi != null)
+                    {
+                        pixelSize = ActualRsi.Size;
+                    }
+
+                    return pixelSize;
+                }
+            }
+
+            /// <inheritdoc/>
+            public Box2 CalculateBoundingBox()
+            {
+                // TODO: scale & rotation for layers is currently unimplemented.
+                return Box2.CenteredAround(Offset, PixelSize / EyeManager.PixelsPerMeter);
+            }
+
         }
 
         void IAnimationProperties.SetAnimatableProperty(string name, object value)

Robust.Client/GameObjects/EntitySystems/AudioSystem.cs

@@ -11,6 +11,7 @@ using Robust.Shared.Log;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
+using Robust.Shared.Physics.Broadphase;
 using Robust.Shared.Utility;
 
 namespace Robust.Client.GameObjects
@@ -24,6 +25,8 @@ namespace Robust.Client.GameObjects
         [Dependency] private readonly IEyeManager _eyeManager = default!;
         [Dependency] private readonly IEntityManager _entityManager = default!;
 
+        private SharedBroadPhaseSystem _broadPhaseSystem = default!;
+
         private readonly List<PlayingStream> _playingClydeStreams = new();
 
         public int OcclusionCollisionMask;
@@ -31,10 +34,13 @@ namespace Robust.Client.GameObjects
         /// <inheritdoc />
         public override void Initialize()
         {
+            base.Initialize();
             SubscribeNetworkEvent<PlayAudioEntityMessage>(PlayAudioEntityHandler);
             SubscribeNetworkEvent<PlayAudioGlobalMessage>(PlayAudioGlobalHandler);
             SubscribeNetworkEvent<PlayAudioPositionalMessage>(PlayAudioPositionalHandler);
             SubscribeNetworkEvent<StopAudioMessageClient>(StopAudioMessageHandler);
+
+            _broadPhaseSystem = Get<SharedBroadPhaseSystem>();
         }
 
         private void StopAudioMessageHandler(StopAudioMessageClient ev)
@@ -141,7 +147,7 @@ namespace Robust.Client.GameObjects
                             var occlusion = 0f;
                             if (sourceRelative.Length > 0)
                             {
-                                occlusion = IoCManager.Resolve<IPhysicsManager>().IntersectRayPenetration(
+                                occlusion = _broadPhaseSystem.IntersectRayPenetration(
                                     pos.MapId,
                                     new CollisionRay(
                                         pos.Position,

Robust.Client/GameObjects/EntitySystems/VelocityDebugSystem.cs

@@ -0,0 +1,51 @@
+using Robust.Client.Graphics;
+using Robust.Client.Player;
+using Robust.Client.UserInterface;
+using Robust.Client.UserInterface.Controls;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Maths;
+
+namespace Robust.Client.GameObjects
+{
+    public class VelocityDebugSystem : EntitySystem
+    {
+        [Dependency] private readonly IEyeManager _eyeManager = default!;
+        [Dependency] private readonly IPlayerManager _playerManager = default!;
+
+        internal bool Enabled { get; set; }
+
+        private Label _label = default!;
+
+        public override void Initialize()
+        {
+            base.Initialize();
+            _label = new Label();
+            IoCManager.Resolve<IUserInterfaceManager>().StateRoot.AddChild(_label);
+        }
+
+        public override void FrameUpdate(float frameTime)
+        {
+            base.FrameUpdate(frameTime);
+            if (!Enabled)
+            {
+                _label.Visible = false;
+                return;
+            }
+
+            var player = _playerManager.LocalPlayer?.ControlledEntity;
+
+            if (player == null || !player.TryGetComponent(out PhysicsComponent? body))
+            {
+                _label.Visible = false;
+                return;
+            }
+
+            var screenPos = _eyeManager.WorldToScreen(player.Transform.WorldPosition);
+            LayoutContainer.SetPosition(_label, screenPos + new Vector2(0, 50));
+            _label.Visible = true;
+
+            _label.Text = $"Speed: {body.LinearVelocity.Length}\nLinear: {body.LinearVelocity.X:0.00}, {body.LinearVelocity.Y:0.00}\nAngular:{body.AngularVelocity}";
+        }
+    }
+}

Robust.Client/GameStates/NetInterpOverlay.cs

@@ -29,7 +29,7 @@ namespace Robust.Client.GameStates
             handle.UseShader(_shader);
             var worldHandle = (DrawingHandleWorld) handle;
             var viewport = _eyeManager.GetWorldViewport();
-            foreach (var boundingBox in _componentManager.EntityQuery<IPhysicsComponent>(true))
+            foreach (var boundingBox in _componentManager.EntityQuery<IPhysBody>(true))
             {
                 // all entities have a TransformComponent
                 var transform = ((IComponent)boundingBox).Owner.Transform;
@@ -42,7 +42,7 @@ namespace Robust.Client.GameStates
                 if(transform.LerpDestination == null)
                     continue;
 
-                var aabb = ((IPhysBody)boundingBox).AABB;
+                var aabb = boundingBox.GetWorldAABB();
 
                 // if not on screen, or too small, continue
                 if (!aabb.Translated(transform.WorldPosition).Intersects(viewport) || aabb.IsEmpty())

Robust.Client/Graphics/Clyde/Audio/Clyde.Audio.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.IO;
@@ -36,6 +36,9 @@ namespace Robust.Client.Graphics.Clyde
         private readonly ConcurrentQueue<(int sourceHandle, int filterHandle)> _bufferedSourceDisposeQueue = new();
         private readonly ConcurrentQueue<int> _bufferDisposeQueue = new();
 
+        // The base gain value for a listener, used to boost the default volume.
+        private const float _baseGain = 2f;
+
         public bool HasAlDeviceExtension(string extension) => _alcDeviceExtensions.Contains(extension);
         public bool HasAlContextExtension(string extension) => _alContextExtensions.Contains(extension);
 
@@ -182,7 +185,7 @@ namespace Robust.Client.Graphics.Clyde
 
         public void SetMasterVolume(float newVolume)
         {
-            AL.Listener(ALListenerf.Gain, newVolume);
+            AL.Listener(ALListenerf.Gain, _baseGain * newVolume);
         }
 
         public IClydeAudioSource CreateAudioSource(AudioStream stream)

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

@@ -183,27 +183,61 @@ namespace Robust.Client.Graphics.Clyde
                     break;
                 }
 
+
+                RenderTexture? entityPostRenderTarget = null;
                 Vector2i roundedPos = default;
                 if (entry.sprite.PostShader != null)
                 {
-                    _renderHandle.UseRenderTarget(EntityPostRenderTarget);
-                    _renderHandle.Clear(new Color());
-                    // Calculate viewport so that the entity thinks it's drawing to the same position,
-                    // which is necessary for light application,
-                    // but it's ACTUALLY drawing into the center of the render target.
-                    var spritePos = entry.sprite.Owner.Transform.WorldPosition;
-                    var screenPos = _eyeManager.WorldToScreen(spritePos);
-                    var (roundedX, roundedY) = roundedPos = (Vector2i) screenPos;
-                    var flippedPos = new Vector2i(roundedX, screenSize.Y - roundedY);
-                    flippedPos -= EntityPostRenderTarget.Size / 2;
-                    _renderHandle.Viewport(Box2i.FromDimensions(-flippedPos, screenSize));
+                    // calculate world bounding box
+                    var spriteBB = entry.sprite.CalculateBoundingBox();
+                    var spriteLB = spriteBB.BottomLeft;
+                    var spriteRT = spriteBB.TopRight;
+
+                    // finally we can calculate screen bounding in pixels
+                    var screenLB = _eyeManager.WorldToScreen(spriteLB);
+                    var screenRT = _eyeManager.WorldToScreen(spriteRT);
+
+                    // we need to scale RT a for effects like emission or highlight
+                    // scale can be passed with PostShader as variable in future
+                    var postShadeScale = 1.25f;
+                    var screenSpriteSize = (Vector2i)((screenRT - screenLB) * postShadeScale).Rounded();
+                    screenSpriteSize.Y = -screenSpriteSize.Y;
+
+                    // I'm not 100% sure why it works, but without it post-shader
+                    // can be lower or upper by 1px than original sprite depending on sprite rotation or scale
+                    // probably some rotation rounding error
+                    if (screenSpriteSize.X % 2 != 0)
+                        screenSpriteSize.X++;
+                    if (screenSpriteSize.Y % 2 != 0)
+                        screenSpriteSize.Y++;
+
+                    // check that sprite size is valid
+                    if (screenSpriteSize.X > 0 && screenSpriteSize.Y > 0)
+                    {
+                        // create new render texture with correct sprite size
+                        entityPostRenderTarget = CreateRenderTarget(screenSpriteSize,
+                            new RenderTargetFormatParameters(RenderTargetColorFormat.Rgba8Srgb, true),
+                            name: nameof(entityPostRenderTarget));
+                        _renderHandle.UseRenderTarget(entityPostRenderTarget);
+                        _renderHandle.Clear(new Color());
+
+                        // Calculate viewport so that the entity thinks it's drawing to the same position,
+                        // which is necessary for light application,
+                        // but it's ACTUALLY drawing into the center of the render target.
+                        var spritePos = spriteBB.Center;
+                        var screenPos = _eyeManager.WorldToScreen(spritePos);
+                        var (roundedX, roundedY) = roundedPos = (Vector2i)screenPos;
+                        var flippedPos = new Vector2i(roundedX, screenSize.Y - roundedY);
+                        flippedPos -= entityPostRenderTarget.Size / 2;
+                        _renderHandle.Viewport(Box2i.FromDimensions(-flippedPos, screenSize));
+                    }
                 }
 
                 var matrix = entry.worldMatrix;
                 var worldPosition = new Vector2(matrix.R0C2, matrix.R1C2);
                 entry.sprite.Render(_renderHandle.DrawingHandleWorld, in entry.worldRotation, in worldPosition);
 
-                if (entry.sprite.PostShader != null)
+                if (entry.sprite.PostShader != null && entityPostRenderTarget != null)
                 {
                     var oldProj = _currentMatrixProj;
                     var oldView = _currentMatrixView;
@@ -216,11 +250,11 @@ namespace Robust.Client.Graphics.Clyde
                     _renderHandle.SetProjView(proj, view);
                     _renderHandle.SetModelTransform(Matrix3.Identity);
 
-                    var rounded = roundedPos - EntityPostRenderTarget.Size / 2;
+                    var rounded = roundedPos - entityPostRenderTarget.Size / 2;
 
-                    var box = Box2i.FromDimensions(rounded, EntityPostRenderTarget.Size);
+                    var box = Box2i.FromDimensions(rounded, entityPostRenderTarget.Size);
 
-                    _renderHandle.DrawTextureScreen(EntityPostRenderTarget.Texture,
+                    _renderHandle.DrawTextureScreen(entityPostRenderTarget.Texture,
                         box.BottomLeft, box.BottomRight, box.TopLeft, box.TopRight,
                         Color.White, null);
 

Robust.Client/Graphics/Clyde/Clyde.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Globalization;
@@ -39,8 +39,6 @@ namespace Robust.Client.Graphics.Clyde
         private GLUniformBuffer<ProjViewMatrices> ProjViewUBO = default!;
         private GLUniformBuffer<UniformConstants> UniformConstantsUBO = default!;
 
-        private RenderTexture EntityPostRenderTarget = default!;
-
         private GLBuffer BatchVBO = default!;
         private GLBuffer BatchEBO = default!;
         private GLHandle BatchVAO;
@@ -316,10 +314,6 @@ namespace Robust.Client.Graphics.Clyde
             ProjViewUBO = new GLUniformBuffer<ProjViewMatrices>(this, BindingIndexProjView, nameof(ProjViewUBO));
             UniformConstantsUBO = new GLUniformBuffer<UniformConstants>(this, BindingIndexUniformConstants, nameof(UniformConstantsUBO));
 
-            EntityPostRenderTarget = CreateRenderTarget(Vector2i.One * 8 * EyeManager.PixelsPerMeter,
-                new RenderTargetFormatParameters(RenderTargetColorFormat.Rgba8Srgb, true),
-                name: nameof(EntityPostRenderTarget));
-
             CreateMainViewport();
         }
 

Robust.Client/Physics/BroadPhaseSystem.cs

@@ -0,0 +1,13 @@
+using Robust.Shared.Physics.Broadphase;
+
+namespace Robust.Client.Physics
+{
+    internal sealed class BroadPhaseSystem : SharedBroadPhaseSystem
+    {
+        public override void Initialize()
+        {
+            base.Initialize();
+            UpdatesBefore.Add(typeof(PhysicsSystem));
+        }
+    }
+}

Robust.Client/Physics/DebugPhysicsIslandSystem.cs

@@ -0,0 +1,135 @@
+using System;
+using System.Collections.Generic;
+using Robust.Client.Graphics;
+using Robust.Shared.Console;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics;
+using Robust.Shared.Timing;
+
+namespace Robust.Client.Physics
+{
+    internal sealed class PhysicsIslandCommand : IConsoleCommand
+    {
+        public string Command => "showislands";
+        public string Description => "Shows the current physics bodies involved in each physics island.";
+        public string Help => "showislands";
+        public void Execute(IConsoleShell shell, string argStr, string[] args)
+        {
+            if (args.Length != 0)
+            {
+                shell.WriteLine("This command doesn't take args!");
+                return;
+            }
+
+            EntitySystem.Get<DebugPhysicsIslandSystem>().Mode ^= DebugPhysicsIslandMode.Solve;
+        }
+    }
+
+    internal sealed class DebugPhysicsIslandSystem : EntitySystem
+    {
+        [Dependency] private readonly IGameTiming _gameTiming = default!;
+
+        public DebugPhysicsIslandMode Mode { get; set; } = DebugPhysicsIslandMode.None;
+
+        /*
+         * Island solve debug:
+         * This will draw above every body involved in a particular island solve.
+         */
+
+        public readonly Queue<(TimeSpan Time, List<IPhysBody> Bodies)> IslandSolve = new();
+        public const float SolveDuration = 0.1f;
+
+        public override void Initialize()
+        {
+            base.Initialize();
+            SubscribeLocalEvent<IslandSolveMessage>(HandleIslandSolveMessage);
+            IoCManager.Resolve<IOverlayManager>().AddOverlay(new PhysicsIslandOverlay());
+        }
+
+        public override void FrameUpdate(float frameTime)
+        {
+            base.FrameUpdate(frameTime);
+            while (IslandSolve.TryPeek(out var solve))
+            {
+                if (solve.Time.TotalSeconds + SolveDuration > _gameTiming.CurTime.TotalSeconds)
+                {
+                    IslandSolve.Dequeue();
+                }
+                else
+                {
+                    break;
+                }
+            }
+        }
+
+        public override void Shutdown()
+        {
+            base.Shutdown();
+            IoCManager.Resolve<IOverlayManager>().RemoveOverlay(nameof(PhysicsIslandOverlay));
+        }
+
+        private void HandleIslandSolveMessage(IslandSolveMessage message)
+        {
+            if ((Mode & DebugPhysicsIslandMode.Solve) == 0x0) return;
+            IslandSolve.Enqueue((_gameTiming.CurTime, message.Bodies));
+        }
+    }
+
+    [Flags]
+    internal enum DebugPhysicsIslandMode : ushort
+    {
+        None = 0,
+        Solve = 1 << 0,
+        Contacts = 1 << 1,
+    }
+
+    internal sealed class PhysicsIslandOverlay : Overlay
+    {
+        [Dependency] private readonly IEyeManager _eyeManager = default!;
+        [Dependency] private readonly IGameTiming _gameTiming = default!;
+
+        private DebugPhysicsIslandSystem _islandSystem = default!;
+
+        public override OverlaySpace Space => OverlaySpace.WorldSpace;
+
+        public PhysicsIslandOverlay() : base(nameof(PhysicsIslandOverlay))
+        {
+            _islandSystem = EntitySystem.Get<DebugPhysicsIslandSystem>();
+            _eyeManager = IoCManager.Resolve<IEyeManager>();
+            _gameTiming = IoCManager.Resolve<IGameTiming>();
+        }
+
+        protected override void Draw(DrawingHandleBase handle, OverlaySpace currentSpace)
+        {
+            var worldHandle = (DrawingHandleWorld) handle;
+
+            DrawIslandSolve(worldHandle);
+        }
+
+        private void DrawIslandSolve(DrawingHandleWorld handle)
+        {
+            if ((_islandSystem.Mode & DebugPhysicsIslandMode.Solve) == 0x0) return;
+
+            var viewport = _eyeManager.GetWorldViewport();
+
+            foreach (var solve in _islandSystem.IslandSolve)
+            {
+                var ratio = (float) Math.Max(
+                    (solve.Time.TotalSeconds + DebugPhysicsIslandSystem.SolveDuration -
+                     _gameTiming.CurTime.TotalSeconds) / DebugPhysicsIslandSystem.SolveDuration, 0.0f);
+
+                if (ratio <= 0.0f) continue;
+
+                foreach (var body in solve.Bodies)
+                {
+                    var worldAABB = body.GetWorldAABB();
+                    if (!viewport.Intersects(worldAABB)) continue;
+
+                    handle.DrawRect(worldAABB, Color.Green.WithAlpha(ratio * 0.5f));
+                }
+            }
+        }
+    }
+}

Robust.Client/Placement/PlacementManager.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using Robust.Client.ResourceManagement;
@@ -21,6 +21,7 @@ using Robust.Shared.Reflection;
 using Robust.Shared.Utility;
 using Robust.Shared.Serialization;
 using Robust.Shared.Timing;
+using Robust.Shared.Log;
 
 namespace Robust.Client.Placement
 {
@@ -87,7 +88,17 @@ namespace Robust.Client.Placement
         public bool Eraser { get; private set; }
 
         /// <summary>
-        ///     The texture we use to show from our placement manager to represent the entity to place
+        /// Holds the selection rectangle for the eraser
+        /// </summary>
+        public Box2? EraserRect { get; set; }
+
+        /// <summary>
+        /// Drawing shader for drawing without being affected by lighting
+        /// </summary>
+        private ShaderInstance? _drawingShader { get; set; }
+
+        /// <summary>
+        /// The texture we use to show from our placement manager to represent the entity to place
         /// </summary>
         public List<IDirectionalTextureProvider>? CurrentTextures { get; set; }
 
@@ -153,6 +164,8 @@ namespace Robust.Client.Placement
 
         public void Initialize()
         {
+            _drawingShader = _prototypeManager.Index<ShaderPrototype>("unshaded").Instance();
+
             NetworkManager.RegisterNetMessage<MsgPlacement>(MsgPlacement.NAME, HandlePlacementMessage);
 
             _modeDictionary.Clear();
@@ -182,7 +195,17 @@ namespace Robust.Client.Placement
                 .Bind(EngineKeyFunctions.EditorGridPlace, InputCmdHandler.FromDelegate(
                     session =>
                     {
-                        if (IsActive && !Eraser) ActivateGridMode();
+                        if (IsActive)
+                        {
+                            if (Eraser)
+                            {
+                                EraseRectMode();
+                            }
+                            else
+                            {
+                                ActivateGridMode();
+                            }
+                        }
                     }))
                 .Bind(EngineKeyFunctions.EditorPlaceObject, new PointerStateInputCmdHandler(
                     (session, coords, uid) =>
@@ -190,6 +213,13 @@ namespace Robust.Client.Placement
                         if (!IsActive)
                             return false;
 
+                        if (EraserRect.HasValue)
+                        {
+                            HandleRectDeletion(StartPoint, EraserRect.Value);
+                            EraserRect = null;
+                            return true;
+                        }
+
                         if (Eraser)
                         {
                             if (HandleDeletion(coords))
@@ -308,6 +338,7 @@ namespace Robust.Client.Placement
             _placenextframe = false;
             IsActive = false;
             Eraser = false;
+            EraserRect = null;
             PlacementOffset = Vector2i.Zero;
         }
 
@@ -384,6 +415,15 @@ namespace Robust.Client.Placement
             NetworkManager.ClientSendMessage(msg);
         }
 
+        public void HandleRectDeletion(EntityCoordinates start, Box2 rect)
+        {
+            var msg = NetworkManager.CreateNetMessage<MsgPlacement>();
+            msg.PlaceType = PlacementManagerMessage.RequestRectRemove;
+            msg.EntityCoordinates = new EntityCoordinates(StartPoint.EntityId, rect.BottomLeft);
+            msg.RectSize = rect.Size;
+            NetworkManager.ClientSendMessage(msg);
+        }
+
         public void ToggleEraser()
         {
             if (!Eraser && !IsActive)
@@ -459,11 +499,62 @@ namespace Robust.Client.Placement
             return true;
         }
 
+        private bool CurrentEraserMouseCoordinates(out EntityCoordinates coordinates)
+        {
+            var ent = PlayerManager.LocalPlayer?.ControlledEntity;
+            if (ent == null)
+            {
+                coordinates = new EntityCoordinates();
+                return false;
+            }
+            else
+            {
+                var map = ent.Transform.MapID;
+                if (map == MapId.Nullspace || !Eraser)
+                {
+                    coordinates = new EntityCoordinates();
+                    return false;
+                }
+                coordinates = EntityCoordinates.FromMap(ent.EntityManager, MapManager,
+                    eyeManager.ScreenToMap(new ScreenCoordinates(_inputManager.MouseScreenPosition)));
+                return true;
+            }
+        }
+
         /// <inheritdoc />
         public void FrameUpdate(FrameEventArgs e)
         {
             if (!CurrentMousePosition(out var mouseScreen))
+            {
+                if (EraserRect.HasValue)
+                {
+                    if (!CurrentEraserMouseCoordinates(out EntityCoordinates end))
+                        return;
+                    float b, l, t, r;
+                    if (StartPoint.X < end.X)
+                    {
+                        l = StartPoint.X;
+                        r = end.X;
+                    }
+                    else
+                    {
+                        l = end.X;
+                        r = StartPoint.X;
+                    }
+                    if (StartPoint.Y < end.Y)
+                    {
+                        b = StartPoint.Y;
+                        t = end.Y;
+                    }
+                    else
+                    {
+                        b = end.Y;
+                        t = StartPoint.Y;
+                    }
+                    EraserRect = new Box2(l, b, r, t);
+                }
                 return;
+            }
 
             CurrentMode!.AlignPlacementMode(mouseScreen);
 
@@ -501,6 +592,15 @@ namespace Robust.Client.Placement
             PlacementType = PlacementTypes.Grid;
         }
 
+        private void EraseRectMode()
+        {
+            if (!CurrentEraserMouseCoordinates(out EntityCoordinates coordinates))
+                return;
+
+            StartPoint = coordinates;
+            EraserRect = new Box2(coordinates.Position, Vector2.Zero);
+        }
+
         private bool DeactivateSpecialPlacement()
         {
             if (PlacementType == PlacementTypes.None)
@@ -513,7 +613,14 @@ namespace Robust.Client.Placement
         private void Render(DrawingHandleWorld handle)
         {
             if (CurrentMode == null || !IsActive)
+            {
+                if (EraserRect.HasValue)
+                {
+                    handle.UseShader(_drawingShader);
+                    handle.DrawRect(EraserRect.Value, new Color(255, 0, 0, 50));
+                }
                 return;
+            }
 
             CurrentMode.Render(handle);
 

Robust.Client/Placement/PlacementMode.cs

@@ -4,8 +4,10 @@ using System.Diagnostics.CodeAnalysis;
 using System.Linq;
 using Robust.Client.Graphics;
 using Robust.Client.ResourceManagement;
+using Robust.Shared.GameObjects;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics.Broadphase;
 using Robust.Shared.Utility;
 
 namespace Robust.Client.Placement
@@ -230,7 +232,7 @@ namespace Robust.Client.Placement
                 bounds.Width,
                 bounds.Height);
 
-            return pManager.PhysicsManager.TryCollideRect(collisionBox, mapCoords.MapId);
+            return EntitySystem.Get<SharedBroadPhaseSystem>().TryCollideRect(collisionBox, mapCoords.MapId);
         }
 
         protected Vector2 ScreenToWorld(Vector2 point)

Robust.Client/Player/IPlayerManager.cs

@@ -1,13 +1,13 @@
-using System;
+using System;
 using System.Collections.Generic;
 using Robust.Shared.GameStates;
 using Robust.Shared.Network;
 
 namespace Robust.Client.Player
 {
-    public interface IPlayerManager
+    public interface IPlayerManager : Shared.Players.ISharedPlayerManager
     {
-        IEnumerable<IPlayerSession> Sessions { get; }
+        new IEnumerable<IPlayerSession> Sessions { get; }
         IReadOnlyDictionary<NetUserId, IPlayerSession> SessionsDict { get; }
 
         LocalPlayer? LocalPlayer { get; }
@@ -17,8 +17,6 @@ namespace Robust.Client.Player
         /// </summary>
         event Action<LocalPlayerChangedEventArgs>? LocalPlayerChanged;
 
-        int PlayerCount { get; }
-        int MaxPlayers { get; }
         event EventHandler PlayerListUpdated;
 
         void Initialize();

Robust.Client/Player/IPlayerSession.cs

@@ -1,20 +1,11 @@
-using Robust.Shared.Players;
+using System;
+using Robust.Shared.Players;
 
 namespace Robust.Client.Player
 {
     /// <summary>
-    ///     Client side session of a player.
+    /// Client side session of a player.
     /// </summary>
-    public interface IPlayerSession : ICommonSession
-    {
-        /// <summary>
-        ///     Current name of this player.
-        /// </summary>
-        new string Name { get; set; }
-
-        /// <summary>
-        ///     Current connection latency of this session from the server to their client.
-        /// </summary>
-        short Ping { get; set; }
-    }
+    [Obsolete("Use the base " + nameof(ICommonSession))]
+    public interface IPlayerSession : ICommonSession { }
 }

Robust.Client/Player/PlayerManager.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using Robust.Shared.Configuration;
@@ -8,6 +8,7 @@ using Robust.Shared.GameStates;
 using Robust.Shared.IoC;
 using Robust.Shared.Network;
 using Robust.Shared.Network.Messages;
+using Robust.Shared.Players;
 using Robust.Shared.Utility;
 using Robust.Shared.ViewVariables;
 
@@ -30,6 +31,21 @@ namespace Robust.Client.Player
         /// </summary>
         private readonly Dictionary<NetUserId, IPlayerSession> _sessions = new();
 
+        /// <inheritdoc />
+        public IEnumerable<ICommonSession> NetworkedSessions
+        {
+            get
+            {
+                if (LocalPlayer is not null)
+                    return new[] {LocalPlayer.Session};
+
+                return Enumerable.Empty<ICommonSession>();
+            }
+        }
+
+        /// <inheritdoc />
+        IEnumerable<ICommonSession> ISharedPlayerManager.Sessions => _sessions.Values;
+
         /// <inheritdoc />
         public int PlayerCount => _sessions.Values.Count;
 
@@ -52,9 +68,9 @@ namespace Robust.Client.Player
         private LocalPlayer? _localPlayer;
         public event Action<LocalPlayerChangedEventArgs>? LocalPlayerChanged;
 
-
         /// <inheritdoc />
-        [ViewVariables] public IEnumerable<IPlayerSession> Sessions => _sessions.Values;
+        [ViewVariables]
+        IEnumerable<IPlayerSession> IPlayerManager.Sessions => _sessions.Values;
 
         /// <inheritdoc />
         public IReadOnlyDictionary<NetUserId, IPlayerSession> SessionsDict => _sessions;

Robust.Client/Player/PlayerSession.cs

@@ -1,25 +1,50 @@
-using Robust.Shared.Enums;
+using Robust.Shared.Enums;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Network;
+using Robust.Shared.Players;
 
 namespace Robust.Client.Player
 {
-
     internal sealed class PlayerSession : IPlayerSession
     {
         /// <inheritdoc />
-        public SessionStatus Status { get; set; } = SessionStatus.Connecting;
+        internal SessionStatus Status { get; set; } = SessionStatus.Connecting;
 
+        /// <inheritdoc />
+        SessionStatus ICommonSession.Status
+        {
+            get => this.Status;
+            set => this.Status = value;
+        }
+
+        /// <inheritdoc />
         public IEntity? AttachedEntity { get; set; }
 
+        /// <inheritdoc />
+        public EntityUid? AttachedEntityUid => AttachedEntity?.Uid;
+
         /// <inheritdoc />
         public NetUserId UserId { get; }
 
         /// <inheritdoc cref="IPlayerSession" />
-        public string Name { get; set; } = "<Unknown>";
+        internal string Name { get; set; } = "<Unknown>";
+
+        /// <inheritdoc cref="IPlayerSession" />
+        string ICommonSession.Name
+        {
+            get => this.Name;
+            set => this.Name = value;
+        }
 
         /// <inheritdoc />
-        public short Ping { get; set; }
+        internal short Ping { get; set; }
+
+        /// <inheritdoc />
+        short ICommonSession.Ping
+        {
+            get => this.Ping;
+            set => this.Ping = value;
+        }
 
         /// <summary>
         ///     Creates an instance of a PlayerSession.

Robust.Client/Robust.Client.csproj

@@ -11,13 +11,13 @@
   </PropertyGroup>
   <Import Project="..\MSBuild\Robust.DefineConstants.targets" />
   <ItemGroup>
-    <PackageReference Include="DiscordRichPresence" Version="1.0.166" />
-    <PackageReference Include="JetBrains.Annotations" Version="2020.1.0" PrivateAssets="All" />
+    <PackageReference Include="DiscordRichPresence" Version="1.0.175" />
+    <PackageReference Include="JetBrains.Annotations" Version="2020.3.0" PrivateAssets="All" />
     <PackageReference Include="nfluidsynth" Version="0.3.1" />
+    <PackageReference Include="NJsonSchema" Version="10.3.8" />
     <PackageReference Include="NVorbis" Version="0.10.1" />
     <PackageReference Include="Newtonsoft.Json" Version="12.0.3" />
-    <PackageReference Include="NJsonSchema" Version="10.3.1" Condition="'$(Configuration)' == 'Debug'" />
-    <PackageReference Include="SixLabors.ImageSharp" Version="1.0.2" />
+    <PackageReference Include="SixLabors.ImageSharp" Version="1.0.3" />
     <PackageReference Include="OpenToolkit.Graphics" Version="4.0.0-pre9.1" />
     <PackageReference Include="OpenToolkit.OpenAL" Version="4.0.0-pre9.1" />
     <PackageReference Include="SpaceWizards.SharpFont" Version="1.0.1" />

Robust.Client/UserInterface/Controls/ItemList.cs

@@ -473,6 +473,8 @@ namespace Robust.Client.UserInterface.Controls
 
             _scrollBar.ValueTarget -= _getScrollSpeed() * args.Delta.Y;
             _isAtBottom = _scrollBar.IsAtEnd;
+
+            args.Handle();
         }
 
         [Pure]

Robust.Client/UserInterface/Controls/ScrollBar.cs

@@ -101,12 +101,12 @@ namespace Robust.Client.UserInterface.Controls
             }
 
             var box = _getGrabberBox();
-            if (!box.Contains(args.RelativePosition))
+            if (!box.Contains(args.RelativePixelPosition))
             {
                 return;
             }
 
-            _grabData = (args.RelativePosition, Value);
+            _grabData = (args.RelativePixelPosition, Value);
             _updatePseudoClass();
             args.Handle();
         }

Robust.Client/UserInterface/Controls/ScrollContainer.cs

@@ -17,6 +17,9 @@ namespace Robust.Client.UserInterface.Controls
 
         private bool _suppressScrollValueChanged;
 
+        public int ScrollSpeedX { get; set; } = 50;
+        public int ScrollSpeedY { get; set; } = 50;
+
         public ScrollContainer()
         {
             MouseFilter = MouseFilterMode.Pass;
@@ -197,13 +200,15 @@ namespace Robust.Client.UserInterface.Controls
 
             if (_vScrollEnabled)
             {
-                _vScrollBar.ValueTarget -= args.Delta.Y * 50;
+                _vScrollBar.ValueTarget -= args.Delta.Y * ScrollSpeedY;
             }
 
             if (_hScrollEnabled)
             {
-                _hScrollBar.ValueTarget += args.Delta.X * 50;
+                _hScrollBar.ValueTarget += args.Delta.X * ScrollSpeedX;
             }
+
+            args.Handle();
         }
 
         protected override void ChildAdded(Control newChild)

Robust.Client/UserInterface/CustomControls/BaseWindow.cs

@@ -228,7 +228,7 @@ namespace Robust.Client.UserInterface.CustomControls
                 // An explaination: The BadOpenGLVersionWindow was showing up off the top-left corner of the screen.
                 // Basically, if OpenCentered happens super-early, RootControl doesn't get time to layout children.
                 // But we know that this is always going to be one of the roots anyway for now.
-                LayoutContainer.SetPosition(this, (UserInterfaceManager.RootControl.Size - Size) / 2);
+                LayoutContainer.SetPosition(this, (UserInterfaceManager.RootControl.Size - SetSize) / 2);
                 _firstTimeOpened = false;
             }
             else
@@ -244,7 +244,7 @@ namespace Robust.Client.UserInterface.CustomControls
                 Measure(Vector2.Infinity);
                 SetSize = DesiredSize;
                 Open();
-                LayoutContainer.SetPosition(this, (0, (Parent!.Size.Y - Size.Y) / 2));
+                LayoutContainer.SetPosition(this, (0, (Parent!.Size.Y - DesiredSize.Y) / 2));
                 _firstTimeOpened = false;
             }
             else

Robust.Client/UserInterface/CustomControls/EntitySpawnWindow.cs

@@ -148,13 +148,6 @@ namespace Robust.Client.UserInterface.CustomControls
             SearchBar.GrabKeyboardFocus();
         }
 
-        public override void Close()
-        {
-            base.Close();
-
-            Dispose();
-        }
-
         protected override void Dispose(bool disposing)
         {
             base.Dispose(disposing);
@@ -201,6 +194,7 @@ namespace Robust.Client.UserInterface.CustomControls
         private void OnEraseButtonToggled(BaseButton.ButtonToggledEventArgs args)
         {
             placementManager.ToggleEraser();
+            OverrideMenu.Disabled = args.Pressed;
         }
 
         private void BuildEntityList(string? searchStr = null)
@@ -510,6 +504,7 @@ namespace Robust.Client.UserInterface.CustomControls
             }
 
             EraseButton.Pressed = false;
+            OverrideMenu.Disabled = false;
         }
 
         private class DoNotMeasure : Control

Robust.Client/ViewVariables/ViewVariablesManager.cs

@@ -229,7 +229,11 @@ namespace Robust.Client.ViewVariables
 
         public async void OpenVV(ViewVariablesObjectSelector selector)
         {
-            var window = new SS14Window {Title = "View Variables"};
+            var window = new SS14Window
+            {
+                Title = "View Variables",
+                SetSize = _defaultWindowSize
+            };
             var loadingLabel = new Label {Text = "Retrieving remote object data from server..."};
             window.Contents.AddChild(loadingLabel);
 

Robust.Server/Console/ServerConsoleHost.cs

@@ -159,7 +159,7 @@ namespace Robust.Server.Console
                 _systemConsole.Print(text + "\n");
         }
 
-        private static string FormatPlayerString(IBaseSession? session)
+        private static string FormatPlayerString(ICommonSession? session)
         {
             return session != null ? $"{session.Name}" : "[HOST]";
         }

Robust.Server/Debugging/DebugDrawingManager.cs

@@ -1,4 +1,4 @@
-using System.Diagnostics;
+using System.Diagnostics;
 using Robust.Shared.IoC;
 using Robust.Shared.Network;
 using Robust.Shared.Network.Messages;
@@ -14,7 +14,7 @@ namespace Robust.Server.Debugging
         public void Initialize()
         {
             _net.RegisterNetMessage<MsgRay>(MsgRay.NAME);
-            _physics.DebugDrawRay += data => PhysicsOnDebugDrawRay(data);
+            // TODO _physics.DebugDrawRay += data => PhysicsOnDebugDrawRay(data);
         }
 
         [Conditional("DEBUG")]

Robust.Server/GameObjects/EntitySystems/TileLookup/GridTileLookupSystem.cs

@@ -6,6 +6,7 @@ using Robust.Shared.GameObjects;
 using Robust.Shared.IoC;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics;
 
 namespace Robust.Server.GameObjects
 {
@@ -198,8 +199,8 @@ namespace Robust.Server.GameObjects
         private Box2 GetEntityBox(IEntity entity)
         {
             // Need to clip the aabb as anything with an edge intersecting another tile might be picked up, such as walls.
-            if (entity.TryGetComponent(out IPhysicsComponent? physics))
-                return new Box2(physics.WorldAABB.BottomLeft + 0.01f, physics.WorldAABB.TopRight - 0.01f);
+            if (entity.TryGetComponent(out IPhysBody? physics))
+                return new Box2(physics.GetWorldAABB().BottomLeft + 0.01f, physics.GetWorldAABB().TopRight - 0.01f);
 
             // Don't want to accidentally get neighboring tiles unless we're near an edge
             return Box2.CenteredAround(entity.Transform.Coordinates.ToMapPos(EntityManager), Vector2.One / 2);

Robust.Server/GameObjects/ServerComponentFactory.cs

@@ -1,4 +1,5 @@
-using Robust.Shared.GameObjects;
+using Robust.Shared.GameObjects;
+using Robust.Shared.Physics;
 
 namespace Robust.Server.GameObjects
 {
@@ -27,7 +28,8 @@ namespace Robust.Server.GameObjects
             RegisterReference<BasicActorComponent, IActorComponent>();
 
             Register<PhysicsComponent>();
-            RegisterReference<PhysicsComponent, IPhysicsComponent>();
+            RegisterReference<PhysicsComponent, IPhysBody>();
+
             Register<OccluderComponent>();
 
             RegisterIgnore("Input");

Robust.Server/GameObjects/ServerEntityManager.cs

@@ -9,6 +9,7 @@ using Robust.Shared.Configuration;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics;
 using Robust.Shared.Prototypes;
 using Robust.Shared.Timing;
 using Robust.Shared.Utility;
@@ -367,7 +368,7 @@ namespace Robust.Server.GameObjects
                     continue;
                 }
 
-                if (entity.TryGetComponent(out IPhysicsComponent? body))
+                if (entity.TryGetComponent(out IPhysBody? body))
                 {
                     if (body.LinearVelocity.EqualsApprox(Vector2.Zero, MinimumMotionForMovers))
                     {
@@ -534,7 +535,7 @@ namespace Robust.Server.GameObjects
                     continue;
                 }
 
-                if (!entity.TryGetComponent(out IPhysicsComponent? body))
+                if (!entity.TryGetComponent(out IPhysBody? body))
                 {
                     // can't be a mover w/o physics
                     continue;
@@ -788,7 +789,7 @@ namespace Robust.Server.GameObjects
                     {
                         addToMovers = true;
                     }
-                    else if (entity.TryGetComponent(out IPhysicsComponent? physics)
+                    else if (entity.TryGetComponent(out IPhysBody? physics)
                              && physics.LastModifiedTick >= currentTick)
                     {
                         addToMovers = true;

Robust.Server/Physics/BroadPhaseSystem.cs

@@ -0,0 +1,14 @@
+using Robust.Server.GameObjects;
+using Robust.Shared.Physics.Broadphase;
+
+namespace Robust.Server.Physics
+{
+    internal sealed class BroadPhaseSystem : SharedBroadPhaseSystem
+    {
+        public override void Initialize()
+        {
+            base.Initialize();
+            UpdatesBefore.Add(typeof(PhysicsSystem));
+        }
+    }
+}

Robust.Server/Placement/PlacementManager.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using Robust.Shared.IoC;
 using Robust.Shared.Map;
 using System.Collections.Generic;
@@ -59,6 +59,9 @@ namespace Robust.Server.Placement
                 case PlacementManagerMessage.RequestEntRemove:
                     HandleEntRemoveReq(msg.EntityUid);
                     break;
+                case PlacementManagerMessage.RequestRectRemove:
+                    HandleRectRemoveReq(msg);
+                    break;
             }
         }
 
@@ -202,6 +205,19 @@ namespace Robust.Server.Placement
                 _entityManager.DeleteEntity(entity);
         }
 
+        private void HandleRectRemoveReq(MsgPlacement msg)
+        {
+            EntityCoordinates start = msg.EntityCoordinates;
+            Vector2 rectSize = msg.RectSize;
+            foreach (IEntity entity in _entityManager.GetEntitiesIntersecting(start.GetMapId(_entityManager),
+                new Box2(start.Position, start.Position + rectSize)))
+            {
+                if (entity.Deleted || entity.HasComponent<IMapGridComponent>() || entity.HasComponent<IActorComponent>())
+                    continue;
+                entity.Delete();
+            }
+        }
+
         /// <summary>
         ///  Places mob in entity placement mode with given settings.
         /// </summary>

Robust.Server/Player/IPlayerManager.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Diagnostics.CodeAnalysis;
 using Robust.Shared.Enums;
@@ -13,21 +13,10 @@ namespace Robust.Server.Player
     /// <summary>
     ///     Manages each players session when connected to the server.
     /// </summary>
-    public interface IPlayerManager
+    public interface IPlayerManager : Shared.Players.ISharedPlayerManager
     {
-        /// <summary>
-        ///     Number of players currently connected to this server.
-        ///     Fetching this is thread safe.
-        /// </summary>
-        int PlayerCount { get; }
-
         BoundKeyMap KeyMap { get; }
 
-        /// <summary>
-        ///     Maximum number of players that can connect to this server at one time.
-        /// </summary>
-        int MaxPlayers { get; }
-
         /// <summary>
         ///     Raised when the <see cref="SessionStatus" /> of a <see cref="IPlayerSession" /> is changed.
         /// </summary>

Robust.Server/Player/IPlayerSession.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Network;
 using Robust.Shared.Players;
@@ -7,7 +7,6 @@ namespace Robust.Server.Player
 {
     public interface IPlayerSession : ICommonSession
     {
-        EntityUid? AttachedEntityUid { get; }
         INetChannel ConnectedClient { get; }
         DateTime ConnectedTime { get; }
 

Robust.Server/Player/PlayerManager.cs

@@ -14,6 +14,7 @@ using Robust.Shared.IoC;
 using Robust.Shared.Map;
 using Robust.Shared.Network;
 using Robust.Shared.Network.Messages;
+using Robust.Shared.Players;
 using Robust.Shared.Reflection;
 using Robust.Shared.Timing;
 using Robust.Shared.Utility;
@@ -53,6 +54,11 @@ namespace Robust.Server.Player
         [ViewVariables]
         private readonly Dictionary<string, NetUserId> _userIdMap = new();
 
+        /// <inheritdoc />
+        public IEnumerable<ICommonSession> NetworkedSessions => _sessions.Values;
+
+        /// <inheritdoc />
+        public IEnumerable<ICommonSession> Sessions => _sessions.Values;
 
         /// <inheritdoc />
         [ViewVariables]

Robust.Server/Player/PlayerSession.cs

@@ -1,9 +1,10 @@
-using Robust.Shared.GameStates;
+using Robust.Shared.GameStates;
 using Robust.Server.GameObjects;
 using System;
 using Robust.Shared.Enums;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Network;
+using Robust.Shared.Players;
 using Robust.Shared.ViewVariables;
 
 namespace Robust.Server.Player
@@ -42,11 +43,32 @@ namespace Robust.Server.Player
         private SessionStatus _status = SessionStatus.Connecting;
 
         /// <inheritdoc />
-        public string Name { get; }
+        
+        [ViewVariables]
+        internal string Name { get; set; }
 
         /// <inheritdoc />
+        string ICommonSession.Name
+        {
+            get => this.Name;
+            set => this.Name = value;
+        }
+
         [ViewVariables]
-        public SessionStatus Status
+        internal short Ping
+        {
+            get => ConnectedClient.Ping;
+            set => throw new NotSupportedException();
+        }
+
+        short ICommonSession.Ping
+        {
+            get => this.Ping;
+            set => this.Ping = value;
+        }
+
+        [ViewVariables]
+        internal SessionStatus Status
         {
             get => _status;
             set
@@ -62,6 +84,13 @@ namespace Robust.Server.Player
             }
         }
 
+        /// <inheritdoc />
+        SessionStatus ICommonSession.Status
+        {
+            get => this.Status;
+            set => this.Status = value;
+        }
+
         /// <inheritdoc />
         public DateTime ConnectedTime { get; private set; }
 

Robust.Server/Robust.Server.csproj

@@ -10,9 +10,9 @@
   </PropertyGroup>
   <Import Project="..\MSBuild\Robust.DefineConstants.targets" />
   <ItemGroup>
-    <PackageReference Include="JetBrains.Annotations" Version="2020.1.0" PrivateAssets="All" />
-    <PackageReference Include="Microsoft.Data.Sqlite" Version="5.0.0" />
-    <PackageReference Include="prometheus-net" Version="4.0.0" />
+    <PackageReference Include="JetBrains.Annotations" Version="2020.3.0" PrivateAssets="All" />
+    <PackageReference Include="Microsoft.Data.Sqlite" Version="5.0.3" />
+    <PackageReference Include="prometheus-net" Version="4.1.1" />
     <PackageReference Include="Serilog.Sinks.Loki" Version="3.0.0" />
     <PackageReference Include="Microsoft.Extensions.Primitives" Version="5.0.0" />
   </ItemGroup>

Robust.Shared.Maths/Box2.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 
@@ -118,6 +118,12 @@ namespace Robust.Shared.Maths
             return FromDimensions(center - size / 2, size);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Box2 CentredAroundZero(Vector2 size)
+        {
+            return FromDimensions(-size / 2, size);
+        }
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public readonly bool Intersects(in Box2 other)
         {
@@ -236,6 +242,16 @@ namespace Robust.Shared.Maths
                 center + halfSize);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public readonly Box2 Scale(Vector2 scale)
+        {
+            var center = Center;
+            var halfSize = (Size / 2) * scale;
+            return new Box2(
+                center - halfSize,
+                center + halfSize);
+        }
+
         /// <summary>Returns a Box2 translated by the given amount.</summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public readonly Box2 Translated(Vector2 point)

Robust.Shared.Maths/Robust.Shared.Maths.csproj

@@ -9,7 +9,7 @@
   </PropertyGroup>
   <Import Project="..\MSBuild\Robust.DefineConstants.targets" />
   <ItemGroup>
-    <PackageReference Include="JetBrains.Annotations" Version="2020.1.0" PrivateAssets="All" />
+    <PackageReference Include="JetBrains.Annotations" Version="2020.3.0" PrivateAssets="All" />
     <PackageReference Include="Newtonsoft.Json" Version="12.0.3" />
     <PackageReference Condition="'$(TargetFramework)' == 'net472'" Include="System.Runtime.CompilerServices.Unsafe" Version="4.5.2" />
   </ItemGroup>

Robust.Shared.Maths/Vector2.cs

@@ -43,6 +43,11 @@ namespace Robust.Shared.Maths
 
         public static readonly Vector2 Infinity = new(float.PositiveInfinity, float.PositiveInfinity);
 
+        /// <summary>
+        ///     A vector with NaN X and Y.
+        /// </summary>
+        public static readonly Vector2 NaN = new(float.NaN, float.NaN);
+
         /// <summary>
         ///     Construct a vector from its coordinates.
         /// </summary>
@@ -90,7 +95,7 @@ namespace Robust.Shared.Maths
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public readonly Vector2 Rounded()
         {
-            return new((float) MathF.Round(X), (float) MathF.Round(Y));
+            return new(MathF.Round(X), MathF.Round(Y));
         }
 
         /// <summary>

Robust.Shared.Scripting/Robust.Shared.Scripting.csproj

@@ -13,7 +13,7 @@
     <PackageReference Include="Microsoft.CodeAnalysis.CSharp.Features" Version="3.8.0" />
     <PackageReference Include="Microsoft.CodeAnalysis.CSharp.Scripting" Version="3.8.0" />
     <PackageReference Include="Microsoft.CodeAnalysis.CSharp.Workspaces" Version="3.8.0" />
-    <PackageReference Include="JetBrains.Annotations" Version="2020.1.0" PrivateAssets="All" />
+    <PackageReference Include="JetBrains.Annotations" Version="2020.3.0" PrivateAssets="All" />
   </ItemGroup>
 
   <ItemGroup>

Robust.Shared/CVars.cs

@@ -280,6 +280,105 @@ namespace Robust.Shared
         public static readonly CVarDef<string> PlayerName =
             CVarDef.Create("player.name", "JoeGenero", CVar.ARCHIVE | CVar.CLIENTONLY);
 
+        /*
+         * PHYSICS
+         */
+
+        // - Sleep
+        public static readonly CVarDef<float> AngularSleepTolerance =
+            CVarDef.Create("physics.angsleeptol", 2.0f / 180.0f * MathF.PI);
+
+        public static readonly CVarDef<float> LinearSleepTolerance =
+            CVarDef.Create("physics.linsleeptol", 0.001f);
+
+        public static readonly CVarDef<bool> SleepAllowed =
+            CVarDef.Create("physics.sleepallowed", true);
+
+        // Box2D default is 0.5f
+        public static readonly CVarDef<float> TimeToSleep =
+            CVarDef.Create("physics.timetosleep", 0.50f);
+
+        // - Solver
+        // These are the minimum recommended by Box2D with the standard being 8 velocity 3 position iterations.
+        // Trade-off is obviously performance vs how long it takes to stabilise.
+        public static readonly CVarDef<int> PositionIterations =
+            CVarDef.Create("physics.positer", 3);
+
+        public static readonly CVarDef<int> VelocityIterations =
+            CVarDef.Create("physics.veliter", 8);
+
+        public static readonly CVarDef<bool> WarmStarting =
+            CVarDef.Create("physics.warmstart", true);
+
+        public static readonly CVarDef<bool> AutoClearForces =
+            CVarDef.Create("physics.autoclearforces", true);
+
+        /// <summary>
+        /// A velocity threshold for elastic collisions. Any collision with a relative linear
+        /// velocity below this threshold will be treated as inelastic.
+        /// </summary>
+        public static readonly CVarDef<float> VelocityThreshold =
+            CVarDef.Create("physics.velocitythreshold", 0.5f);
+
+        // TODO: Copy Box2D's comments on baumgarte I think it's on the solver class.
+        /// <summary>
+        ///     How much overlap is resolved per tick.
+        /// </summary>
+        public static readonly CVarDef<float> Baumgarte =
+            CVarDef.Create("physics.baumgarte", 0.2f);
+
+        /// <summary>
+        /// A small length used as a collision and constraint tolerance. Usually it is
+        /// chosen to be numerically significant, but visually insignificant.
+        /// </summary>
+        public static readonly CVarDef<float> LinearSlop =
+            CVarDef.Create("physics.linearslop", 0.005f);
+
+        /// <summary>
+        /// A small angle used as a collision and constraint tolerance. Usually it is
+        /// chosen to be numerically significant, but visually insignificant.
+        /// </summary>
+        public static readonly CVarDef<float> AngularSlop =
+            CVarDef.Create("physics.angularslop", 2.0f / 180.0f * MathF.PI);
+
+        /// <summary>
+        /// The radius of the polygon/edge shape skin. This should not be modified. Making
+        /// this smaller means polygons will have an insufficient buffer for continuous collision.
+        /// Making it larger may create artifacts for vertex collision.
+        /// </summary>
+        /// <remarks>
+        ///     Default is set to be 2 x linearslop. TODO Should we listen to linearslop changes?
+        /// </remarks>
+        public static readonly CVarDef<float> PolygonRadius =
+            CVarDef.Create("physics.polygonradius", 2 * 0.005f);
+
+        /// <summary>
+        /// If true, it will run a GiftWrap convex hull on all polygon inputs.
+        /// This makes for a more stable engine when given random input,
+        /// but if speed of the creation of polygons are more important,
+        /// you might want to set this to false.
+        /// </summary>
+        public static readonly CVarDef<bool> ConvexHullPolygons =
+            CVarDef.Create("physics.convexhullpolygons", true);
+
+        public static readonly CVarDef<int> MaxPolygonVertices =
+            CVarDef.Create("physics.maxpolygonvertices", 8);
+
+        public static readonly CVarDef<float> MaxLinearCorrection =
+            CVarDef.Create("physics.maxlinearcorrection", 0.2f);
+
+        public static readonly CVarDef<float> MaxAngularCorrection =
+            CVarDef.Create("physics.maxangularcorrection", 8.0f / 180.0f * MathF.PI);
+
+        // - Maximums
+        // Squared
+        public static readonly CVarDef<float> MaxLinVelocity =
+            CVarDef.Create("physics.maxlinvelocity", 4.0f);
+
+        // Squared
+        public static readonly CVarDef<float> MaxAngVelocity =
+            CVarDef.Create("physics.maxangvelocity", 0.5f * MathF.PI);
+
         /*
          * DISCORD
          */

Robust.Shared/Containers/ContainerHelpers.cs

@@ -108,7 +108,7 @@ namespace Robust.Shared.Containers
         /// <summary>
         ///     Attempts to remove all entities in a container.
         /// </summary>
-        public static void EmptyContainer(this IContainer container, bool force = false, EntityCoordinates? moveTo = null)
+        public static void EmptyContainer(this IContainer container, bool force = false, EntityCoordinates? moveTo = null, bool attachToGridOrMap = false)
         {
             foreach (var entity in container.ContainedEntities.ToArray())
             {
@@ -121,6 +121,9 @@ namespace Robust.Shared.Containers
 
                 if (moveTo.HasValue)
                     entity.Transform.Coordinates = moveTo.Value;
+
+                if(attachToGridOrMap)
+                    entity.Transform.AttachToGridOrMap();
             }
         }
 

Robust.Shared/ContentPack/Sandbox.yml

@@ -407,10 +407,16 @@ Types:
       - "System.Reflection.Assembly GetAssembly(System.Type)"
       - "System.Reflection.Assembly GetExecutingAssembly()"
       - "System.Type[] GetTypes()"
+      - "System.Reflection.AssemblyName GetName()"
     AssemblyCompanyAttribute: { All: True }
     AssemblyConfigurationAttribute: { All: True }
     AssemblyFileVersionAttribute: { All: True }
     AssemblyInformationalVersionAttribute: { All: True }
+    AssemblyName:
+      Methods:
+      - "string get_FullName()"
+      - "System.Version get_Version()"
+      - "string get_Name()"
     AssemblyProductAttribute: { All: True }
     AssemblyTitleAttribute: { All: True }
     DefaultMemberAttribute: { All: True }
@@ -1274,6 +1280,7 @@ Types:
     ValueTuple`7: { All: True }
     ValueTuple`8: { All: True }
     ValueType: { All: True }
+    Version: { All: True }
     Void: { All: True }
   YamlDotNet.Core.Events:
     MappingStyle: { } # Enum

Robust.Shared/Enums/NetworkEnums.cs

@@ -7,6 +7,7 @@
         PlacementFailed,
         RequestPlacement,
         RequestEntRemove,
+        RequestRectRemove,
     }
 
     public enum SessionStatus : byte

Robust.Shared/GameObjects/ComponentManager.cs

@@ -4,6 +4,7 @@ using System.Diagnostics.CodeAnalysis;
 using System.Linq;
 using System.Runtime.CompilerServices;
 using Robust.Shared.Exceptions;
+using Robust.Shared.Physics;
 using Robust.Shared.Serialization;
 using Robust.Shared.Utility;
 using DependencyAttribute = Robust.Shared.IoC.DependencyAttribute;
@@ -129,9 +130,12 @@ namespace Robust.Shared.GameObjects
                 ComponentAdded?.Invoke(this, new AddedComponentEventArgs(component));
             }
 
-            var defaultSerializer = DefaultValueSerializer.Reader();
-            defaultSerializer.CurrentType = component.GetType();
-            component.ExposeData(defaultSerializer);
+            if (entity.Initialized || entity.Initializing)
+            {
+                var defaultSerializer = DefaultValueSerializer.Reader();
+                defaultSerializer.CurrentType = component.GetType();
+                component.ExposeData(defaultSerializer);
+            }
 
             _componentDependencyManager.OnComponentAdd(entity, component);
 
@@ -190,7 +194,7 @@ namespace Robust.Shared.GameObjects
                 {
                     ITransformComponent _ => 0,
                     IMetaDataComponent _ => 1,
-                    IPhysicsComponent _ => 2,
+                    IPhysBody _ => 2,
                     _ => int.MaxValue
                 };
 

Robust.Shared/GameObjects/Components/Collidable/CollisionChangeMessage.cs

@@ -2,13 +2,16 @@ namespace Robust.Shared.GameObjects
 {
     public class CollisionChangeMessage : EntitySystemMessage
     {
+        public PhysicsComponent Body { get; }
+
         public EntityUid Owner { get; }
         public bool CanCollide { get; }
 
-        public CollisionChangeMessage(EntityUid owner, bool canCollide)
+        public CollisionChangeMessage(PhysicsComponent body, EntityUid owner, bool canCollide)
         {
+            Body = body;
             Owner = owner;
             CanCollide = canCollide;
         }
     }
-}
+}

Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Collision.cs

@@ -1,25 +1,37 @@
 using System;
-using System.Collections;
 using System.Collections.Generic;
+using Robust.Shared.Containers;
 using Robust.Shared.IoC;
+using Robust.Shared.Log;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
-using Robust.Shared.Players;
+using Robust.Shared.Physics.Broadphase;
+using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Physics.Dynamics.Contacts;
+using Robust.Shared.Physics.Dynamics.Joints;
 using Robust.Shared.Serialization;
+using Robust.Shared.Utility;
 using Robust.Shared.ViewVariables;
 
 namespace Robust.Shared.GameObjects
 {
     public interface ICollideBehavior
     {
-        void CollideWith(IEntity collidedWith);
-
         /// <summary>
-        ///     Called after all collisions have been processed, as well as how many collisions occured
+        ///     We'll pass in both our body and the other body to save the behaviors having to get these components themselves.
         /// </summary>
-        /// <param name="collisionCount"></param>
-        void PostCollide(int collisionCount) { }
+        void CollideWith(IPhysBody ourBody, IPhysBody otherBody);
+    }
+
+    public interface IPostCollide
+    {
+        /// <summary>
+        ///     Run behaviour after all other collision behaviors have run.
+        /// </summary>
+        /// <param name="ourBody"></param>
+        /// <param name="otherBody"></param>
+        void PostCollide(IPhysBody ourBody, IPhysBody otherBody);
     }
 
     public interface ICollideSpecial
@@ -27,483 +39,6 @@ namespace Robust.Shared.GameObjects
         bool PreventCollide(IPhysBody collidedwith);
     }
 
-    public partial interface IPhysicsComponent : IComponent, IPhysBody
-    {
-        public new bool Hard { get; set; }
-        bool IsColliding(Vector2 offset, bool approximate = true);
-
-        IEnumerable<IEntity> GetCollidingEntities(Vector2 offset, bool approximate = true);
-        bool UpdatePhysicsTree();
-
-        void RemovedFromPhysicsTree(MapId mapId);
-        void AddedToPhysicsTree(MapId mapId);
-    }
-
-    public partial class PhysicsComponent : Component, IPhysicsComponent
-    {
-        [Dependency] private readonly IPhysicsManager _physicsManager = default!;
-
-        private bool _canCollide;
-        private bool _isHard;
-        private BodyStatus _status;
-        private BodyType _bodyType;
-        private List<IPhysShape> _physShapes = new();
-
-        /// <inheritdoc />
-        public override string Name => "Physics";
-
-        /// <inheritdoc />
-        public override uint? NetID => NetIDs.PHYSICS;
-
-        public IEntity Entity => Owner;
-
-        /// <inheritdoc />
-        public MapId MapID => Owner.Transform.MapID;
-
-        /// <inheritdoc />
-        public int ProxyId { get; set; }
-
-        /// <inheritdoc />
-        [ViewVariables(VVAccess.ReadWrite)]
-        public BodyType BodyType { get; set; } = BodyType.Static;
-
-        /// <inheritdoc />
-        public int SleepAccumulator
-        {
-            get => _sleepAccumulator;
-            set
-            {
-                if (_sleepAccumulator == value)
-                    return;
-
-                _sleepAccumulator = value;
-                Awake = _physicsManager.SleepTimeThreshold > SleepAccumulator;
-            }
-        }
-
-        private int _sleepAccumulator;
-
-        // TODO: When SleepTimeThreshold updates we need to update Awake
-        public int SleepThreshold
-        {
-            get => _physicsManager.SleepTimeThreshold;
-            set => _physicsManager.SleepTimeThreshold = value;
-        }
-
-        /// <inheritdoc />
-        [ViewVariables]
-        public bool Awake
-        {
-            get => _awake;
-            private set
-            {
-                if (_awake == value)
-                    return;
-
-                _awake = value;
-                Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
-            }
-        }
-
-        private bool _awake = true;
-
-        /// <inheritdoc />
-        public void WakeBody()
-        {
-            if (CanMove())
-                SleepAccumulator = 0;
-        }
-
-        public PhysicsComponent()
-        {
-            PhysicsShapes = new PhysShapeList(this);
-        }
-
-        /// <inheritdoc />
-        public override void ExposeData(ObjectSerializer serializer)
-        {
-            base.ExposeData(serializer);
-
-            serializer.DataField(ref _canCollide, "on", true);
-            serializer.DataField(ref _isHard, "hard", true);
-            serializer.DataField(ref _status, "status", BodyStatus.OnGround);
-            serializer.DataField(ref _bodyType, "bodyType", BodyType.Static);
-            serializer.DataField(ref _physShapes, "shapes", new List<IPhysShape> {new PhysShapeAabb()});
-            serializer.DataField(ref _anchored, "anchored", true);
-            serializer.DataField(ref _mass, "mass", 1.0f);
-        }
-
-        /// <param name="player"></param>
-        /// <inheritdoc />
-        public override ComponentState GetComponentState(ICommonSession player)
-        {
-            return new PhysicsComponentState(_canCollide, _status, _physShapes, _isHard, _mass, LinearVelocity, AngularVelocity, Anchored);
-        }
-
-        /// <inheritdoc />
-        public override void HandleComponentState(ComponentState? curState, ComponentState? nextState)
-        {
-            if (curState == null)
-                return;
-
-            var newState = (PhysicsComponentState) curState;
-
-            _canCollide = newState.CanCollide;
-            _status = newState.Status;
-            _isHard = newState.Hard;
-            _physShapes = newState.PhysShapes;
-
-            foreach (var shape in _physShapes)
-            {
-                shape.ApplyState();
-            }
-
-            Dirty();
-            UpdateEntityTree();
-            Mass = newState.Mass / 1000f; // gram to kilogram
-
-            LinearVelocity = newState.LinearVelocity;
-            // Logger.Debug($"{IGameTiming.TickStampStatic}: [{Owner}] {LinearVelocity}");
-            AngularVelocity = newState.AngularVelocity;
-            Anchored = newState.Anchored;
-            // TODO: Does it make sense to reset controllers here?
-            // This caused space movement to break in content and I'm not 100% sure this is a good fix.
-            // Look man the CM test is in 5 hours cut me some slack.
-            //_controllers = null;
-            // Reset predict flag to false to avoid predicting stuff too long.
-            // Another possibly bad hack for content at the moment.
-            Predict = false;
-        }
-
-        /// <inheritdoc />
-        [ViewVariables]
-        Box2 IPhysBody.WorldAABB
-        {
-            get
-            {
-                var pos = Owner.Transform.WorldPosition;
-                return ((IPhysBody) this).AABB.Translated(pos);
-            }
-        }
-
-        /// <inheritdoc />
-        [ViewVariables]
-        Box2 IPhysBody.AABB
-        {
-            get
-            {
-                var angle = Owner.Transform.WorldRotation;
-                var bounds = new Box2();
-
-                foreach (var shape in _physShapes)
-                {
-                    var shapeBounds = shape.CalculateLocalBounds(angle);
-                    bounds = bounds.IsEmpty() ? shapeBounds : bounds.Union(shapeBounds);
-                }
-
-                return bounds;
-            }
-        }
-
-        /// <inheritdoc />
-        [ViewVariables]
-        public IList<IPhysShape> PhysicsShapes { get; }
-
-        /// <summary>
-        ///     Enables or disabled collision processing of this component.
-        /// </summary>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public bool CanCollide
-        {
-            get => _canCollide;
-            set
-            {
-                if (_canCollide == value)
-                    return;
-
-                _canCollide = value;
-
-                Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local,
-                    new CollisionChangeMessage(Owner.Uid, _canCollide));
-                Dirty();
-            }
-        }
-
-        /// <summary>
-        ///     Non-hard physics bodies will not cause action collision (e.g. blocking of movement)
-        ///     while still raising collision events.
-        /// </summary>
-        /// <remarks>
-        ///     This is useful for triggers or such to detect collision without actually causing a blockage.
-        /// </remarks>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public bool Hard
-        {
-            get => _isHard;
-            set
-            {
-                if (_isHard == value)
-                    return;
-
-                _isHard = value;
-                Dirty();
-            }
-        }
-
-        /// <summary>
-        ///     Bitmask of the collision layers this component is a part of.
-        /// </summary>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public int CollisionLayer
-        {
-            get
-            {
-                var layers = 0x0;
-
-                foreach (var shape in _physShapes)
-                    layers = layers | shape.CollisionLayer;
-                return layers;
-            }
-        }
-
-        /// <summary>
-        ///     Bitmask of the layers this component collides with.
-        /// </summary>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public int CollisionMask
-        {
-            get
-            {
-                var mask = 0x0;
-
-                foreach (var shape in _physShapes)
-                    mask = mask | shape.CollisionMask;
-                return mask;
-            }
-        }
-
-        /// <inheritdoc />
-        public override void Initialize()
-        {
-            base.Initialize();
-
-            // normally ExposeData would create this
-            if (_physShapes == null)
-            {
-                _physShapes = new List<IPhysShape> {new PhysShapeAabb()};
-            }
-            else
-            {
-                foreach (var shape in _physShapes)
-                {
-                    ShapeAdded(shape);
-                }
-            }
-
-            foreach (var controller in _controllers.Values)
-            {
-                controller.ControlledComponent = this;
-            }
-
-            Dirty();
-            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local,
-                new CollisionChangeMessage(Owner.Uid, _canCollide));
-        }
-
-        public override void OnAdd()
-        {
-            base.OnAdd();
-            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
-        }
-
-        public override void OnRemove()
-        {
-            base.OnRemove();
-
-            // In case somebody starts sharing shapes across multiple components I guess?
-            foreach (var shape in _physShapes)
-            {
-                ShapeRemoved(shape);
-            }
-
-            // Should we not call this if !_canCollide? PathfindingSystem doesn't care at least.
-            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new CollisionChangeMessage(Owner.Uid, false));
-            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
-        }
-
-        private void ShapeAdded(IPhysShape shape)
-        {
-            shape.OnDataChanged += ShapeDataChanged;
-        }
-
-        private void ShapeRemoved(IPhysShape item)
-        {
-            item.OnDataChanged -= ShapeDataChanged;
-        }
-
-        /// <inheritdoc />
-        protected override void Startup()
-        {
-            base.Startup();
-            _physicsManager.AddBody(this);
-        }
-
-        /// <inheritdoc />
-        protected override void Shutdown()
-        {
-            RemoveControllers();
-            _physicsManager.RemoveBody(this);
-            base.Shutdown();
-        }
-
-        public bool IsColliding(Vector2 offset, bool approx = true)
-        {
-            return _physicsManager.IsColliding(this, offset, approx);
-        }
-
-        public IEnumerable<IEntity> GetCollidingEntities(Vector2 offset, bool approx = true)
-        {
-            return _physicsManager.GetCollidingEntities(this, offset, approx);
-        }
-
-        public bool UpdatePhysicsTree()
-            => _physicsManager.Update(this);
-
-        public void RemovedFromPhysicsTree(MapId mapId)
-        {
-            _physicsManager.RemovedFromMap(this, mapId);
-        }
-
-        public void AddedToPhysicsTree(MapId mapId)
-        {
-            _physicsManager.AddedToMap(this, mapId);
-        }
-
-        private bool UpdateEntityTree() => Owner.EntityManager.UpdateEntityTree(Owner);
-
-        public bool IsOnGround()
-        {
-            return Status == BodyStatus.OnGround;
-        }
-
-        public bool IsInAir()
-        {
-            return Status == BodyStatus.InAir;
-        }
-
-        private void ShapeDataChanged()
-        {
-            Dirty();
-            UpdatePhysicsTree();
-        }
-
-        // Custom IList<> implementation so that we can hook addition/removal of shapes.
-        // To hook into their OnDataChanged event correctly.
-        private sealed class PhysShapeList : IList<IPhysShape>
-        {
-            private readonly PhysicsComponent _owner;
-
-            public PhysShapeList(PhysicsComponent owner)
-            {
-                _owner = owner;
-            }
-
-            public IEnumerator<IPhysShape> GetEnumerator()
-            {
-                return _owner._physShapes.GetEnumerator();
-            }
-
-            IEnumerator IEnumerable.GetEnumerator()
-            {
-                return GetEnumerator();
-            }
-
-            public void Add(IPhysShape item)
-            {
-                _owner._physShapes.Add(item);
-
-                ItemAdded(item);
-            }
-
-            public void Clear()
-            {
-                foreach (var item in _owner._physShapes)
-                {
-                    ItemRemoved(item);
-                }
-
-                _owner._physShapes.Clear();
-            }
-
-            public bool Contains(IPhysShape item)
-            {
-                return _owner._physShapes.Contains(item);
-            }
-
-            public void CopyTo(IPhysShape[] array, int arrayIndex)
-            {
-                _owner._physShapes.CopyTo(array, arrayIndex);
-            }
-
-            public bool Remove(IPhysShape item)
-            {
-                var found = _owner._physShapes.Remove(item);
-
-                if (found)
-                {
-                    ItemRemoved(item);
-                }
-
-                return found;
-            }
-
-            public int Count => _owner._physShapes.Count;
-            public bool IsReadOnly => false;
-
-            public int IndexOf(IPhysShape item)
-            {
-                return _owner._physShapes.IndexOf(item);
-            }
-
-            public void Insert(int index, IPhysShape item)
-            {
-                _owner._physShapes.Insert(index, item);
-                ItemAdded(item);
-            }
-
-            public void RemoveAt(int index)
-            {
-                var item = _owner._physShapes[index];
-                ItemRemoved(item);
-
-                _owner._physShapes.RemoveAt(index);
-            }
-
-            public IPhysShape this[int index]
-            {
-                get => _owner._physShapes[index];
-                set
-                {
-                    var oldItem = _owner._physShapes[index];
-                    ItemRemoved(oldItem);
-
-                    _owner._physShapes[index] = value;
-                    ItemAdded(value);
-                }
-            }
-
-            private void ItemAdded(IPhysShape item)
-            {
-                _owner.ShapeAdded(item);
-            }
-
-            public void ItemRemoved(IPhysShape item)
-            {
-                _owner.ShapeRemoved(item);
-            }
-        }
-    }
-
     [Serializable, NetSerializable]
     public enum BodyStatus: byte
     {
@@ -512,15 +47,28 @@ namespace Robust.Shared.GameObjects
     }
 
     /// <summary>
-    ///     Sent whenever a <see cref="IPhysicsComponent"/> is changed.
+    ///     Sent whenever a <see cref="IPhysBody"/> is changed.
     /// </summary>
     public sealed class PhysicsUpdateMessage : EntitySystemMessage
     {
-        public IPhysicsComponent Component { get; }
+        public PhysicsComponent Component { get; }
 
-        public PhysicsUpdateMessage(IPhysicsComponent component)
+        public PhysicsUpdateMessage(PhysicsComponent component)
         {
             Component = component;
         }
     }
+
+    public sealed class FixtureUpdateMessage : EntitySystemMessage
+    {
+        public PhysicsComponent Body { get; }
+
+        public Fixture Fixture { get; }
+
+        public FixtureUpdateMessage(PhysicsComponent body, Fixture fixture)
+        {
+            Body = body;
+            Fixture = fixture;
+        }
+    }
 }

Robust.Shared/GameObjects/Components/Collidable/PhysicsComponentState.cs

@@ -2,6 +2,8 @@
 using System.Collections.Generic;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
+using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Physics.Dynamics.Joints;
 using Robust.Shared.Serialization;
 
 namespace Robust.Shared.GameObjects
@@ -10,10 +12,11 @@ namespace Robust.Shared.GameObjects
     public class PhysicsComponentState : ComponentState
     {
         public readonly bool CanCollide;
+        public readonly bool SleepingAllowed;
+        public readonly bool FixedRotation;
         public readonly BodyStatus Status;
-        public readonly List<IPhysShape> PhysShapes;
-        public readonly bool Hard;
-
+        public readonly List<Fixture> Fixtures;
+        public readonly List<Joint> Joints;
 
         /// <summary>
         ///     Current mass of the entity, stored in grams.
@@ -21,31 +24,45 @@ namespace Robust.Shared.GameObjects
         public readonly int Mass;
         public readonly Vector2 LinearVelocity;
         public readonly float AngularVelocity;
-        public readonly bool Anchored;
+        public readonly BodyType BodyType;
 
         /// <summary>
         ///
         /// </summary>
         /// <param name="canCollide"></param>
+        /// <param name="sleepingAllowed"></param>
+        /// <param name="fixedRotation"></param>
         /// <param name="status"></param>
-        /// <param name="physShapes"></param>
-        /// <param name="hard"></param>
+        /// <param name="fixtures"></param>
+        /// <param name="joints"></param>
         /// <param name="mass">Current Mass of the entity.</param>
         /// <param name="linearVelocity">Current linear velocity of the entity in meters per second.</param>
         /// <param name="angularVelocity">Current angular velocity of the entity in radians per sec.</param>
-        /// <param name="anchored">Whether or not the entity is anchored in place.</param>
-        public PhysicsComponentState(bool canCollide, BodyStatus status, List<IPhysShape> physShapes, bool hard, float mass, Vector2 linearVelocity, float angularVelocity, bool anchored)
+        /// <param name="bodyType"></param>
+        public PhysicsComponentState(
+            bool canCollide,
+            bool sleepingAllowed,
+            bool fixedRotation,
+            BodyStatus status,
+            List<Fixture> fixtures,
+            List<Joint> joints,
+            float mass,
+            Vector2 linearVelocity,
+            float angularVelocity,
+            BodyType bodyType)
             : base(NetIDs.PHYSICS)
         {
             CanCollide = canCollide;
+            SleepingAllowed = sleepingAllowed;
+            FixedRotation = fixedRotation;
             Status = status;
-            PhysShapes = physShapes;
-            Hard = hard;
+            Fixtures = fixtures;
+            Joints = joints;
 
             LinearVelocity = linearVelocity;
             AngularVelocity = angularVelocity;
-            Mass = (int)Math.Round(mass * 1000); // rounds kg to nearest gram
-            Anchored = anchored;
+            Mass = (int) Math.Round(mass * 1000); // rounds kg to nearest gram
+            BodyType = bodyType;
         }
     }
 }

Robust.Shared/GameObjects/Components/CollisionWakeComponent.cs

@@ -0,0 +1,27 @@
+using Robust.Shared.Physics;
+
+namespace Robust.Shared.GameObjects
+{
+    /// <summary>
+    ///     An optimisation component for stuff that should be set as collidable when its awake and non-collidable when asleep.
+    /// </summary>
+    [RegisterComponent]
+    public sealed class CollisionWakeComponent : Component
+    {
+        public override string Name => "CollisionWake";
+
+        public override void HandleMessage(ComponentMessage message, IComponent? component)
+        {
+            base.HandleMessage(message, component);
+            switch (message)
+            {
+                case PhysicsWakeCompMessage msg:
+                    msg.Body.CanCollide = true;
+                    break;
+                case PhysicsSleepCompMessage msg:
+                    msg.Body.CanCollide = false;
+                    break;
+            }
+        }
+    }
+}

Robust.Shared/GameObjects/Components/ITransformComponent.cs

@@ -13,7 +13,22 @@ namespace Robust.Shared.GameObjects
     public interface ITransformComponent : IComponent
     {
         /// <summary>
-        /// Disables or enables to ability to locally rotate the entity. When set it removes any local rotation.
+        ///     Defer updates to the EntityTree and MoveEvent calls if toggled.
+        /// </summary>
+        bool DeferUpdates { get; set; }
+
+        /// <summary>
+        ///     While updating did we actually defer anything?
+        /// </summary>
+        bool UpdatesDeferred { get; }
+
+        /// <summary>
+        ///     Run MoveEvent, RotateEvent, and UpdateEntityTree updates.
+        /// </summary>
+        void RunDeferred();
+
+        /// <summary>
+        ///     Disables or enables to ability to locally rotate the entity. When set it removes any local rotation.
         /// </summary>
         bool NoLocalRotation { get; set; }
 
@@ -103,25 +118,12 @@ namespace Robust.Shared.GameObjects
         /// </summary>
         GridId GridID { get; }
 
-        /// <summary>
-        ///     Whether external system updates should run or not (e.g. EntityTree, Matrices, PhysicsTree).
-        ///     These should be manually run later.
-        /// </summary>
-        bool DeferUpdates { get; set; }
+        bool UpdateEntityTree();
 
         void AttachToGridOrMap();
         void AttachParent(ITransformComponent parent);
         void AttachParent(IEntity parent);
 
-        /// <summary>
-        ///     Run the updates marked as deferred (UpdateEntityTree and movement events).
-        ///     Don't call this unless you REALLY need to.
-        /// </summary>
-        /// <remarks>
-        ///    Physics optimisation so these aren't spammed during physics updates.
-        /// </remarks>
-        void RunPhysicsDeferred();
-
         IEnumerable<ITransformComponent> Children { get; }
         int ChildCount { get; }
         IEnumerable<EntityUid> ChildEntityUids { get; }

Robust.Shared/GameObjects/Components/Localization/GrammarComponent.cs

@@ -0,0 +1,40 @@
+
+using Robust.Shared.Enums;
+using Robust.Shared.IoC;
+using Robust.Shared.Reflection;
+using Robust.Shared.Serialization;
+using Robust.Shared.ViewVariables;
+
+namespace Robust.Shared.GameObjects.Components.Localization
+{
+    [RegisterComponent]
+    public class GrammarComponent : Component
+    {
+        public override string Name => "Grammar";
+        public override uint? NetID => NetIDs.GRAMMAR;
+
+        [ViewVariables]
+        public string LocalizationId = "";
+
+        [ViewVariables]
+        public Gender? Gender = null;
+
+        [ViewVariables]
+        public bool? ProperNoun = null;
+
+        public override void ExposeData(ObjectSerializer serializer)
+        {
+            serializer.DataField(ref LocalizationId, "localizationId", "");
+
+            if (serializer.TryReadDataFieldCached("gender", out string? gender0))
+            {
+                var refl = IoCManager.Resolve<IReflectionManager>();
+                if (refl.TryParseEnumReference(gender0!, out var gender))
+                {
+                    Gender = (Gender)gender;
+                }
+            }
+            serializer.DataField(ref ProperNoun,     "proper",         null);
+        }
+    }
+}

Robust.Shared/GameObjects/Components/NetIDs.cs

@@ -15,5 +15,6 @@ namespace Robust.Shared.GameObjects
         public const uint CONTAINER_MANAGER = 25;
         public const uint OCCLUDER = 26;
         public const uint EYE = 28;
+        public const uint GRAMMAR = 29;
     }
 }

Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs

@@ -29,13 +29,18 @@ namespace Robust.Shared.GameObjects
         private Vector2 _prevPosition;
         private Angle _prevRotation;
 
+        // Cache changes so we can distribute them after physics is done (better cache)
+        private EntityCoordinates? _oldCoords;
+        private Angle? _oldLocalRotation;
+
+        public bool UpdatesDeferred => _oldCoords != null || _oldLocalRotation != null;
+
         [ViewVariables(VVAccess.ReadWrite)]
         public bool ActivelyLerping { get; set; }
 
         [ViewVariables] private readonly SortedSet<EntityUid> _children = new();
 
         [Dependency] private readonly IMapManager _mapManager = default!;
-        [Dependency] private readonly IEntityManager _entityManager = default!;
 
         /// <inheritdoc />
         public override string Name => "Transform";
@@ -49,6 +54,7 @@ namespace Robust.Shared.GameObjects
 
         private bool _mapIdInitialized;
 
+        public bool DeferUpdates { get; set; }
 
         /// <inheritdoc />
         [ViewVariables]
@@ -73,13 +79,6 @@ namespace Robust.Shared.GameObjects
             }
         }
 
-        /// <inheritdoc />
-        public bool DeferUpdates { get; set; }
-
-        // Deferred fields
-        private Angle? _oldLocalRotation;
-        private EntityCoordinates? _oldCoords;
-
         /// <inheritdoc />
         [ViewVariables(VVAccess.ReadWrite)]
         public bool NoLocalRotation
@@ -120,7 +119,6 @@ namespace Robust.Shared.GameObjects
                 {
                     RebuildMatrices();
                     UpdateEntityTree();
-                    UpdatePhysicsTree();
                     Owner.EntityManager.EventBus.RaiseEvent(
                         EventSource.Local, new RotateEvent(Owner, oldRotation, _localRotation));
                 }
@@ -176,7 +174,7 @@ namespace Robust.Shared.GameObjects
         public EntityUid ParentUid
         {
             get => _parent;
-            set => Parent = _entityManager.GetEntity(value).Transform;
+            set => Parent = Owner.EntityManager.GetEntity(value).Transform;
         }
 
         /// <inheritdoc />
@@ -265,7 +263,7 @@ namespace Robust.Shared.GameObjects
 
                 if (value.EntityId != _parent)
                 {
-                    var newEntity = _entityManager.GetEntity(value.EntityId);
+                    var newEntity = Owner.EntityManager.GetEntity(value.EntityId);
                     AttachParent(newEntity);
                 }
 
@@ -283,7 +281,6 @@ namespace Robust.Shared.GameObjects
                     }
 
                     UpdateEntityTree();
-                    UpdatePhysicsTree();
                 }
                 else
                 {
@@ -316,7 +313,6 @@ namespace Robust.Shared.GameObjects
                 {
                     RebuildMatrices();
                     UpdateEntityTree();
-                    UpdatePhysicsTree();
                     Owner.EntityManager.EventBus.RaiseEvent(
                         EventSource.Local, new MoveEvent(Owner, oldGridPos, Coordinates));
                 }
@@ -327,35 +323,6 @@ namespace Robust.Shared.GameObjects
             }
         }
 
-        /// <inheritdoc />
-        public void RunPhysicsDeferred()
-        {
-            // if we resolved to (close enough) to the OG position then no update.
-            if ((_oldCoords == null || _oldCoords.Equals(Coordinates)) &&
-                (_oldLocalRotation == null || _oldLocalRotation.Equals(_localRotation)))
-            {
-                return;
-            }
-
-            RebuildMatrices();
-            UpdateEntityTree();
-            UpdatePhysicsTree();
-
-            if (_oldCoords != null)
-            {
-                Owner.EntityManager.EventBus.RaiseEvent(
-                    EventSource.Local, new MoveEvent(Owner, _oldCoords.Value, Coordinates));
-                _oldCoords = null;
-            }
-
-            if (_oldLocalRotation != null)
-            {
-                Owner.EntityManager.EventBus.RaiseEvent(
-                    EventSource.Local, new RotateEvent(Owner, _oldLocalRotation.Value, _localRotation));
-                _oldLocalRotation = null;
-            }
-        }
-
         [ViewVariables]
         public IEnumerable<ITransformComponent> Children =>
             _children.Select(u => Owner.EntityManager.GetEntity(u).Transform);
@@ -478,6 +445,33 @@ namespace Robust.Shared.GameObjects
             base.OnRemove();
         }
 
+        public void RunDeferred()
+        {
+            // if we resolved to (close enough) to the OG position then no update.
+            if ((_oldCoords == null || _oldCoords.Equals(Coordinates)) &&
+                (_oldLocalRotation == null || _oldLocalRotation.Equals(_localRotation)))
+            {
+                return;
+            }
+
+            RebuildMatrices();
+            UpdateEntityTree();
+
+            if (_oldCoords != null)
+            {
+                Owner.EntityManager.EventBus.RaiseEvent(
+                    EventSource.Local, new MoveEvent(Owner, _oldCoords.Value, Coordinates));
+                _oldCoords = null;
+            }
+
+            if (_oldLocalRotation != null)
+            {
+                Owner.EntityManager.EventBus.RaiseEvent(
+                    EventSource.Local, new RotateEvent(Owner, _oldLocalRotation.Value, _localRotation));
+                _oldLocalRotation = null;
+            }
+        }
+
         /// <summary>
         /// Detaches this entity from its parent.
         /// </summary>
@@ -495,7 +489,7 @@ namespace Robust.Shared.GameObjects
             IEntity newMapEntity;
             if (_mapManager.TryFindGridAt(mapPos, out var mapGrid))
             {
-                newMapEntity = _entityManager.GetEntity(mapGrid.GridEntityId);
+                newMapEntity = Owner.EntityManager.GetEntity(mapGrid.GridEntityId);
             }
             else if (_mapManager.HasMapEntity(mapPos.MapId))
             {
@@ -516,7 +510,10 @@ namespace Robust.Shared.GameObjects
 
             AttachParent(newMapEntity);
 
+            // Technically we're not moving, just changing parent.
+            DeferUpdates = true;
             WorldPosition = mapPos.Position;
+            DeferUpdates = false;
 
             Dirty();
         }
@@ -627,24 +624,12 @@ namespace Robust.Shared.GameObjects
 
         private void MapIdChanged(MapId oldId)
         {
-            IPhysicsComponent? collider;
-
             if (oldId != MapId.Nullspace)
             {
-                _entityManager.RemoveFromEntityTree(Owner, oldId);
-
-                if (Initialized && Owner.TryGetComponent(out collider))
-                {
-                    collider.RemovedFromPhysicsTree(oldId);
-                }
+                Owner.EntityManager.RemoveFromEntityTree(Owner, oldId);
             }
 
-            if (MapID != MapId.Nullspace && Initialized && Owner.TryGetComponent(out collider))
-            {
-                collider.AddedToPhysicsTree(MapID);
-            }
-
-            _entityManager.EventBus.RaiseEvent(EventSource.Local, new EntMapIdChangedMessage(Owner, oldId));
+            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntMapIdChangedMessage(Owner, oldId));
         }
 
         public void AttachParent(IEntity parent)
@@ -760,7 +745,6 @@ namespace Robust.Shared.GameObjects
 
                 Dirty();
                 UpdateEntityTree();
-                TryUpdatePhysicsTree();
             }
 
             if (nextState is TransformComponentState nextTransform)
@@ -782,6 +766,7 @@ namespace Robust.Shared.GameObjects
         // Hooks for GodotTransformComponent go here.
         protected virtual void SetPosition(Vector2 position)
         {
+            // DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y));
             _localPosition = position;
         }
 
@@ -824,12 +809,7 @@ namespace Robust.Shared.GameObjects
             _invLocalMatrix = itransMat;
         }
 
-        private bool TryUpdatePhysicsTree() => Initialized && UpdatePhysicsTree();
-
-        private bool UpdatePhysicsTree() =>
-            Owner.TryGetComponent(out IPhysicsComponent? collider) && collider.UpdatePhysicsTree();
-
-        private bool UpdateEntityTree() => _entityManager.UpdateEntityTree(Owner);
+        public bool UpdateEntityTree() => Owner.EntityManager.UpdateEntityTree(Owner);
 
         public string GetDebugString()
         {

Robust.Shared/GameObjects/Entity.cs

@@ -3,6 +3,7 @@ using System.Collections.Generic;
 using System.Diagnostics.CodeAnalysis;
 using System.Linq;
 using Robust.Shared.Network;
+using Robust.Shared.Physics;
 using Robust.Shared.Prototypes;
 using Robust.Shared.Timing;
 using Robust.Shared.Utility;
@@ -129,7 +130,7 @@ namespace Robust.Shared.GameObjects
                 .OrderBy(x => x switch
                 {
                     ITransformComponent _ => 0,
-                    IPhysicsComponent _ => 1,
+                    IPhysBody _ => 1,
                     _ => int.MaxValue
                 });
 
@@ -168,7 +169,7 @@ namespace Robust.Shared.GameObjects
                 .OrderBy(x => x switch
                 {
                     ITransformComponent _ => 0,
-                    IPhysicsComponent _ => 1,
+                    IPhysBody _ => 1,
                     _ => int.MaxValue
                 });
 

Robust.Shared/GameObjects/EntityManager.cs

@@ -476,9 +476,9 @@ namespace Robust.Shared.GameObjects
         /// <inheritdoc />
         public IEnumerable<IEntity> GetEntitiesIntersecting(IEntity entity, bool approximate = false)
         {
-            if (entity.TryGetComponent<IPhysicsComponent>(out var component))
+            if (entity.TryGetComponent<IPhysBody>(out var component))
             {
-                return GetEntitiesIntersecting(entity.Transform.MapID, component.WorldAABB, approximate);
+                return GetEntitiesIntersecting(entity.Transform.MapID, component.GetWorldAABB(), approximate);
             }
 
             return GetEntitiesIntersecting(entity.Transform.Coordinates, approximate);
@@ -493,9 +493,9 @@ namespace Robust.Shared.GameObjects
 
         private static bool Intersecting(IEntity entity, Vector2 mapPosition)
         {
-            if (entity.TryGetComponent(out IPhysicsComponent? component))
+            if (entity.TryGetComponent(out IPhysBody? component))
             {
-                if (component.WorldAABB.Contains(mapPosition))
+                if (component.GetWorldAABB().Contains(mapPosition))
                     return true;
             }
             else
@@ -539,9 +539,9 @@ namespace Robust.Shared.GameObjects
         /// <inheritdoc />
         public IEnumerable<IEntity> GetEntitiesInRange(IEntity entity, float range, bool approximate = false)
         {
-            if (entity.TryGetComponent<IPhysicsComponent>(out var component))
+            if (entity.TryGetComponent<IPhysBody>(out var component))
             {
-                return GetEntitiesInRange(entity.Transform.MapID, component.WorldAABB, range, approximate);
+                return GetEntitiesInRange(entity.Transform.MapID, component.GetWorldAABB(), range, approximate);
             }
 
             return GetEntitiesInRange(entity.Transform.Coordinates, range, approximate);
@@ -591,7 +591,11 @@ namespace Robust.Shared.GameObjects
 
             if (!_entityTreesPerMap.TryGetValue(mapId, out var entTree))
             {
-                entTree = EntityTreeFactory();
+                entTree = new DynamicTree<IEntity>(
+                    GetWorldAabbFromEntity,
+                    capacity: 16,
+                    growthFunc: x => x == 16 ? 3840 : x + 256
+                );
                 _entityTreesPerMap.Add(mapId, entTree);
             }
 
@@ -635,20 +639,13 @@ namespace Robust.Shared.GameObjects
             }
         }
 
-        private static DynamicTree<IEntity> EntityTreeFactory() =>
-            new(
-                GetWorldAabbFromEntity,
-                capacity: 16,
-                growthFunc: x => x == 16 ? 3840 : x + 256
-            );
-
-        protected static Box2 GetWorldAabbFromEntity(in IEntity ent)
+        protected Box2 GetWorldAabbFromEntity(in IEntity ent)
         {
             if (ent.Deleted)
                 return new Box2(0, 0, 0, 0);
 
-            if (ent.TryGetComponent(out IPhysicsComponent? collider))
-                return collider.WorldAABB;
+            if (ent.TryGetComponent(out IPhysBody? collider))
+                return collider.GetWorldAABB(_mapManager);
 
             var pos = ent.Transform.WorldPosition;
             return new Box2(pos, pos);

Robust.Shared/GameObjects/EntityQuery.cs

@@ -2,6 +2,7 @@
 using System.Collections.Generic;
 using System.Linq;
 using JetBrains.Annotations;
+using Robust.Shared.Physics;
 using Robust.Shared.Utility;
 
 namespace Robust.Shared.GameObjects
@@ -120,9 +121,9 @@ namespace Robust.Shared.GameObjects
         /// <inheritdoc />
         public bool Match(IEntity entity)
         {
-            if(Entity.TryGetComponent<IPhysicsComponent>(out var physics))
+            if(Entity.TryGetComponent<IPhysBody>(out var physics))
             {
-                return physics.MapID == entity.Transform.MapID && physics.WorldAABB.Contains(entity.Transform.WorldPosition);
+                return physics.MapID == entity.Transform.MapID && physics.GetWorldAABB().Contains(entity.Transform.WorldPosition);
             }
             return false;
         }

Robust.Shared/GameObjects/EntitySystemManager.cs

@@ -158,12 +158,12 @@ namespace Robust.Shared.GameObjects
         private static (IEnumerable<IEntitySystem> frameUpd, IEnumerable<IEntitySystem> upd)
             CalculateUpdateOrder(Dictionary<Type, IEntitySystem>.ValueCollection systems)
         {
-            var allNodes = new List<GraphNode>();
-            var typeToNode = new Dictionary<Type, GraphNode>();
+            var allNodes = new List<GraphNode<IEntitySystem>>();
+            var typeToNode = new Dictionary<Type, GraphNode<IEntitySystem>>();
 
             foreach (var system in systems)
             {
-                var node = new GraphNode(system);
+                var node = new GraphNode<IEntitySystem>(system);
 
                 allNodes.Add(node);
                 typeToNode.Add(system.GetType(), node);
@@ -193,10 +193,10 @@ namespace Robust.Shared.GameObjects
             return (frameUpdate, update);
         }
 
-        private static IEnumerable<GraphNode> TopologicalSort(IEnumerable<GraphNode> nodes)
+        internal static IEnumerable<GraphNode<T>> TopologicalSort<T>(IEnumerable<GraphNode<T>> nodes)
         {
             var elems = nodes.ToDictionary(node => node,
-                node => new HashSet<GraphNode>(node.DependsOn));
+                node => new HashSet<GraphNode<T>>(node.DependsOn));
             while (elems.Count > 0)
             {
                 var elem =
@@ -331,12 +331,12 @@ namespace Robust.Shared.GameObjects
         }
 
         [DebuggerDisplay("GraphNode: {" + nameof(System) + "}")]
-        private sealed class GraphNode
+        internal sealed class GraphNode<T>
         {
-            public readonly IEntitySystem System;
-            public readonly List<GraphNode> DependsOn = new();
+            public readonly T System;
+            public readonly List<GraphNode<T>> DependsOn = new();
 
-            public GraphNode(IEntitySystem system)
+            public GraphNode(T system)
             {
                 System = system;
             }

Robust.Shared/GameObjects/Systems/SharedGridTraversalSystem.cs

@@ -0,0 +1,66 @@
+using System.Collections.Generic;
+using Robust.Shared.Containers;
+using Robust.Shared.IoC;
+using Robust.Shared.Map;
+
+namespace Robust.Shared.GameObjects
+{
+    /// <summary>
+    ///     Handles moving entities between grids as they move around.
+    /// </summary>
+    internal sealed class SharedGridTraversalSystem : EntitySystem
+    {
+        [Dependency] private readonly IMapManager _mapManager = default!;
+
+        private Queue<MoveEvent> _queuedMoveEvents = new();
+
+        public override void Initialize()
+        {
+            base.Initialize();
+            SubscribeLocalEvent<MoveEvent>(QueueMoveEvent);
+        }
+
+        public override void Shutdown()
+        {
+            base.Shutdown();
+            UnsubscribeLocalEvent<MoveEvent>();
+        }
+
+        public override void Update(float frameTime)
+        {
+            base.Update(frameTime);
+            while (_queuedMoveEvents.Count > 0)
+            {
+                var moveEvent = _queuedMoveEvents.Dequeue();
+                var entity = moveEvent.Sender;
+
+                if (entity.Deleted || !entity.HasComponent<PhysicsComponent>() || entity.IsInContainer()) continue;
+
+                var transform = entity.Transform;
+                // Change parent if necessary
+                // Given islands will probably have a bunch of static bodies in them then we'll verify velocities first as it's way cheaper
+
+                // This shoouullddnnn'''tt de-parent anything in a container because none of that should have physics applied to it.
+                if (_mapManager.TryFindGridAt(transform.MapID, moveEvent.NewPosition.ToMapPos(EntityManager), out var grid) &&
+                    grid.GridEntityId.IsValid() &&
+                    grid.GridEntityId != entity.Uid)
+                {
+                    // Also this may deparent if 2 entities are parented but not using containers so fix that
+                    if (grid.GridEntityId != transform.ParentUid)
+                    {
+                        transform.AttachParent(EntityManager.GetEntity(grid.GridEntityId));
+                    }
+                }
+                else
+                {
+                    transform.AttachParent(_mapManager.GetMapEntity(transform.MapID));
+                }
+            }
+        }
+
+        private void QueueMoveEvent(MoveEvent moveEvent)
+        {
+            _queuedMoveEvents.Enqueue(moveEvent);
+        }
+    }
+}

Robust.Shared/GameObjects/Systems/SharedPhysicsSystem.cs

@@ -1,106 +1,241 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
-using Robust.Shared.Containers;
+using Robust.Server.GameObjects;
+using Robust.Shared.IoC;
 using Robust.Shared.Map;
-using Robust.Shared.Maths;
 using Robust.Shared.Physics;
-using Robust.Shared.Random;
-using Robust.Shared.Timing;
+using Robust.Shared.Physics.Controllers;
+using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Reflection;
 using DependencyAttribute = Robust.Shared.IoC.DependencyAttribute;
+using Logger = Robust.Shared.Log.Logger;
 
 namespace Robust.Shared.GameObjects
 {
     public abstract class SharedPhysicsSystem : EntitySystem
     {
-        [Dependency] private readonly ITileDefinitionManager _tileDefinitionManager = default!;
+        /*
+         * TODO:
+         * Port acruid's box solver in to reduce allocs for building manifolds (this one is important for perf to remove the disgusting ctors and casts)
+         * Raycasts for non-box shapes.
+         * SetTransformIgnoreContacts for teleports (and anything else left on the physics body in Farseer)
+         * Actual center of mass for shapes (currently just assumes center coordinate)
+         * Circle offsets to entity.
+         * TOI Solver (continuous collision detection)
+         * Poly cutting
+         * Chain shape
+         * (Content) grenade launcher grenades that explode after time rather than impact.
+         */
+
+        /*
+         * Multi-threading notes:
+         * Sources:
+         * https://github.com/VelcroPhysics/VelcroPhysics/issues/29
+         * Aether2D
+         * Rapier
+         * https://www.slideshare.net/takahiroharada/solver-34909157
+         *
+         * SO essentially what we should look at doing from what I can discern:
+         * Build islands sequentially and then solve them all in parallel (as static bodies are the only thing shared
+         * it should be okay given they're never written to)
+         * After this, we can then look at doing narrowphase in parallel maybe (at least Aether2D does it) +
+         * position constraints in parallel + velocity constraints in parallel
+         *
+         * The main issue to tackle is graph colouring; Aether2D just seems to use locks for the parallel constraints solver
+         * though rapier has a graph colouring implementation (and because of this we should be able to avoid using locks) which we could try using.
+         *
+         * Given the kind of game SS14 is (our target game I guess) parallelising the islands will probably be the biggest benefit.
+         */
+
         [Dependency] private readonly IMapManager _mapManager = default!;
-        [Dependency] private readonly IPhysicsManager _physicsManager = default!;
-        [Dependency] private readonly IRobustRandom _random = default!;
-        [Dependency] private readonly IGameTiming _timing = default!;
 
-        private const float Epsilon = 1.0e-6f;
+        public IReadOnlyDictionary<MapId, PhysicsMap> Maps => _maps;
+        private Dictionary<MapId, PhysicsMap> _maps = new();
 
-        private readonly List<Manifold> _collisionCache = new();
+        internal IReadOnlyList<VirtualController> Controllers => _controllers;
+        private List<VirtualController> _controllers = new();
 
-        /// <summary>
-        ///     Physics objects that are awake and usable for world simulation.
-        /// </summary>
-        private readonly HashSet<IPhysicsComponent> _awakeBodies = new();
-
-        /// <summary>
-        ///     Physics objects that are awake and predicted and usable for world simulation.
-        /// </summary>
-        private readonly HashSet<IPhysicsComponent> _predictedAwakeBodies = new();
-
-        /// <summary>
-        ///     VirtualControllers on applicable <see cref="IPhysicsComponent"/>s
-        /// </summary>
-        private Dictionary<IPhysicsComponent, IEnumerable<VirtualController>> _controllers =
-            new();
-
-        // We'll defer changes to IPhysicsComponent until each step is done.
-        private readonly List<IPhysicsComponent> _queuedDeletions = new();
-        private readonly List<IPhysicsComponent> _queuedUpdates = new();
-
-        /// <summary>
-        ///     Updates to EntityTree etc. that are deferred until the end of physics.
-        /// </summary>
-        private readonly HashSet<IPhysicsComponent> _deferredUpdates = new();
-
-        // CVars aren't replicated to client (yet) so not using a cvar server-side for this.
-        private float _speedLimit = 30.0f;
+        // TODO: Stoer all the controllers here akshully
 
         public override void Initialize()
         {
             base.Initialize();
+
+            // Having a nullspace map just makes a bunch of code easier, we just don't iterate on it.
+            var nullMap = new PhysicsMap(MapId.Nullspace);
+            _maps[MapId.Nullspace] = nullMap;
+            nullMap.Initialize();
+
+            _mapManager.MapCreated += HandleMapCreated;
+            _mapManager.MapDestroyed += HandleMapDestroyed;
+
             SubscribeLocalEvent<PhysicsUpdateMessage>(HandlePhysicsUpdateMessage);
+            SubscribeLocalEvent<PhysicsWakeMessage>(HandleWakeMessage);
+            SubscribeLocalEvent<PhysicsSleepMessage>(HandleSleepMessage);
+            SubscribeLocalEvent<EntMapIdChangedMessage>(HandleMapChange);
+
+            SubscribeLocalEvent<EntInsertedIntoContainerMessage>(HandleContainerInserted);
+            SubscribeLocalEvent<EntRemovedFromContainerMessage>(HandleContainerRemoved);
+            BuildControllers();
+            Logger.DebugS("physics", $"Found {_controllers.Count} physics controllers.");
+        }
+
+        private void BuildControllers()
+        {
+            var reflectionManager = IoCManager.Resolve<IReflectionManager>();
+            var typeFactory = IoCManager.Resolve<IDynamicTypeFactory>();
+            var allControllerTypes = new List<Type>();
+
+            foreach (var type in reflectionManager.GetAllChildren(typeof(VirtualController)))
+            {
+                if (type.IsAbstract) continue;
+                allControllerTypes.Add(type);
+            }
+
+            var instantiated = new Dictionary<Type, VirtualController>();
+
+            foreach (var type in allControllerTypes)
+            {
+                instantiated.Add(type, (VirtualController) typeFactory.CreateInstance(type));
+            }
+
+            // Build dependency graph, copied from EntitySystemManager *COUGH
+
+            var nodes = new Dictionary<Type, EntitySystemManager.GraphNode<VirtualController>>();
+
+            foreach (var (_, controller) in instantiated)
+            {
+                var node = new EntitySystemManager.GraphNode<VirtualController>(controller);
+                nodes[controller.GetType()] = node;
+            }
+
+            foreach (var (type, node) in nodes)
+            {
+                foreach (var before in instantiated[type].UpdatesBefore)
+                {
+                    nodes[before].DependsOn.Add(node);
+                }
+
+                foreach (var after in instantiated[type].UpdatesAfter)
+                {
+                    node.DependsOn.Add(nodes[after]);
+                }
+            }
+
+            _controllers = GameObjects.EntitySystemManager.TopologicalSort(nodes.Values).Select(c => c.System).ToList();
+
+            foreach (var controller in _controllers)
+            {
+                controller.Initialize();
+            }
+        }
+
+        public override void Shutdown()
+        {
+            base.Shutdown();
+
+            _mapManager.MapCreated -= HandleMapCreated;
+            _mapManager.MapDestroyed -= HandleMapDestroyed;
+
+            UnsubscribeLocalEvent<PhysicsUpdateMessage>();
+            UnsubscribeLocalEvent<PhysicsWakeMessage>();
+            UnsubscribeLocalEvent<PhysicsSleepMessage>();
+            UnsubscribeLocalEvent<EntMapIdChangedMessage>();
+
+            UnsubscribeLocalEvent<EntInsertedIntoContainerMessage>();
+            UnsubscribeLocalEvent<EntRemovedFromContainerMessage>();
+        }
+
+        private void HandleMapCreated(object? sender, MapEventArgs eventArgs)
+        {
+            // Server just creates nullspace map on its own but sends it to client hence we will just ignore it.
+            if (_maps.ContainsKey(eventArgs.Map)) return;
+
+            var map = new PhysicsMap(eventArgs.Map);
+            _maps.Add(eventArgs.Map, map);
+            map.Initialize();
+            Logger.DebugS("physics", $"Created physics map for {eventArgs.Map}");
+        }
+
+        private void HandleMapDestroyed(object? sender, MapEventArgs eventArgs)
+        {
+            _maps.Remove(eventArgs.Map);
+            Logger.DebugS("physics", $"Destroyed physics map for {eventArgs.Map}");
+        }
+
+        private void HandleMapChange(EntMapIdChangedMessage message)
+        {
+            if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent))
+                return;
+
+            var oldMapId = message.OldMapId;
+            if (oldMapId != MapId.Nullspace)
+            {
+                _maps[oldMapId].RemoveBody(physicsComponent);
+                physicsComponent.ClearJoints();
+            }
+
+            var newMapId = message.Entity.Transform.MapID;
+            if (newMapId != MapId.Nullspace)
+            {
+                _maps[newMapId].AddBody(physicsComponent);
+            }
         }
 
         private void HandlePhysicsUpdateMessage(PhysicsUpdateMessage message)
         {
-            if (message.Component.Deleted || !message.Component.Awake)
+            var mapId = message.Component.Owner.Transform.MapID;
+
+            if (mapId == MapId.Nullspace)
+                return;
+
+            if (message.Component.Deleted || !message.Component.CanCollide)
             {
-                _queuedDeletions.Add(message.Component);
+                _maps[mapId].RemoveBody(message.Component);
             }
             else
             {
-                _queuedUpdates.Add(message.Component);
+                _maps[mapId].AddBody(message.Component);
             }
         }
 
-        /// <summary>
-        ///     Process the changes to cached <see cref="IPhysicsComponent"/>s
-        /// </summary>
-        private void ProcessQueue()
+        private void HandleWakeMessage(PhysicsWakeMessage message)
         {
-            // At this stage only the dynamictree cares about asleep bodies
-            // Implicitly awake bodies so don't need to check .Awake again
-            // Controllers should wake their body up (inside)
-            foreach (var physics in _queuedUpdates)
-            {
-                if (physics.Predict)
-                    _predictedAwakeBodies.Add(physics);
+            var mapId = message.Body.Owner.Transform.MapID;
 
-                _awakeBodies.Add(physics);
+            if (mapId == MapId.Nullspace)
+                return;
 
-                if (physics.Controllers.Count > 0 && !_controllers.ContainsKey(physics))
-                    _controllers.Add(physics, physics.Controllers.Values);
+            _maps[mapId].AddAwakeBody(message.Body);
+        }
 
-            }
+        private void HandleSleepMessage(PhysicsSleepMessage message)
+        {
+            var mapId = message.Body.Owner.Transform.MapID;
 
-            _queuedUpdates.Clear();
+            if (mapId == MapId.Nullspace)
+                return;
 
-            foreach (var physics in _queuedDeletions)
-            {
-                // If an entity was swapped from awake -> sleep -> awake then it's still relevant.
-                if (!physics.Deleted && physics.Awake) continue;
-                _awakeBodies.Remove(physics);
-                _predictedAwakeBodies.Remove(physics);
-                _controllers.Remove(physics);
-            }
+            _maps[mapId].RemoveSleepBody(message.Body);
+        }
 
-            _queuedDeletions.Clear();
+        private void HandleContainerInserted(EntInsertedIntoContainerMessage message)
+        {
+            if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent)) return;
+
+            var mapId = message.Container.Owner.Transform.MapID;
+
+            _maps[mapId].RemoveBody(physicsComponent);
+        }
+
+        private void HandleContainerRemoved(EntRemovedFromContainerMessage message)
+        {
+            if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent)) return;
+
+            var mapId = message.Container.Owner.Transform.MapID;
+
+            _maps[mapId].AddBody(physicsComponent);
         }
 
         /// <summary>
@@ -110,361 +245,28 @@ namespace Robust.Shared.GameObjects
         /// <param name="prediction">Should only predicted entities be considered in this simulation step?</param>
         protected void SimulateWorld(float deltaTime, bool prediction)
         {
-            var simulatedBodies = prediction ? _predictedAwakeBodies : _awakeBodies;
-
-            ProcessQueue();
-
-            foreach (var body in simulatedBodies)
+            foreach (var controller in _controllers)
             {
-                // running prediction updates will not cause a body to go to sleep.
-                if(!prediction)
-                    body.SleepAccumulator++;
-
-                // if the body cannot move, nothing to do here
-                if(!body.CanMove())
-                    continue;
-
-                var linearVelocity = Vector2.Zero;
-
-                foreach (var controller in body.Controllers.Values)
-                {
-                    controller.UpdateBeforeProcessing();
-                    linearVelocity += controller.LinearVelocity;
-                }
-
-                // i'm not sure if this is the proper way to solve this, but
-                // these are not kinematic bodies, so we need to preserve the previous
-                // velocity.
-                //if (body.LinearVelocity.LengthSquared < linearVelocity.LengthSquared)
-                    body.LinearVelocity = linearVelocity;
-
-                // Integrate forces
-                body.LinearVelocity += body.Force * body.InvMass * deltaTime;
-                body.AngularVelocity += body.Torque * body.InvI * deltaTime;
-
-                // forces are instantaneous, so these properties are cleared
-                // once integrated. If you want to apply a continuous force,
-                // it has to be re-applied every tick.
-                body.Force = Vector2.Zero;
-                body.Torque = 0f;
+                controller.UpdateBeforeSolve(prediction, deltaTime);
             }
 
-            // Calculate collisions and store them in the cache
-            ProcessCollisions(_awakeBodies);
-
-            // Remove all entities that were deleted during collision handling
-            ProcessQueue();
-
-            // Process frictional forces
-            foreach (var physics in _awakeBodies)
+            foreach (var (mapId, map) in _maps)
             {
-                ProcessFriction(physics, deltaTime);
+                if (mapId == MapId.Nullspace) continue;
+                map.Step(deltaTime, prediction);
             }
 
-            foreach (var (_, controllers) in _controllers)
+            foreach (var controller in _controllers)
             {
-                foreach (var controller in controllers)
-                {
-                    controller.UpdateAfterProcessing();
-                }
+                controller.UpdateAfterSolve(prediction, deltaTime);
             }
 
-            // Remove all entities that were deleted due to the controller
-            ProcessQueue();
-
-            const int solveIterationsAt60 = 4;
-
-            var multiplier = deltaTime / (1f / 60);
-
-            var divisions = MathHelper.Clamp(
-                MathF.Round(solveIterationsAt60 * multiplier, MidpointRounding.AwayFromZero),
-                1,
-                20
-            );
-
-            if (_timing.InSimulation) divisions = 1;
-
-            for (var i = 0; i < divisions; i++)
+            // Go through and run all of the deferred events now
+            foreach (var (mapId, map) in _maps)
             {
-                foreach (var physics in simulatedBodies)
-                {
-                    if (physics.CanMove())
-                    {
-                        UpdatePosition(physics, deltaTime / divisions);
-                    }
-                }
-
-                for (var j = 0; j < divisions; ++j)
-                {
-                    if (FixClipping(_collisionCache, divisions))
-                    {
-                        break;
-                    }
-                }
+                if (mapId == MapId.Nullspace) continue;
+                map.ProcessQueue();
             }
-
-            // As we also defer the updates for the _collisionCache we need to update all entities
-            foreach (var physics in _deferredUpdates)
-            {
-                var transform = physics.Owner.Transform;
-                transform.DeferUpdates = false;
-                transform.RunPhysicsDeferred();
-            }
-
-            _deferredUpdates.Clear();
-        }
-
-        // Runs collision behavior and updates cache
-        private void ProcessCollisions(IEnumerable<IPhysicsComponent> awakeBodies)
-        {
-            _collisionCache.Clear();
-            var combinations = new HashSet<(EntityUid, EntityUid)>();
-            foreach (var aPhysics in awakeBodies)
-            {
-                foreach (var b in _physicsManager.GetCollidingEntities(aPhysics, Vector2.Zero, false))
-                {
-                    var aUid = aPhysics.Entity.Uid;
-                    var bUid = b.Uid;
-
-                    if (bUid.CompareTo(aUid) > 0)
-                    {
-                        var tmpUid = bUid;
-                        bUid = aUid;
-                        aUid = tmpUid;
-                    }
-
-                    if (!combinations.Add((aUid, bUid)))
-                    {
-                        continue;
-                    }
-
-                    var bPhysics = b.GetComponent<IPhysicsComponent>();
-                    _collisionCache.Add(new Manifold(aPhysics, bPhysics, aPhysics.Hard && bPhysics.Hard));
-                }
-            }
-
-            var counter = 0;
-
-            if (_collisionCache.Count > 0)
-            {
-                while(GetNextCollision(_collisionCache, counter, out var collision))
-                {
-                    collision.A.WakeBody();
-                    collision.B.WakeBody();
-
-                    counter++;
-                    var impulse = _physicsManager.SolveCollisionImpulse(collision);
-                    if (collision.A.CanMove())
-                    {
-                        collision.A.ApplyImpulse(-impulse);
-                    }
-
-                    if (collision.B.CanMove())
-                    {
-                        collision.B.ApplyImpulse(impulse);
-                    }
-                }
-            }
-
-            var collisionsWith = new Dictionary<ICollideBehavior, int>();
-            foreach (var collision in _collisionCache)
-            {
-                // Apply onCollide behavior
-                foreach (var behavior in collision.A.Entity.GetAllComponents<ICollideBehavior>().ToArray())
-                {
-                    var entity = collision.B.Entity;
-                    if (entity.Deleted) break;
-                    behavior.CollideWith(entity);
-                    if (collisionsWith.ContainsKey(behavior))
-                    {
-                        collisionsWith[behavior] += 1;
-                    }
-                    else
-                    {
-                        collisionsWith[behavior] = 1;
-                    }
-                }
-
-                foreach (var behavior in collision.B.Entity.GetAllComponents<ICollideBehavior>().ToArray())
-                {
-                    var entity = collision.A.Entity;
-                    if (entity.Deleted) break;
-                    behavior.CollideWith(entity);
-                    if (collisionsWith.ContainsKey(behavior))
-                    {
-                        collisionsWith[behavior] += 1;
-                    }
-                    else
-                    {
-                        collisionsWith[behavior] = 1;
-                    }
-                }
-            }
-
-            foreach (var behavior in collisionsWith.Keys)
-            {
-                behavior.PostCollide(collisionsWith[behavior]);
-            }
-        }
-
-        private bool GetNextCollision(IReadOnlyList<Manifold> collisions, int counter, out Manifold collision)
-        {
-            // The *4 is completely arbitrary
-            if (counter > collisions.Count * 4)
-            {
-                collision = default;
-                return false;
-            }
-
-            var offset = _random.Next(collisions.Count - 1);
-            for (var i = 0; i < collisions.Count; i++)
-            {
-                var index = (i + offset) % collisions.Count;
-                if (collisions[index].Unresolved)
-                {
-                    collision = collisions[index];
-                    return true;
-                }
-
-            }
-
-            collision = default;
-            return false;
-        }
-
-        private void ProcessFriction(IPhysicsComponent body, float deltaTime)
-        {
-            if (body.LinearVelocity == Vector2.Zero) return;
-
-            // sliding friction coefficient, and current gravity at current location
-            var (friction, gravity) = GetFriction(body);
-
-            // friction between the two objects
-            var effectiveFriction = friction * body.Friction;
-
-            // current acceleration due to friction
-            var fAcceleration = effectiveFriction * gravity;
-
-            // integrate acceleration
-            var fVelocity = fAcceleration * deltaTime;
-
-            // Clamp friction because friction can't make you accelerate backwards
-            friction = Math.Min(fVelocity, body.LinearVelocity.Length);
-
-            if (friction == 0.0f)
-            {
-                return;
-            }
-
-            // No multiplication/division by mass here since that would be redundant.
-            var frictionVelocityChange = body.LinearVelocity.Normalized * -friction;
-
-            body.LinearVelocity += frictionVelocityChange;
-        }
-
-        private void UpdatePosition(IPhysicsComponent physics, float frameTime)
-        {
-            var ent = physics.Entity;
-
-            if (!physics.CanMove() || (physics.LinearVelocity.LengthSquared < Epsilon && MathF.Abs(physics.AngularVelocity) < Epsilon))
-                return;
-
-            if (physics.LinearVelocity != Vector2.Zero)
-            {
-                if (ent.IsInContainer())
-                {
-                    var relayEntityMoveMessage = new RelayMovementEntityMessage(ent);
-                    ent.Transform.Parent!.Owner.SendMessage(ent.Transform, relayEntityMoveMessage);
-                    // This prevents redundant messages from being sent if solveIterations > 1 and also simulates the entity "colliding" against the locker door when it opens.
-                    physics.LinearVelocity = Vector2.Zero;
-                }
-            }
-
-            physics.Owner.Transform.DeferUpdates = true;
-            _deferredUpdates.Add(physics);
-
-            // Slow zone up in here
-            if (physics.LinearVelocity.Length > _speedLimit)
-                physics.LinearVelocity = physics.LinearVelocity.Normalized * _speedLimit;
-
-            var newPosition = physics.WorldPosition + physics.LinearVelocity * frameTime;
-            var owner = physics.Owner;
-            var transform = owner.Transform;
-
-            // Change parent if necessary
-            if (!owner.IsInContainer())
-            {
-                // This shoouullddnnn'''tt de-parent anything in a container because none of that should have physics applied to it.
-                if (_mapManager.TryFindGridAt(owner.Transform.MapID, newPosition, out var grid) &&
-                    grid.GridEntityId.IsValid() &&
-                    grid.GridEntityId != owner.Uid)
-                {
-                    if (grid.GridEntityId != transform.ParentUid)
-                        transform.AttachParent(owner.EntityManager.GetEntity(grid.GridEntityId));
-                }
-                else
-                {
-                    transform.AttachParent(_mapManager.GetMapEntity(transform.MapID));
-                }
-            }
-
-            physics.WorldRotation += physics.AngularVelocity * frameTime;
-            physics.WorldPosition = newPosition;
-        }
-
-        // Based off of Randy Gaul's ImpulseEngine code
-        // https://github.com/RandyGaul/ImpulseEngine/blob/5181fee1648acc4a889b9beec8e13cbe7dac9288/Manifold.cpp#L123a
-        private bool FixClipping(List<Manifold> collisions, float divisions)
-        {
-            const float allowance = 1 / 128.0f;
-            var percent = MathHelper.Clamp(0.4f / divisions, 0.01f, 1f);
-            var done = true;
-            foreach (var collision in collisions)
-            {
-                if (!collision.Hard)
-                {
-                    continue;
-                }
-
-                if (collision.A.Owner.Deleted || collision.B.Owner.Deleted)
-                    continue;
-
-                var penetration = _physicsManager.CalculatePenetration(collision.A, collision.B);
-
-                if (penetration <= allowance)
-                    continue;
-
-                done = false;
-                //var correction = collision.Normal * Math.Abs(penetration) * percent;
-                var correction = collision.Normal * Math.Max(penetration - allowance, 0.0f) / (collision.A.InvMass + collision.B.InvMass) * percent;
-                if (collision.A.CanMove())
-                {
-                    collision.A.Owner.Transform.DeferUpdates = true;
-                    _deferredUpdates.Add(collision.A);
-                    collision.A.Owner.Transform.WorldPosition -= correction * collision.A.InvMass;
-                }
-
-                if (collision.B.CanMove())
-                {
-                    collision.B.Owner.Transform.DeferUpdates = true;
-                    _deferredUpdates.Add(collision.B);
-                    collision.B.Owner.Transform.WorldPosition += correction * collision.B.InvMass;
-                }
-            }
-
-            return done;
-        }
-
-        private (float friction, float gravity) GetFriction(IPhysicsComponent body)
-        {
-            if (!body.OnGround)
-                return (0f, 0f);
-
-            var location = body.Owner.Transform;
-            var grid = _mapManager.GetGrid(location.Coordinates.GetGridId(EntityManager));
-            var tile = grid.GetTileRef(location.Coordinates);
-            var tileDef = _tileDefinitionManager[tile.Tile.TypeId];
-            return (tileDef.Friction, grid.HasGravity ? 9.8f : 0f);
         }
     }
 }

Robust.Shared/Localization/LocalizationManager.cs

@@ -9,7 +9,7 @@ using Fluent.Net.RuntimeAst;
 using JetBrains.Annotations;
 using Robust.Shared.ContentPack;
 using Robust.Shared.GameObjects;
-using Robust.Shared.IoC;
+using Robust.Shared.GameObjects.Components.Localization;using Robust.Shared.IoC;
 using Robust.Shared.Log;
 using Robust.Shared.Serialization;
 using Robust.Shared.Utility;
@@ -31,6 +31,101 @@ namespace Robust.Shared.Localization
             _logSawmill = _log.GetSawmill("loc");
         }
 
+        public bool TryGetEntityLocAttrib(IEntity entity, string attribute, [NotNullWhen(true)] out string? value)
+        {
+            var attributeSource = "";
+
+            if (entity.TryGetComponent<GrammarComponent>(out var grammar) && !string.IsNullOrEmpty(grammar.LocalizationId))
+            {
+                attributeSource = grammar.LocalizationId;
+            }
+            else if(!string.IsNullOrEmpty(entity.Prototype?.LocalizationID))
+            {
+                attributeSource = entity.Prototype.LocalizationID;
+            }
+
+            if (!string.IsNullOrEmpty(attributeSource))
+            {
+                if (TryGetString($"{attributeSource}.{attribute}", out value))
+                {
+                    return true;
+                }
+            }
+
+            value = null;
+            return false;
+        }
+
+        void AddBuiltinFunctions(MessageContext context)
+        {
+            //Grammatical gender
+            context.Functions.Add("GENDER", (args, options) => CallFunction((args) =>
+            {
+                if (args.Args.Count < 1) return new LocValueString("other");
+
+                ILocValue entity0 = args.Args[0];
+                if (entity0.Value != null)
+                {
+                    IEntity entity = (IEntity)entity0.Value;
+
+                    if(entity.TryGetComponent<GrammarComponent>(out var grammar) && grammar.Gender.HasValue)
+                    {
+                        return new LocValueString(grammar.Gender.Value.ToString().ToLowerInvariant());
+                    }
+
+                    if(TryGetEntityLocAttrib(entity, "gender", out var gender))
+                    {
+                        return new LocValueString(gender);
+                    }
+                }
+
+                return new LocValueString("other");
+            }, args, options));
+
+            //Proper nouns
+            context.Functions.Add("PROPER", (args, options) => CallFunction((args) =>
+            {
+                if (args.Args.Count < 1) return new LocValueString("other");
+
+                ILocValue entity0 = args.Args[0];
+                if (entity0.Value != null)
+                {
+                    IEntity entity = (IEntity)entity0.Value;
+
+                    if (entity.TryGetComponent<GrammarComponent>(out var grammar) && grammar.ProperNoun.HasValue)
+                    {
+                        return new LocValueString(grammar.ProperNoun.Value.ToString().ToLowerInvariant());
+                    }
+
+                    if (TryGetEntityLocAttrib(entity, "proper", out var proper))
+                    {
+                        return new LocValueString(proper);
+                    }
+                }
+
+                return new LocValueString("other");
+            }, args, options));
+
+            //Misc Attribs
+            context.Functions.Add("ATTRIB", (args, options) => CallFunction((args) =>
+            {
+                if(args.Args.Count < 2) return new LocValueString("other");
+
+                ILocValue entity0 = args.Args[0];
+                if (entity0.Value != null)
+                {
+                    IEntity entity = (IEntity)entity0.Value;
+                    ILocValue attrib0 = args.Args[1];
+                    if (TryGetEntityLocAttrib(entity, attrib0.Format(new LocContext(context)), out var attrib))
+                    {
+                        return new LocValueString(attrib);
+                    }
+                }
+
+                return new LocValueString("other");
+            }, args, options));
+        }
+
         public string GetString(string messageId)
         {
             if (_defaultCulture == null)
@@ -290,6 +385,7 @@ namespace Robust.Shared.Localization
                     Functions = new Dictionary<string, Resolver.ExternalFunction>(),
                 }
             );
+            AddBuiltinFunctions(context);
 
             _contexts.Add(culture, context);
 

Robust.Shared/Map/MapManager.cs

@@ -6,6 +6,8 @@ using Robust.Shared.GameObjects;
 using Robust.Shared.IoC;
 using Robust.Shared.Log;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics;
+using Robust.Shared.Physics.Dynamics;
 using Robust.Shared.Timing;
 using Robust.Shared.Utility;
 
@@ -446,7 +448,7 @@ namespace Robust.Shared.Map
 
                     var collideComp = newEnt.AddComponent<PhysicsComponent>();
                     collideComp.CanCollide = true;
-                    collideComp.PhysicsShapes.Add(new PhysShapeGrid(grid));
+                    collideComp.AddFixture(new Fixture(collideComp, new PhysShapeGrid(grid)) {CollisionMask = MapGridHelpers.CollisionGroup, CollisionLayer = MapGridHelpers.CollisionGroup});
 
                     newEnt.Transform.AttachParent(_entityManager.GetEntity(_mapEntities[currentMapID]));
 

Robust.Shared/Map/PhysShapeGrid.cs

@@ -1,4 +1,5 @@
 using System;
+using Robust.Shared.Configuration;
 using Robust.Shared.IoC;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
@@ -13,33 +14,32 @@ namespace Robust.Shared.Map
     [Serializable, NetSerializable]
     public class PhysShapeGrid : IPhysShape
     {
+        public int ChildCount => 1;
+
+        public Box2 LocalBounds => _mapGrid.LocalBounds;
+
+        /// <summary>
+        /// The radius of this AABB
+        /// </summary>
+        [ViewVariables(VVAccess.ReadWrite)]
+        public float Radius
+        {
+            get => _radius;
+            set
+            {
+                if (MathHelper.CloseTo(_radius, value)) return;
+                _radius = value;
+            }
+        }
+
+        private float _radius;
+        public ShapeType ShapeType => ShapeType.Polygon;
+
         private GridId _gridId;
 
         [NonSerialized]
         private IMapGridInternal _mapGrid = default!;
 
-        /// <inheritdoc />
-        /// <remarks>
-        /// The collision layer of a grid physics shape cannot be changed.
-        /// </remarks>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public int CollisionLayer
-        {
-            get => MapGridHelpers.CollisionGroup;
-            set { }
-        }
-
-        /// <inheritdoc />
-        /// <remarks>
-        /// The collision mask of a grid physics shape cannot be changed.
-        /// </remarks>
-        [ViewVariables(VVAccess.ReadWrite)]
-        public int CollisionMask
-        {
-            get => MapGridHelpers.CollisionGroup;
-            set { }
-        }
-
         /// <inheritdoc />
         public void ApplyState()
         {
@@ -91,6 +91,8 @@ namespace Robust.Shared.Map
                 var mapMan = IoCManager.Resolve<IMapManager>();
                 _mapGrid = (IMapGridInternal)mapMan.GetGrid(_gridId);
             }
+
+            _radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
         }
 
         public event Action? OnDataChanged { add { } remove { } }
@@ -98,7 +100,14 @@ namespace Robust.Shared.Map
         /// <inheritdoc />
         public Box2 CalculateLocalBounds(Angle rotation)
         {
-            return new Box2Rotated(_mapGrid.LocalBounds, rotation).CalcBoundingBox();
+            return new Box2Rotated(_mapGrid.LocalBounds, rotation).CalcBoundingBox().Scale(1 + Radius);
+        }
+
+        public bool Equals(IPhysShape? other)
+        {
+            if (other is not PhysShapeGrid otherGrid) return false;
+            return MathHelper.CloseTo(_radius, otherGrid._radius) &&
+                   _gridId == otherGrid._gridId;
         }
     }
 }

Robust.Shared/Network/Messages/MsgPlacement.cs

@@ -29,6 +29,7 @@ namespace Robust.Shared.Network.Messages
         public int Range { get; set; }
         public string ObjType { get; set; }
         public string AlignOption { get; set; }
+        public Vector2 RectSize { get; set; }
 
         public override void ReadFromBuffer(NetIncomingMessage buffer)
         {
@@ -57,6 +58,10 @@ namespace Robust.Shared.Network.Messages
                 case PlacementManagerMessage.RequestEntRemove:
                     EntityUid = new EntityUid(buffer.ReadInt32());
                     break;
+                case PlacementManagerMessage.RequestRectRemove:
+                    EntityCoordinates = buffer.ReadEntityCoordinates();
+                    RectSize = buffer.ReadVector2();
+                    break;
             }
         }
 
@@ -87,6 +92,10 @@ namespace Robust.Shared.Network.Messages
                 case PlacementManagerMessage.RequestEntRemove:
                     buffer.Write((int)EntityUid);
                     break;
+                case PlacementManagerMessage.RequestRectRemove:
+                    buffer.Write(EntityCoordinates);
+                    buffer.Write(RectSize);
+                    break;
             }
         }
     }

Robust.Shared/Physics/B2DynamicTree.cs

@@ -26,6 +26,7 @@ using System.Diagnostics;
 using System.Diagnostics.CodeAnalysis;
 using System.Runtime.CompilerServices;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics.Dynamics;
 using Robust.Shared.Utility;
 using Vector2 = Robust.Shared.Maths.Vector2;
 
@@ -368,6 +369,17 @@ namespace Robust.Shared.Physics
             return _nodes[proxy].UserData;
         }
 
+        /// <summary>
+        ///     Get the fat AABB for a proxy.
+        /// </summary>
+        /// <param name="proxyId">The proxy id.</param>
+        /// <param name="fatAABB">The fat AABB.</param>
+        public void GetFatAABB(Proxy proxy, out Box2 fatAABB)
+        {
+            DebugTools.Assert(0 <= proxy && proxy < Capacity);
+            fatAABB = _nodes[proxy].Aabb;
+        }
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool WasMoved(Proxy proxy)
         {

Robust.Shared/Physics/BodyType.cs

@@ -10,27 +10,20 @@ namespace Robust.Shared.Physics
     public enum BodyType : byte
     {
         /// <summary>
-        ///     Will not be processed by the collision system. Basically "out of phase" with the world.
-        ///     They will not raise collision events. Forces are still applied to the body, and it can move.
+        ///     Kinematic objects have to be moved manually and have their forces reset every tick.
         /// </summary>
-        None,
+        Kinematic = 0,
 
         /// <summary>
         ///     Static objects have infinite mass and cannot be moved by forces or collisions. They are solid,
         ///     will collide with other objects, and raise collision events. This is what you use for immovable level geometry.
         /// </summary>
-        Static,
+        Static = 1 << 0,
 
         /// <summary>
         ///     Dynamic objects will respond to collisions and forces. They will raise collision events. This is what
         ///     you use for movable objects in the game.
         /// </summary>
-        Dynamic,
-
-        /// <summary>
-        ///     Trigger objects cannot be moved by collisions or forces. They are not solid and won't block objects.
-        ///     Collision events will still be raised.
-        /// </summary>
-        Trigger,
+        Dynamic = 1 << 1,
     }
 }

Robust.Shared/Physics/BroadPhase.cs

@@ -1,94 +0,0 @@
-using System.Collections;
-using System.Collections.Generic;
-using Robust.Shared.Maths;
-
-namespace Robust.Shared.Physics {
-
-    public sealed class BroadPhase : IBroadPhase {
-
-        private readonly DynamicTree<IPhysBody> _tree;
-
-        public BroadPhase() =>
-            _tree = new DynamicTree<IPhysBody>(
-                (in IPhysBody body) => body.WorldAABB,
-                capacity: 3840,
-                growthFunc: x => x + 256
-            );
-
-        public IEnumerator<IPhysBody> GetEnumerator() => _tree.GetEnumerator();
-
-        IEnumerator IEnumerable.GetEnumerator() => ((IEnumerable) _tree).GetEnumerator();
-
-        void ICollection<IPhysBody>.Add(IPhysBody item) => _tree.Add(item);
-
-        public void Clear() => _tree.Clear();
-
-        public bool Contains(IPhysBody item) => _tree.Contains(item);
-
-        public void CopyTo(IPhysBody[] array, int arrayIndex) => _tree.CopyTo(array, arrayIndex);
-
-        public bool Remove(IPhysBody item) => _tree.Remove(item);
-
-        public int Capacity => _tree.Capacity;
-
-        public int Height => _tree.Height;
-
-        public int MaxBalance => _tree.MaxBalance;
-
-        public float AreaRatio => _tree.AreaRatio;
-
-        public int Count => _tree.Count;
-
-        public bool Add(in IPhysBody item) => _tree.Add(in item);
-
-        public bool Remove(in IPhysBody item) => _tree.Remove(in item);
-
-        public bool Update(in IPhysBody item) => _tree.Update(in item);
-
-        public void QueryAabb(DynamicTree<IPhysBody>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
-        {
-            _tree.QueryAabb(callback, aabb, 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 IEnumerable<IPhysBody> QueryAabb(Box2 aabb, bool approx = false)
-        {
-            return _tree.QueryAabb(aabb, 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 Ray ray, bool approx = false) =>
-            _tree.QueryRay(callback, ray, approx);
-
-        public void QueryRay<TState>(ref TState state, DynamicTree<IPhysBody>.RayQueryCallbackDelegate<TState> callback, in Ray ray,
-            bool approx = false)
-        {
-            _tree.QueryRay(ref state, callback, ray, approx);
-        }
-
-        public bool IsReadOnly => _tree.IsReadOnly;
-
-
-    }
-
-}

Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs

@@ -0,0 +1,352 @@
+using System;
+using System.Collections.Generic;
+using Robust.Shared.Map;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Dynamics;
+
+namespace Robust.Shared.Physics.Broadphase
+{
+    public class DynamicTreeBroadPhase : IBroadPhase
+    {
+        public MapId MapId { get; set; }
+
+        public GridId GridId { get; set; }
+
+        // TODO: DynamicTree seems slow at updates when we have large entity counts so when we have boxstation
+        // need to suss out whether chunking it might be useful.
+        private B2DynamicTree<FixtureProxy> _tree = new(capacity: 256);
+
+        private readonly DynamicTree<FixtureProxy>.ExtractAabbDelegate _extractAabb = ExtractAabbFunc;
+
+        private DynamicTree.Proxy[] _moveBuffer;
+        private int _moveCapacity;
+        private int _moveCount;
+
+        private (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[] _pairBuffer;
+        private int _pairCapacity;
+        private int _pairCount;
+        private int _proxyCount;
+        private B2DynamicTree<FixtureProxy>.QueryCallback _queryCallback;
+        private DynamicTree.Proxy _queryProxyId;
+
+        public DynamicTreeBroadPhase(MapId mapId, GridId gridId)
+        {
+            MapId = mapId;
+            GridId = gridId;
+
+            _queryCallback = QueryCallback;
+            _proxyCount = 0;
+
+            _pairCapacity = 16;
+            _pairCount = 0;
+            _pairBuffer = new (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[_pairCapacity];
+
+            _moveCapacity = 16;
+            _moveCount = 0;
+            _moveBuffer = new DynamicTree.Proxy[_moveCapacity];
+        }
+
+        private static Box2 ExtractAabbFunc(in FixtureProxy proxy)
+        {
+            return proxy.AABB;
+        }
+
+        public void UpdatePairs(BroadPhaseDelegate callback)
+        {
+            // Reset pair buffer
+            _pairCount = 0;
+
+            // Perform tree queries for all moving proxies.
+            for (int j = 0; j < _moveCount; ++j)
+            {
+                _queryProxyId = _moveBuffer[j];
+                if (_queryProxyId == DynamicTree.Proxy.Free)
+                {
+                    continue;
+                }
+
+                // We have to query the tree with the fat AABB so that
+                // we don't fail to create a pair that may touch later.
+                Box2 fatAABB;
+                _tree.GetFatAABB(_queryProxyId, out fatAABB);
+
+                // Query tree, create pairs and add them pair buffer.
+                _tree.Query(_queryCallback, in fatAABB);
+            }
+
+            // Reset move buffer
+            _moveCount = 0;
+
+            // Sort the pair buffer to expose duplicates.
+            Array.Sort(_pairBuffer, 0, _pairCount);
+
+            // Send the pairs back to the client.
+            int i = 0;
+            while (i < _pairCount)
+            {
+                var primaryPair = _pairBuffer[i];
+                FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyA)!;
+                FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyB)!;
+
+                callback(GridId, in userDataA, in userDataB);
+                ++i;
+
+                // Skip any duplicate pairs.
+                while (i < _pairCount)
+                {
+                    (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB) pair = _pairBuffer[i];
+                    if (pair.ProxyA != primaryPair.ProxyA || pair.ProxyB != primaryPair.ProxyB)
+                    {
+                        break;
+                    }
+                    ++i;
+                }
+            }
+
+            // Try to keep the tree balanced.
+            //_tree.Rebalance(4);
+        }
+
+        /// <summary>
+        /// This is called from DynamicTree.Query when we are gathering pairs.
+        /// </summary>
+        /// <param name="proxyId"></param>
+        /// <returns></returns>
+        private bool QueryCallback(DynamicTree.Proxy proxyId)
+        {
+            // A proxy cannot form a pair with itself.
+            if (proxyId == _queryProxyId)
+            {
+                return true;
+            }
+
+            // Grow the pair buffer as needed.
+            if (_pairCount == _pairCapacity)
+            {
+                (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[] oldBuffer = _pairBuffer;
+                _pairCapacity *= 2;
+                _pairBuffer = new (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[_pairCapacity];
+                Array.Copy(oldBuffer, _pairBuffer, _pairCount);
+            }
+
+            _pairBuffer[_pairCount].ProxyA = new DynamicTree.Proxy(Math.Min(proxyId, _queryProxyId));
+            _pairBuffer[_pairCount].ProxyB = new DynamicTree.Proxy(Math.Max(proxyId, _queryProxyId));
+            _pairCount++;
+
+            return true;
+        }
+
+        // TODO: Refactor to use fatAABB
+        /// <summary>
+        ///     Already assumed to be within the same broadphase.
+        /// </summary>
+        /// <param name="proxyIdA"></param>
+        /// <param name="proxyIdB"></param>
+        /// <returns></returns>
+        public bool TestOverlap(DynamicTree.Proxy proxyIdA, DynamicTree.Proxy proxyIdB)
+        {
+            var proxyA = _tree.GetUserData(proxyIdA);
+            var proxyB = _tree.GetUserData(proxyIdB);
+
+            if (proxyA == null || proxyB == null) return false;
+
+            return proxyB.AABB.Intersects(proxyA.AABB);
+        }
+
+        public DynamicTree.Proxy AddProxy(ref FixtureProxy proxy)
+        {
+            var proxyID = _tree.CreateProxy(proxy.AABB, proxy);
+            _proxyCount++;
+            BufferMove(proxyID);
+            return proxyID;
+        }
+
+        public void MoveProxy(DynamicTree.Proxy proxy, in Box2 aabb, Vector2 displacement)
+        {
+            var buffer = _tree.MoveProxy(proxy, in aabb, displacement);
+            if (buffer)
+            {
+                BufferMove(proxy);
+            }
+        }
+
+        public void TouchProxy(DynamicTree.Proxy proxy)
+        {
+            BufferMove(proxy);
+        }
+
+        private void BufferMove(DynamicTree.Proxy proxyId)
+        {
+            if (_moveCount == _moveCapacity)
+            {
+                DynamicTree.Proxy[] oldBuffer = _moveBuffer;
+                _moveCapacity *= 2;
+                _moveBuffer = new DynamicTree.Proxy[_moveCapacity];
+                Array.Copy(oldBuffer, _moveBuffer, _moveCount);
+            }
+
+            _moveBuffer[_moveCount] = proxyId;
+            _moveCount++;
+        }
+
+        private void UnBufferMove(int proxyId)
+        {
+            for (int i = 0; i < _moveCount; ++i)
+            {
+                if (_moveBuffer[i] == proxyId)
+                {
+                    _moveBuffer[i] = DynamicTree.Proxy.Free;
+                }
+            }
+        }
+
+        public void RemoveProxy(DynamicTree.Proxy proxy)
+        {
+            UnBufferMove(proxy);
+            _proxyCount--;
+            _tree.DestroyProxy(proxy);
+        }
+
+        public void QueryAABB(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
+        {
+            QueryAabb(ref callback, EasyQueryCallback, aabb, approx);
+        }
+
+        public FixtureProxy? GetProxy(DynamicTree.Proxy proxy)
+        {
+            return _tree.GetUserData(proxy);
+        }
+
+        public void QueryAabb(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
+        {
+            QueryAabb(ref callback, EasyQueryCallback, aabb, approx);
+        }
+
+        public void QueryAabb<TState>(ref TState state, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx = false)
+        {
+            var tuple = (state, _tree, callback, aabb, approx, _extractAabb);
+            _tree.Query(ref tuple, DelegateCache<TState>.AabbQueryState, aabb);
+            state = tuple.state;
+        }
+
+        public IEnumerable<FixtureProxy> QueryAabb(Box2 aabb, bool approx = false)
+        {
+            var list = new List<FixtureProxy>();
+
+            QueryAabb(ref list, (ref List<FixtureProxy> lst, in FixtureProxy i) =>
+            {
+                lst.Add(i);
+                return true;
+            }, aabb, approx);
+
+            return list;
+        }
+
+        public void QueryPoint(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Vector2 point, bool approx = false)
+        {
+            QueryPoint(ref callback, EasyQueryCallback, point, approx);
+        }
+
+        public void QueryPoint<TState>(ref TState state, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Vector2 point, bool approx = false)
+        {
+            var tuple = (state, _tree, callback, point, approx, _extractAabb);
+            _tree.Query(ref tuple,
+                (ref (TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Vector2 point, bool approx, DynamicTree<FixtureProxy>.ExtractAabbDelegate extract) tuple,
+                    DynamicTree.Proxy proxy) =>
+                {
+                    var item = tuple.tree.GetUserData(proxy)!;
+
+                    if (!tuple.approx)
+                    {
+                        var precise = tuple.extract(item);
+                        if (!precise.Contains(tuple.point))
+                        {
+                            return true;
+                        }
+                    }
+
+                    return tuple.callback(ref tuple.state, item);
+                }, Box2.CenteredAround(point, new Vector2(0.1f, 0.1f)));
+            state = tuple.state;
+        }
+
+        public IEnumerable<FixtureProxy> QueryPoint(Vector2 point, bool approx = false)
+        {
+            var list = new List<FixtureProxy>();
+
+            QueryPoint(ref list, (ref List<FixtureProxy> list, in FixtureProxy i) =>
+            {
+                list.Add(i);
+                return true;
+            }, point, approx);
+
+            return list;
+        }
+
+        private static readonly DynamicTree<FixtureProxy>.QueryCallbackDelegate<DynamicTree<FixtureProxy>.QueryCallbackDelegate> EasyQueryCallback =
+            (ref DynamicTree<FixtureProxy>.QueryCallbackDelegate s, in FixtureProxy v) => s(v);
+
+        public void QueryRay<TState>(ref TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback, in Ray ray, bool approx = false)
+        {
+            var tuple = (state, callback, _tree, approx ? null : _extractAabb, ray);
+            _tree.RayCast(ref tuple, DelegateCache<TState>.RayQueryState, ray);
+            state = tuple.state;
+        }
+
+        private static bool AabbQueryStateCallback<TState>(ref (TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx, DynamicTree<FixtureProxy>.ExtractAabbDelegate extract) tuple, DynamicTree.Proxy proxy)
+        {
+            var item = tuple.tree.GetUserData(proxy)!;
+            if (!tuple.approx)
+            {
+                var precise = tuple.extract(item);
+                if (!precise.Intersects(tuple.aabb))
+                {
+                    return true;
+                }
+            }
+
+            return tuple.callback(ref tuple.state, item);
+        }
+
+        private static bool RayQueryStateCallback<TState>(ref (TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.ExtractAabbDelegate? extract, Ray srcRay) tuple, DynamicTree.Proxy proxy, in Vector2 hitPos, float distance)
+        {
+            var item = tuple.tree.GetUserData(proxy)!;
+            var hit = hitPos;
+
+            if (tuple.extract != null)
+            {
+                var precise = tuple.extract(item);
+                if (!tuple.srcRay.Intersects(precise, out distance, out hit))
+                {
+                    return true;
+                }
+            }
+
+            return tuple.callback(ref tuple.state, item, hit, distance);
+        }
+
+        public void QueryRay(DynamicTree<FixtureProxy>.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false)
+        {
+            QueryRay(ref callback, RayQueryDelegateCallbackInst, ray, approx);
+        }
+
+        private static readonly DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<DynamicTree<FixtureProxy>.RayQueryCallbackDelegate> RayQueryDelegateCallbackInst = RayQueryDelegateCallback;
+
+        private static bool RayQueryDelegateCallback(ref DynamicTree<FixtureProxy>.RayQueryCallbackDelegate state, in FixtureProxy value, in Vector2 point, float distFromOrigin)
+        {
+            return state(value, point, distFromOrigin);
+        }
+
+        private static class DelegateCache<TState>
+        {
+            public static readonly
+                B2DynamicTree<FixtureProxy>.QueryCallback<(TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx, DynamicTree<FixtureProxy>.ExtractAabbDelegate extract)> AabbQueryState =
+                    AabbQueryStateCallback;
+
+            public static readonly
+                B2DynamicTree<FixtureProxy>.RayQueryCallback<(TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback,
+                    B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.ExtractAabbDelegate? extract, Ray srcRay)> RayQueryState =
+                    RayQueryStateCallback;
+        }
+    }
+}

Robust.Shared/Physics/BroadPhase/SharedBroadPhaseSystem.cs

@@ -0,0 +1,935 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using Robust.Server.GameObjects;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Map;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Utility;
+
+namespace Robust.Shared.Physics.Broadphase
+{
+    public abstract class SharedBroadPhaseSystem : EntitySystem
+    {
+        /*
+         * That's right both the system implements IBroadPhase and also each grid has its own as well.
+         * The reason for this is other stuff should just be able to check for broadphase with no regard
+         * for the concept of grids, whereas internally this needs to worry about it.
+         */
+
+        // Anything in a container is removed from the graph and anything removed from a container is added to the graph.
+
+        /*
+         * So the Box2D derivatives just use a generic "SynchronizeFixtures" method but it's kinda obtuse so
+         * I just made other methods (AddFixture, RefreshFixtures, etc.) that are clearer on what they're doing.
+         */
+
+        [Dependency] private readonly IMapManager _mapManager = default!;
+
+        private readonly Dictionary<MapId, Dictionary<GridId, IBroadPhase>> _graph = new();
+
+        private Dictionary<IPhysBody, List<IBroadPhase>> _lastBroadPhases = new();
+
+        /// <summary>
+        ///     Given MoveEvent and RotateEvent do the same thing we won't double up on work.
+        /// </summary>
+        private HashSet<IEntity> _handledThisTick = new();
+
+        private Queue<MoveEvent> _queuedMoveEvents = new();
+        private Queue<RotateEvent> _queuedRotateEvent = new();
+        private Queue<EntMapIdChangedMessage> _queuedMapChanges = new();
+        private Queue<FixtureUpdateMessage> _queuedFixtureUpdates = new();
+        private Queue<CollisionChangeMessage> _queuedCollisionChanges = new();
+        private Queue<EntInsertedIntoContainerMessage> _queuedContainerInsert = new();
+        private Queue<EntRemovedFromContainerMessage> _queuedContainerRemove = new();
+
+        private IEnumerable<IBroadPhase> BroadPhases()
+        {
+            foreach (var (_, grids) in _graph)
+            {
+                foreach (var (_, broad) in grids)
+                {
+                    yield return broad;
+                }
+            }
+        }
+
+        /// <summary>
+        ///     Gets the corresponding broadphase for this grid.
+        /// </summary>
+        /// <param name="mapId"></param>
+        /// <param name="gridId"></param>
+        /// <returns>null if broadphase already destroyed or none exists</returns>
+        public IBroadPhase? GetBroadPhase(MapId mapId, GridId gridId)
+        {
+            // Might be null if the grid is being instantiated.
+            if (mapId == MapId.Nullspace || !_graph[mapId].TryGetValue(gridId, out var grid))
+                return null;
+
+            return grid;
+        }
+
+        public ICollection<IBroadPhase> GetBroadPhases(MapId mapId)
+        {
+            return _graph[mapId].Values;
+        }
+
+        public IEnumerable<IBroadPhase> GetBroadPhases(PhysicsComponent body)
+        {
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases)) return Array.Empty<IBroadPhase>();
+            return broadPhases;
+        }
+
+        // Look for now I've hardcoded grids
+        public IEnumerable<(IBroadPhase Broadphase, GridId GridId)> GetBroadphases(PhysicsComponent body)
+        {
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases)) yield break;
+
+            foreach (var (_, grids) in _graph)
+            {
+                foreach (var broad in broadPhases)
+                {
+                    foreach (var (gridId, broadPhase) in grids)
+                    {
+                        if (broad == broadPhase)
+                        {
+                            yield return (broadPhase, gridId);
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        public bool TestOverlap(FixtureProxy proxyA, FixtureProxy proxyB)
+        {
+            // TODO: This should only ever be called on the same grid I think so maybe just assert
+            var mapA = proxyA.Fixture.Body.Owner.Transform.MapID;
+            var mapB = proxyB.Fixture.Body.Owner.Transform.MapID;
+
+            if (mapA != mapB)
+                return false;
+
+            return proxyA.AABB.Intersects(proxyB.AABB);
+        }
+
+        /// <summary>
+        ///     Get the percentage that 2 bodies overlap. Ignores whether collision is turned on for either body.
+        /// </summary>
+        /// <param name="bodyA"></param>
+        /// <param name="bodyB"></param>
+        /// <returns> 0 -> 1.0f based on WorldAABB overlap</returns>
+        public float IntersectionPercent(PhysicsComponent bodyA, PhysicsComponent bodyB)
+        {
+            // TODO: Use actual shapes and not just the AABB?
+            return bodyA.GetWorldAABB(_mapManager).IntersectPercentage(bodyB.GetWorldAABB(_mapManager));
+        }
+
+        public override void Initialize()
+        {
+            base.Initialize();
+
+            SubscribeLocalEvent<CollisionChangeMessage>(QueueCollisionChange);
+            SubscribeLocalEvent<MoveEvent>(QueuePhysicsMove);
+            SubscribeLocalEvent<RotateEvent>(QueuePhysicsRotate);
+            SubscribeLocalEvent<EntMapIdChangedMessage>(QueueMapChange);
+            SubscribeLocalEvent<EntInsertedIntoContainerMessage>(QueueContainerInsertMessage);
+            SubscribeLocalEvent<EntRemovedFromContainerMessage>(QueueContainerRemoveMessage);
+            SubscribeLocalEvent<FixtureUpdateMessage>(QueueFixtureUpdate);
+            _mapManager.OnGridCreated += HandleGridCreated;
+            _mapManager.OnGridRemoved += HandleGridRemoval;
+            _mapManager.MapCreated += HandleMapCreated;
+        }
+
+        public override void Update(float frameTime)
+        {
+            base.Update(frameTime);
+
+            while (_queuedMoveEvents.Count > 0)
+            {
+                var moveEvent = _queuedMoveEvents.Dequeue();
+
+                // Doing this seems to fuck with tp so leave off for now I guess, it's mainly to avoid the rotate duplication
+                if (_handledThisTick.Contains(moveEvent.Sender)) continue;
+
+                _handledThisTick.Add(moveEvent.Sender);
+
+                if (moveEvent.Sender.Deleted || !moveEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent)) continue;
+
+                SynchronizeFixtures(physicsComponent, moveEvent.NewPosition.ToMapPos(EntityManager) - moveEvent.OldPosition.ToMapPos(EntityManager));
+            }
+
+            while (_queuedRotateEvent.Count > 0)
+            {
+                var rotateEvent = _queuedRotateEvent.Dequeue();
+
+                if (_handledThisTick.Contains(rotateEvent.Sender)) continue;
+
+                _handledThisTick.Add(rotateEvent.Sender);
+
+                if (rotateEvent.Sender.Deleted || !rotateEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent))
+                    return;
+
+                SynchronizeFixtures(physicsComponent, Vector2.Zero);
+            }
+
+            _handledThisTick.Clear();
+
+            // TODO: Just call ProcessEventQueue directly?
+            // Manually manage queued stuff ourself given EventBus.QueueEvent happens at the same time every time
+            while (_queuedMapChanges.Count > 0)
+            {
+                HandleMapChange(_queuedMapChanges.Dequeue());
+            }
+
+            while (_queuedContainerInsert.Count > 0)
+            {
+                HandleContainerInsert(_queuedContainerInsert.Dequeue());
+            }
+
+            while (_queuedContainerRemove.Count > 0)
+            {
+                HandleContainerRemove(_queuedContainerRemove.Dequeue());
+            }
+
+            while (_queuedCollisionChanges.Count > 0)
+            {
+                var message = _queuedCollisionChanges.Dequeue();
+                if (message.CanCollide && !message.Body.Deleted)
+                {
+                    AddBody(message.Body);
+                }
+                else
+                {
+                    RemoveBody(message.Body);
+                }
+            }
+
+            while (_queuedFixtureUpdates.Count > 0)
+            {
+                var message = _queuedFixtureUpdates.Dequeue();
+                RefreshFixture(message.Body, message.Fixture);
+            }
+        }
+
+        public override void Shutdown()
+        {
+            base.Shutdown();
+            UnsubscribeLocalEvent<CollisionChangeMessage>();
+            UnsubscribeLocalEvent<MoveEvent>();
+            UnsubscribeLocalEvent<RotateEvent>();
+            UnsubscribeLocalEvent<EntMapIdChangedMessage>();
+            UnsubscribeLocalEvent<EntInsertedIntoContainerMessage>();
+            UnsubscribeLocalEvent<EntRemovedFromContainerMessage>();
+            UnsubscribeLocalEvent<FixtureUpdateMessage>();
+            _mapManager.OnGridCreated -= HandleGridCreated;
+            _mapManager.OnGridRemoved -= HandleGridRemoval;
+            _mapManager.MapCreated -= HandleMapCreated;
+        }
+
+        private void QueuePhysicsMove(MoveEvent moveEvent)
+        {
+            _queuedMoveEvents.Enqueue(moveEvent);
+        }
+
+        private void QueuePhysicsRotate(RotateEvent rotateEvent)
+        {
+            if (!rotateEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent))
+                return;
+
+            SynchronizeFixtures(physicsComponent, Vector2.Zero);
+        }
+
+        private void QueueCollisionChange(CollisionChangeMessage message)
+        {
+            _queuedCollisionChanges.Enqueue(message);
+        }
+
+        private void QueueMapChange(EntMapIdChangedMessage message)
+        {
+            _queuedMapChanges.Enqueue(message);
+        }
+
+        private void QueueContainerInsertMessage(EntInsertedIntoContainerMessage message)
+        {
+            _queuedContainerInsert.Enqueue(message);
+        }
+
+        private void QueueContainerRemoveMessage(EntRemovedFromContainerMessage message)
+        {
+            _queuedContainerRemove.Enqueue(message);
+        }
+
+        private void HandleContainerInsert(EntInsertedIntoContainerMessage message)
+        {
+            if (!message.Entity.Deleted && message.Entity.TryGetComponent(out IPhysBody? physicsComponent))
+            {
+                physicsComponent.CanCollide = false;
+                physicsComponent.Awake = false;
+            }
+        }
+
+        private void HandleContainerRemove(EntRemovedFromContainerMessage message)
+        {
+            if (!message.Entity.Deleted && message.Entity.TryGetComponent(out IPhysBody? physicsComponent))
+            {
+                physicsComponent.CanCollide = true;
+                physicsComponent.Awake = true;
+            }
+        }
+
+        private void QueueFixtureUpdate(FixtureUpdateMessage message)
+        {
+            _queuedFixtureUpdates.Enqueue(message);
+        }
+
+        public void AddBroadPhase(PhysicsComponent body, IBroadPhase broadPhase)
+        {
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                return;
+            }
+
+            if (broadPhases.Contains(broadPhase)) return;
+            broadPhases.Add(broadPhase);
+        }
+
+        /// <summary>
+        ///     Handles map changes for bodies completely
+        /// </summary>
+        /// <param name="message"></param>
+        private void HandleMapChange(EntMapIdChangedMessage message)
+        {
+            if (message.Entity.Deleted ||
+                !message.Entity.TryGetComponent(out PhysicsComponent? body) ||
+                !_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                return;
+            }
+
+            if (Get<SharedPhysicsSystem>().Maps.TryGetValue(message.OldMapId, out var oldMap))
+            {
+                oldMap.RemoveBody(body);
+            }
+
+            body.ClearProxies();
+
+            if (Get<SharedPhysicsSystem>().Maps.TryGetValue(message.Entity.Transform.MapID, out var newMap))
+            {
+                newMap.AddBody(body);
+                body.CreateProxies();
+                SetBroadPhases(body);
+            }
+        }
+
+        private void SetBroadPhases(IPhysBody body)
+        {
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                broadPhases = new List<IBroadPhase>();
+                _lastBroadPhases[body] = broadPhases;
+            }
+
+            broadPhases.Clear();
+
+            foreach (var fixture in body.Fixtures)
+            {
+                foreach (var (gridId, _) in fixture.Proxies)
+                {
+                    var broadPhase = GetBroadPhase(body.Owner.Transform.MapID, gridId);
+                    if (broadPhase == null) continue;
+                    broadPhases.Add(broadPhase);
+                }
+            }
+        }
+
+        private void HandleGridCreated(GridId gridId)
+        {
+            var mapId = _mapManager.GetGrid(gridId).ParentMapId;
+
+            if (!_graph.TryGetValue(mapId, out var grids))
+            {
+                grids = new Dictionary<GridId, IBroadPhase>();
+                _graph[mapId] = grids;
+            }
+
+            grids[gridId] = new DynamicTreeBroadPhase(mapId, gridId);
+        }
+
+        private void HandleMapCreated(object? sender, MapEventArgs eventArgs)
+        {
+            _graph[eventArgs.Map] = new Dictionary<GridId, IBroadPhase>()
+            {
+                {
+                    GridId.Invalid,
+                    new DynamicTreeBroadPhase(eventArgs.Map, GridId.Invalid)
+                }
+            };
+
+        }
+
+        private void HandleGridRemoval(GridId gridId)
+        {
+            foreach (var (_, grids) in _graph)
+            {
+                if (!grids.TryGetValue(gridId, out var broadPhase)) continue;
+
+                var bodyCleanup = false;
+                var toCleanup = new List<IPhysBody>();
+                // Need to cleanup every body that was touching this grid.
+                foreach (var (body, broadPhases) in _lastBroadPhases)
+                {
+                    if (broadPhases.Contains(broadPhase))
+                    {
+                        toCleanup.Add(body);
+                    }
+                }
+
+                foreach (var body in toCleanup)
+                {
+                    RemoveBody(body);
+                }
+
+                grids.Remove(gridId);
+
+                foreach (var body in toCleanup)
+                {
+                    AddBody(body);
+                }
+            }
+        }
+
+        public void AddBody(IPhysBody body)
+        {
+            if (_lastBroadPhases.ContainsKey(body))
+            {
+                return;
+            }
+
+            var mapId = body.Owner.Transform.MapID;
+
+            if (mapId == MapId.Nullspace)
+            {
+                _lastBroadPhases[body] = new List<IBroadPhase>();
+                return;
+            }
+
+            foreach (var fixture in body.Fixtures)
+            {
+                DebugTools.Assert(fixture.Proxies.Count == 0, "Can't add a body to broadphase when it already has proxies!");
+            }
+
+            var broadPhases = new List<IBroadPhase>();
+            _lastBroadPhases[body] = broadPhases;
+
+            body.CreateProxies(_mapManager, this);
+            SetBroadPhases(body);
+
+            // TODO: Remove this garbage
+            EntityManager.UpdateEntityTree(body.Owner);
+        }
+
+        public void RemoveBody(IPhysBody body)
+        {
+            if (!_lastBroadPhases.ContainsKey(body))
+            {
+                return;
+            }
+
+            body.ClearProxies();
+
+            // TODO: Remove after pvs refactor
+            if (!body.Owner.Deleted)
+            {
+                EntityManager.UpdateEntityTree(body.Owner);
+            }
+
+            _lastBroadPhases.Remove(body);
+        }
+
+        /// <summary>
+        ///     Recreates this fixture in the relevant broadphases.
+        /// </summary>
+        /// <param name="body"></param>
+        /// <param name="fixture"></param>
+        public void RefreshFixture(PhysicsComponent body, Fixture fixture)
+        {
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                return;
+            }
+
+            var mapId = body.Owner.Transform.MapID;
+
+            if (mapId == MapId.Nullspace || body.Deleted)
+            {
+                return;
+            }
+
+            fixture.ClearProxies(mapId, this);
+            fixture.CreateProxies(_mapManager, this);
+
+            // Need to update what broadphases are relevant.
+            SetBroadPhases(body);
+        }
+
+        internal void AddFixture(PhysicsComponent body, Fixture fixture)
+        {
+            // If the entity's still being initialized it might have MoveEvent called (might change in future?)
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                return;
+            }
+
+            var mapId = body.Owner.Transform.MapID;
+            DebugTools.Assert(fixture.Proxies.Count == 0);
+
+            if (mapId == MapId.Nullspace || body.Deleted)
+            {
+                body.ClearProxies();
+                return;
+            }
+
+            broadPhases.Clear();
+            fixture.CreateProxies(_mapManager, this);
+
+            foreach (var fix in body.Fixtures)
+            {
+                foreach (var (gridId, _) in fix.Proxies)
+                {
+                    var broadPhase = GetBroadPhase(mapId, gridId);
+                    if (broadPhase == null) continue;
+                    broadPhases.Add(broadPhase);
+                }
+            }
+        }
+
+        internal void RemoveFixture(PhysicsComponent body, Fixture fixture)
+        {
+            // If the entity's still being initialized it might have MoveEvent called (might change in future?)
+            if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
+            {
+                return;
+            }
+
+            var mapId = body.Owner.Transform.MapID;
+            fixture.ClearProxies(mapId, this);
+
+            if (mapId == MapId.Nullspace || body.Deleted)
+            {
+                body.ClearProxies();
+                return;
+            }
+
+            // Need to re-build the broadphases.
+            broadPhases.Clear();
+
+            foreach (var fix in body.Fixtures)
+            {
+                foreach (var (gridId, _) in fix.Proxies)
+                {
+                    var broadPhase = GetBroadPhase(mapId, gridId);
+                    if (broadPhase == null) continue;
+                    broadPhases.Add(broadPhase);
+                }
+            }
+        }
+
+        /// <summary>
+        ///     Move all of the fixtures on this body.
+        /// </summary>
+        /// <param name="body"></param>
+        /// <param name="displacement"></param>
+        private void SynchronizeFixtures(PhysicsComponent body, Vector2 displacement)
+        {
+            // If the entity's still being initialized it might have MoveEvent called (might change in future?)
+            if (!_lastBroadPhases.TryGetValue(body, out var oldBroadPhases))
+            {
+                return;
+            }
+
+            var mapId = body.Owner.Transform.MapID;
+            var worldAABB = body.GetWorldAABB(_mapManager);
+
+            var newBroadPhases = _mapManager
+                .FindGridIdsIntersecting(mapId, worldAABB, true)
+                .Select(gridId => GetBroadPhase(mapId, gridId))
+                .ToArray();
+
+            // Remove from old broadphases
+            foreach (var broadPhase in oldBroadPhases.ToArray())
+            {
+                if (newBroadPhases.Contains(broadPhase)) continue;
+
+                var gridId = GetGridId(broadPhase);
+
+                foreach (var fixture in body.Fixtures)
+                {
+                    fixture.ClearProxies(mapId, this, gridId);
+                }
+
+                oldBroadPhases.Remove(broadPhase);
+            }
+
+            // Update retained broadphases
+            // TODO: These will need swept broadPhases
+            var offset = body.Owner.Transform.WorldPosition;
+            var worldRotation = body.Owner.Transform.WorldRotation;
+
+            foreach (var broadPhase in oldBroadPhases)
+            {
+                if (!newBroadPhases.Contains(broadPhase)) continue;
+
+                var gridId = GetGridId(broadPhase);
+
+                foreach (var fixture in body.Fixtures)
+                {
+                    if (!fixture.Proxies.TryGetValue(gridId, out var proxies)) continue;
+
+                    foreach (var proxy in proxies)
+                    {
+                        double gridRotation = worldRotation;
+
+                        if (gridId != GridId.Invalid)
+                        {
+                            var grid = _mapManager.GetGrid(gridId);
+                            offset -= grid.WorldPosition;
+                            // TODO: Should probably have a helper for this
+                            gridRotation = worldRotation - body.Owner.EntityManager.GetEntity(grid.GridEntityId).Transform.WorldRotation;
+                        }
+
+                        var aabb = fixture.Shape.CalculateLocalBounds(gridRotation).Translated(offset);
+                        proxy.AABB = aabb;
+
+                        broadPhase.MoveProxy(proxy.ProxyId, in aabb, displacement);
+                    }
+                }
+            }
+
+            // Add to new broadphases
+            foreach (var broadPhase in newBroadPhases)
+            {
+                if (broadPhase == null || oldBroadPhases.Contains(broadPhase)) continue;
+                var gridId = GetGridId(broadPhase);
+                oldBroadPhases.Add(broadPhase);
+
+                foreach (var fixture in body.Fixtures)
+                {
+                    DebugTools.Assert(!fixture.Proxies.ContainsKey(gridId));
+
+                    fixture.CreateProxies(broadPhase, _mapManager, this);
+                }
+            }
+        }
+
+        public GridId GetGridId(IBroadPhase broadPhase)
+        {
+            foreach (var (_, grids) in _graph)
+            {
+                foreach (var (gridId, broad) in grids)
+                {
+                    if (broadPhase == broad)
+                    {
+                        return gridId;
+                    }
+                }
+            }
+
+            throw new InvalidOperationException("Unable to find GridId for broadPhase");
+        }
+
+        // This is dirty but so is a lot of other shit so it'll get refactored at some stage tm
+        public List<PhysicsComponent> GetAwakeBodies(MapId mapId, GridId gridId)
+        {
+            var bodies = new List<PhysicsComponent>();
+            var map = Get<SharedPhysicsSystem>().Maps[mapId];
+
+            foreach (var body in map.AwakeBodies)
+            {
+                if (body.Owner.Transform.GridID == gridId)
+                    bodies.Add(body);
+            }
+
+            return bodies;
+        }
+
+        #region Queries
+
+        /// <summary>
+        /// Checks to see if the specified collision rectangle collides with any of the physBodies under management.
+        /// Also fires the OnCollide event of the first managed physBody to intersect with the collider.
+        /// </summary>
+        /// <param name="collider">Collision rectangle to check</param>
+        /// <param name="mapId">Map to check on</param>
+        /// <param name="approximate"></param>
+        /// <returns>true if collides, false if not</returns>
+        public bool TryCollideRect(Box2 collider, MapId mapId, bool approximate = true)
+        {
+            var state = (collider, mapId, found: false);
+
+            foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, collider, true))
+            {
+                var gridCollider = _mapManager.GetGrid(gridId).WorldToLocal(collider.Center);
+                var gridBox = collider.Translated(gridCollider);
+
+                _graph[mapId][gridId].QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
+                {
+                    if (proxy.Fixture.CollisionLayer == 0x0)
+                        return true;
+
+                    if (proxy.AABB.Intersects(gridBox))
+                    {
+                        state.found = true;
+                        return false;
+                    }
+                    return true;
+                }, gridBox, approximate);
+            }
+
+            return state.found;
+        }
+
+        public IEnumerable<PhysicsComponent> GetCollidingEntities(PhysicsComponent body, Vector2 offset, bool approximate = true)
+        {
+            // If the body has just had its collision enabled or disabled it may not be ready yet so we'll wait a tick.
+            if (!body.CanCollide || body.Owner.Transform.MapID == MapId.Nullspace)
+            {
+                return Array.Empty<PhysicsComponent>();
+            }
+
+            // Unfortunately due to the way grids are currently created we have to queue CanCollide event changes, hence we need to do this here.
+            if (!_lastBroadPhases.ContainsKey(body))
+            {
+                AddBody(body);
+            }
+
+            var modifiers = body.Entity.GetAllComponents<ICollideSpecial>();
+            var entities = new List<PhysicsComponent>();
+
+            var state = (body, modifiers, entities);
+
+            foreach (var broadPhase in _lastBroadPhases[body])
+            {
+                foreach (var fixture in body.Fixtures)
+                {
+                    foreach (var proxy in fixture.Proxies[GetGridId(broadPhase)])
+                    {
+                        broadPhase.QueryAabb(ref state,
+                            (ref (PhysicsComponent body, IEnumerable<ICollideSpecial> modifiers, List<PhysicsComponent> entities) state,
+                                in FixtureProxy other) =>
+                            {
+                                if (other.Fixture.Body.Deleted || other.Fixture.Body == body) return true;
+                                if ((proxy.Fixture.CollisionMask & other.Fixture.CollisionLayer) == 0x0) return true;
+
+                                var preventCollision = false;
+                                var otherModifiers = other.Fixture.Body.Owner.GetAllComponents<ICollideSpecial>();
+
+                                foreach (var modifier in state.modifiers)
+                                {
+                                    preventCollision |= modifier.PreventCollide(other.Fixture.Body);
+                                }
+                                foreach (var modifier in otherModifiers)
+                                {
+                                    preventCollision |= modifier.PreventCollide(body);
+                                }
+
+                                if (preventCollision)
+                                    return true;
+
+                                state.entities.Add(other.Fixture.Body);
+                                return true;
+                            }, proxy.AABB, approximate);
+                    }
+                }
+            }
+
+            return entities;
+        }
+
+        /// <summary>
+        /// Get all entities colliding with a certain body.
+        /// </summary>
+        /// <param name="body"></param>
+        /// <returns></returns>
+        public IEnumerable<PhysicsComponent> GetCollidingEntities(MapId mapId, in Box2 worldAABB)
+        {
+            if (mapId == MapId.Nullspace) return Array.Empty<PhysicsComponent>();
+
+            var bodies = new HashSet<PhysicsComponent>();
+
+            foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, worldAABB, true))
+            {
+                Vector2 offset;
+
+                if (gridId == GridId.Invalid)
+                {
+                    offset = Vector2.Zero;
+                }
+                else
+                {
+                    offset = -_mapManager.GetGrid(gridId).WorldPosition;
+                }
+
+                var gridBox = worldAABB.Translated(offset);
+                foreach (var proxy in _graph[mapId][gridId].QueryAabb(gridBox, false))
+                {
+                    if (bodies.Contains(proxy.Fixture.Body)) continue;
+                    bodies.Add(proxy.Fixture.Body);
+                }
+            }
+
+            return bodies;
+        }
+
+        // TODO: This will get every body but we don't need to do that
+        /// <summary>
+        ///     Checks whether a body is colliding
+        /// </summary>
+        /// <param name="body"></param>
+        /// <param name="offset"></param>
+        /// <returns></returns>
+        public bool IsColliding(PhysicsComponent body, Vector2 offset, bool approximate)
+        {
+            return GetCollidingEntities(body, offset, approximate).Any();
+        }
+        #endregion
+
+        #region RayCast
+        /// <summary>
+        ///     Casts a ray in the world, returning the first entity it hits (or all entities it hits, if so specified)
+        /// </summary>
+        /// <param name="mapId"></param>
+        /// <param name="ray">Ray to cast in the world.</param>
+        /// <param name="maxLength">Maximum length of the ray in meters.</param>
+        /// <param name="predicate">A predicate to check whether to ignore an entity or not. If it returns true, it will be ignored.</param>
+        /// <param name="returnOnFirstHit">If true, will only include the first hit entity in results. Otherwise, returns all of them.</param>
+        /// <returns>A result object describing the hit, if any.</returns>
+        public IEnumerable<RayCastResults> IntersectRayWithPredicate(MapId mapId, CollisionRay ray,
+            float maxLength = 50F,
+            Func<IEntity, bool>? predicate = null, bool returnOnFirstHit = true)
+        {
+            List<RayCastResults> results = new();
+            var endPoint = ray.Position + ray.Direction.Normalized * maxLength;
+            var rayBox = new Box2(Vector2.ComponentMin(ray.Position, endPoint),
+                Vector2.ComponentMax(ray.Position, endPoint));
+
+            foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, rayBox, true))
+            {
+                Vector2 offset;
+
+                if (gridId == GridId.Invalid)
+                {
+                    offset = Vector2.Zero;
+                }
+                else
+                {
+                    offset = _mapManager.GetGrid(gridId).WorldPosition;
+                }
+
+                var broadPhase = GetBroadPhase(mapId, gridId);
+                var gridRay = new CollisionRay(ray.Position - offset, ray.Direction, ray.CollisionMask);
+                // TODO: Probably need rotation when we get rotatable grids
+
+                broadPhase?.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
+                {
+                    if (returnOnFirstHit && results.Count > 0) return true;
+
+                    if (distFromOrigin > maxLength)
+                    {
+                        return true;
+                    }
+
+                    if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
+                    {
+                        return true;
+                    }
+
+                    if (predicate?.Invoke(proxy.Fixture.Body.Entity) == true)
+                    {
+                        return true;
+                    }
+
+                    // TODO: Shape raycast here
+
+                    // Need to convert it back to world-space.
+                    var result = new RayCastResults(distFromOrigin, point + offset, proxy.Fixture.Body.Entity);
+                    results.Add(result);
+                    EntityManager.EventBus.QueueEvent(EventSource.Local,
+                        new DebugDrawRayMessage(
+                            new DebugRayData(ray, maxLength, result)));
+                    return true;
+                }, gridRay);
+            }
+
+            if (results.Count == 0)
+            {
+                EntityManager.EventBus.QueueEvent(EventSource.Local,
+                    new DebugDrawRayMessage(
+                        new DebugRayData(ray, maxLength, null)));
+            }
+
+            results.Sort((a, b) => a.Distance.CompareTo(b.Distance));
+            return results;
+        }
+
+        /// <summary>
+        ///     Casts a ray in the world and returns the first entity it hits, or a list of all entities it hits.
+        /// </summary>
+        /// <param name="mapId"></param>
+        /// <param name="ray">Ray to cast in the world.</param>
+        /// <param name="maxLength">Maximum length of the ray in meters.</param>
+        /// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
+        /// <param name="returnOnFirstHit">If false, will return a list of everything it hits, otherwise will just return a list of the first entity hit</param>
+        /// <returns>An enumerable of either the first entity hit or everything hit</returns>
+        public IEnumerable<RayCastResults> IntersectRay(MapId mapId, CollisionRay ray, float maxLength = 50, IEntity? ignoredEnt = null, bool returnOnFirstHit = true)
+            => IntersectRayWithPredicate(mapId, ray, maxLength, entity => entity == ignoredEnt, returnOnFirstHit);
+
+        /// <summary>
+        ///     Casts a ray in the world and returns the distance the ray traveled while colliding with entities
+        /// </summary>
+        /// <param name="mapId"></param>
+        /// <param name="ray">Ray to cast in the world.</param>
+        /// <param name="maxLength">Maximum length of the ray in meters.</param>
+        /// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
+        /// <returns>The distance the ray traveled while colliding with entities</returns>
+        public float IntersectRayPenetration(MapId mapId, CollisionRay ray, float maxLength, IEntity? ignoredEnt = null)
+        {
+            var penetration = 0f;
+            // TODO: Just make an actual box
+            var rayBox = new Box2(ray.Position - maxLength, ray.Position + maxLength);
+
+            foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, rayBox, true))
+            {
+                var offset = gridId == GridId.Invalid ? Vector2.Zero : _mapManager.GetGrid(gridId).WorldPosition;
+
+                var broadPhase = GetBroadPhase(mapId, gridId);
+                if (broadPhase == null) continue;
+
+                var gridRay = new CollisionRay(ray.Position - offset, ray.Direction, ray.CollisionMask);
+                // TODO: Probably need rotation when we get rotatable grids
+
+                broadPhase.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
+                {
+                    if (distFromOrigin > maxLength || proxy.Fixture.Body.Entity == ignoredEnt) return true;
+
+                    if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
+                    {
+                        return true;
+                    }
+
+                    if (new Ray(point + ray.Direction * proxy.AABB.Size.Length * 2, -ray.Direction).Intersects(
+                        proxy.AABB, out _, out var exitPoint))
+                    {
+                        penetration += (point - exitPoint).Length;
+                    }
+                    return true;
+                }, gridRay);
+            }
+
+            return penetration;
+        }
+        #endregion
+    }
+}

Robust.Shared/Physics/Collision/AlignedRectangle.cs

@@ -0,0 +1,115 @@
+using System;
+using Robust.Shared.Maths;
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// A rectangle that is always axis-aligned.
+    /// </summary>
+    [Serializable]
+    internal readonly struct AlignedRectangle : IEquatable<AlignedRectangle>
+    {
+        /// <summary>
+        /// Center point of the rectangle in world space.
+        /// </summary>
+        public readonly Vector2 Center;
+
+        /// <summary>
+        /// Half of the total width and height of the rectangle.
+        /// </summary>
+        public readonly Vector2 HalfExtents;
+
+        /// <summary>
+        ///     A 1x1 unit rectangle with the origin centered on the world origin.
+        /// </summary>
+        public static readonly AlignedRectangle UnitCentered = new(new Vector2(0.5f, 0.5f));
+
+        /// <summary>
+        ///     The lower X coordinate of the left edge of the box.
+        /// </summary>
+        public float Left => Center.X - HalfExtents.X;
+
+        /// <summary>
+        ///     The higher X coordinate of the right edge of the box.
+        /// </summary>
+        public float Right => Center.X + HalfExtents.X;
+
+        /// <summary>
+        ///     The lower Y coordinate of the top edge of the box.
+        /// </summary>
+        public float Bottom => Center.Y + HalfExtents.Y;
+
+        /// <summary>
+        ///     The higher Y coordinate of the bottom of the box.
+        /// </summary>
+        public float Top => Center.Y + HalfExtents.Y;
+
+        public AlignedRectangle(Box2 box)
+        {
+            var halfWidth = box.Width / 2;
+            var halfHeight = box.Height / 2;
+
+            HalfExtents = new Vector2(halfWidth, halfHeight);
+            Center = new Vector2(box.Left + halfWidth, box.Height + halfHeight);
+        }
+
+        public AlignedRectangle(Vector2 halfExtents)
+        {
+            Center = default;
+            HalfExtents = halfExtents;
+        }
+
+        public AlignedRectangle(Vector2 center, Vector2 halfExtents)
+        {
+            Center = center;
+            HalfExtents = halfExtents;
+        }
+
+        /// <summary>
+        /// Given a point, returns the closest point to it inside the box.
+        /// </summary>
+        public Vector2 ClosestPoint(in Vector2 position)
+        {
+            // clamp the point to the border of the box
+            var cx = MathHelper.Clamp(position.X, Left, Right);
+            var cy = MathHelper.Clamp(position.Y, Bottom, Top);
+
+            return new Vector2(cx, cy);
+        }
+
+        #region Equality members
+
+        public bool Equals(AlignedRectangle other)
+        {
+            return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents);
+        }
+
+        public override bool Equals(object? obj)
+        {
+            return obj is AlignedRectangle other && Equals(other);
+        }
+
+        public override int GetHashCode()
+        {
+            return HashCode.Combine(Center, HalfExtents);
+        }
+
+        public static bool operator ==(AlignedRectangle left, AlignedRectangle right) {
+            return left.Equals(right);
+        }
+
+        public static bool operator !=(AlignedRectangle left, AlignedRectangle right) {
+            return !left.Equals(right);
+        }
+
+        #endregion
+
+        /// <summary>
+        /// Returns the string representation of this object.
+        /// </summary>
+        public override string ToString()
+        {
+            return $"({Left}, {Bottom}, {Right}, {Top})";
+        }
+    }
+}

Robust.Shared/Physics/Collision/ClipVertex.cs

@@ -0,0 +1,35 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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 Robust.Shared.Maths;
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// Used for computing contact manifolds.
+    /// </summary>
+    internal struct ClipVertex
+    {
+        public ContactID ID;
+        public Vector2 V;
+    }
+}

Robust.Shared/Physics/Collision/DistanceInput.cs

@@ -0,0 +1,37 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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.
+*/
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// Input for Distance.ComputeDistance().
+    /// You have to option to use the shape radii in the computation.
+    /// </summary>
+    internal sealed class DistanceInput
+    {
+        public DistanceProxy ProxyA = new();
+        public DistanceProxy ProxyB = new();
+        public Transform TransformA;
+        public Transform TransformB;
+        public bool UseRadii;
+    }
+}

Robust.Shared/Physics/Collision/DistanceManager.cs

@@ -0,0 +1,187 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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;
+using Robust.Shared.Maths;
+
+namespace Robust.Shared.Physics.Collision
+{
+
+    internal static class DistanceManager
+    {
+        private const byte MaxGJKIterations = 20;
+
+        public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
+        {
+            cache = new SimplexCache();
+
+            /*
+            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                ++GJKCalls;
+            */
+
+            // Initialize the simplex.
+            Simplex simplex = new Simplex();
+            simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
+
+            // These store the vertices of the last simplex so that we
+            // can check for duplicates and prevent cycling.
+            var saveA = new int[3];
+            var saveB = new int[3];
+
+            //float distanceSqr1 = Settings.MaxFloat;
+
+            // Main iteration loop.
+            int iter = 0;
+            while (iter < MaxGJKIterations)
+            {
+                // Copy simplex so we can identify duplicates.
+                int saveCount = simplex.Count;
+                for (var i = 0; i < saveCount; ++i)
+                {
+                    saveA[i] = simplex.V[i].IndexA;
+                    saveB[i] = simplex.V[i].IndexB;
+                }
+
+                switch (simplex.Count)
+                {
+                    case 1:
+                        break;
+                    case 2:
+                        simplex.Solve2();
+                        break;
+                    case 3:
+                        simplex.Solve3();
+                        break;
+                    default:
+                        throw new ArgumentOutOfRangeException();
+                }
+
+                // If we have 3 points, then the origin is in the corresponding triangle.
+                if (simplex.Count == 3)
+                {
+                    break;
+                }
+
+                //FPE: This code was not used anyway.
+                // Compute closest point.
+                //Vector2 p = simplex.GetClosestPoint();
+                //float distanceSqr2 = p.LengthSquared();
+
+                // Ensure progress
+                //if (distanceSqr2 >= distanceSqr1)
+                //{
+                //break;
+                //}
+                //distanceSqr1 = distanceSqr2;
+
+                // Get search direction.
+                Vector2 d = simplex.GetSearchDirection();
+
+                // Ensure the search direction is numerically fit.
+                if (d.LengthSquared < float.Epsilon * float.Epsilon)
+                {
+                    // The origin is probably contained by a line segment
+                    // or triangle. Thus the shapes are overlapped.
+
+                    // We can't return zero here even though there may be overlap.
+                    // In case the simplex is a point, segment, or triangle it is difficult
+                    // to determine if the origin is contained in the CSO or very close to it.
+                    break;
+                }
+
+                // Compute a tentative new simplex vertex using support points.
+                SimplexVertex vertex = simplex.V[simplex.Count];
+                vertex.IndexA = input.ProxyA.GetSupport(Transform.MulT(input.TransformA.Quaternion2D, -d));
+                vertex.WA = Transform.Mul(input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
+
+                vertex.IndexB = input.ProxyB.GetSupport(Transform.MulT(input.TransformB.Quaternion2D, d));
+                vertex.WB = Transform.Mul(input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
+                vertex.W = vertex.WB - vertex.WA;
+                simplex.V[simplex.Count] = vertex;
+
+                // Iteration count is equated to the number of support point calls.
+                ++iter;
+
+                /*
+                if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                    ++GJKIters;
+                */
+
+                // Check for duplicate support points. This is the main termination criteria.
+                bool duplicate = false;
+                for (int i = 0; i < saveCount; ++i)
+                {
+                    if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
+                    {
+                        duplicate = true;
+                        break;
+                    }
+                }
+
+                // If we found a duplicate support point we must exit to avoid cycling.
+                if (duplicate)
+                {
+                    break;
+                }
+
+                // New vertex is ok and needed.
+                ++simplex.Count;
+            }
+
+            // Prepare output.
+            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
+            output.Distance = (output.PointA - output.PointB).Length;
+            output.Iterations = iter;
+
+            // Cache the simplex.
+            simplex.WriteCache(ref cache);
+
+            // Apply radii if requested.
+            if (input.UseRadii)
+            {
+                float rA = input.ProxyA.Radius;
+                float rB = input.ProxyB.Radius;
+
+                if (output.Distance > rA + rB && output.Distance > float.Epsilon)
+                {
+                    // Shapes are still no overlapped.
+                    // Move the witness points to the outer surface.
+                    output.Distance -= rA + rB;
+                    Vector2 normal = output.PointB - output.PointA;
+                    normal = normal.Normalized;
+                    output.PointA += normal * rA;
+                    output.PointB -= normal * rB;
+                }
+                else
+                {
+                    // Shapes are overlapped when radii are considered.
+                    // Move the witness points to the middle.
+                    Vector2 p = (output.PointA + output.PointB) * 0.5f;
+                    output.PointA = p;
+                    output.PointB = p;
+                    output.Distance = 0.0f;
+                }
+            }
+        }
+    }
+}

Robust.Shared/Physics/Collision/DistanceProxy.cs

@@ -0,0 +1,608 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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;
+using System.Collections.Generic;
+using System.Diagnostics;
+using Robust.Shared.Map;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Dynamics.Shapes;
+using Robust.Shared.Utility;
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// A distance proxy is used by the GJK algorithm.
+    /// It encapsulates any shape.
+    /// </summary>
+    internal sealed class DistanceProxy
+    {
+        internal float Radius;
+        internal List<Vector2> Vertices = new();
+
+        // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
+
+        /// <summary>
+        /// Initialize the proxy using the given shape. The shape
+        /// must remain in scope while the proxy is in use.
+        /// </summary>
+        /// <param name="shape">The shape.</param>
+        /// <param name="index">The index.</param>
+        public void Set(IPhysShape shape, int index)
+        {
+            Vertices.Clear();
+
+            switch (shape.ShapeType)
+            {
+                case ShapeType.Aabb:
+                    var aabb = (PhysShapeAabb) shape;
+                    var bounds = aabb.LocalBounds;
+
+                    Vertices.Add(bounds.BottomRight);
+                    Vertices.Add(bounds.TopRight);
+                    Vertices.Add(bounds.TopLeft);
+                    Vertices.Add(bounds.BottomLeft);
+
+                    Radius = aabb.Radius;
+
+                    break;
+                case ShapeType.Circle:
+                    PhysShapeCircle circle = (PhysShapeCircle) shape;
+                    // TODO: Circle's position offset to entity, someday.
+                    Vertices.Add(Vector2.Zero);
+                    Radius = circle.Radius;
+                    break;
+
+                case ShapeType.Polygon:
+                    // TODO: When manifold building gets fixed replace it back with a cast.
+                    var polygon = new PolygonShape(shape);
+
+                    foreach (var vert in polygon.Vertices)
+                    {
+                        Vertices.Add(vert);
+                    }
+
+                    Radius = polygon.Radius;
+                    break;
+
+                case ShapeType.Chain:
+                    throw new NotImplementedException();
+                    /*
+                    ChainShape chain = (ChainShape) shape;
+                    Debug.Assert(0 <= index && index < chain.Vertices.Count);
+                    Vertices.Clear();
+                    Vertices.Add(chain.Vertices[index]);
+                    Vertices.Add(index + 1 < chain.Vertices.Count ? chain.Vertices[index + 1] : chain.Vertices[0]);
+
+                    Radius = chain.Radius;
+                    */
+                    break;
+
+                case ShapeType.Edge:
+                    EdgeShape edge = (EdgeShape) shape;
+
+                    Vertices.Add(edge.Vertex1);
+                    Vertices.Add(edge.Vertex2);
+
+                    Radius = edge.Radius;
+                    break;
+                case ShapeType.Rectangle:
+                    var rectangle = (PhysShapeRect) shape;
+                    var calcedBounds = rectangle.CachedBounds;
+
+                    Vertices.Add(calcedBounds.BottomRight);
+                    Vertices.Add(calcedBounds.TopRight);
+                    Vertices.Add(calcedBounds.TopLeft);
+                    Vertices.Add(calcedBounds.BottomLeft);
+
+                    Radius = rectangle.Radius;
+                    break;
+                default:
+                    throw new InvalidOperationException($"Invalid shapetype specified {shape.ShapeType}");
+            }
+        }
+
+        /// <summary>
+        /// Get the supporting vertex index in the given direction.
+        /// </summary>
+        /// <param name="direction">The direction.</param>
+        /// <returns></returns>
+        public int GetSupport(Vector2 direction)
+        {
+            int bestIndex = 0;
+            float bestValue = Vector2.Dot(Vertices[0], direction);
+            for (int i = 1; i < Vertices.Count; ++i)
+            {
+                float value = Vector2.Dot(Vertices[i], direction);
+                if (value > bestValue)
+                {
+                    bestIndex = i;
+                    bestValue = value;
+                }
+            }
+
+            return bestIndex;
+        }
+
+        /// <summary>
+        /// Get the supporting vertex in the given direction.
+        /// </summary>
+        /// <param name="direction">The direction.</param>
+        /// <returns></returns>
+        public Vector2 GetSupportVertex(Vector2 direction)
+        {
+            int bestIndex = 0;
+            float bestValue = Vector2.Dot(Vertices[0], direction);
+            for (int i = 1; i < Vertices.Count; ++i)
+            {
+                float value = Vector2.Dot(Vertices[i], direction);
+                if (value > bestValue)
+                {
+                    bestIndex = i;
+                    bestValue = value;
+                }
+            }
+
+            return Vertices[bestIndex];
+        }
+    }
+
+    /// <summary>
+    /// Output for Distance.ComputeDistance().
+    /// </summary>
+    public struct DistanceOutput
+    {
+        public float Distance;
+
+        /// <summary>
+        /// Number of GJK iterations used
+        /// </summary>
+        public int Iterations;
+
+        /// <summary>
+        /// Closest point on shapeA
+        /// </summary>
+        public Vector2 PointA;
+
+        /// <summary>
+        /// Closest point on shapeB
+        /// </summary>
+        public Vector2 PointB;
+    }
+
+    internal struct SimplexVertex
+    {
+        /// <summary>
+        /// Barycentric coordinate for closest point
+        /// </summary>
+        public float A;
+
+        /// <summary>
+        /// wA index
+        /// </summary>
+        public int IndexA;
+
+        /// <summary>
+        /// wB index
+        /// </summary>
+        public int IndexB;
+
+        /// <summary>
+        /// wB - wA
+        /// </summary>
+        public Vector2 W;
+
+        /// <summary>
+        /// Support point in proxyA
+        /// </summary>
+        public Vector2 WA;
+
+        /// <summary>
+        /// Support point in proxyB
+        /// </summary>
+        public Vector2 WB;
+    }
+
+    internal class Simplex
+    {
+        // Made it a class from a struct as it seemed silly to be a struct considering it's being mutated constantly.
+
+        internal int Count;
+        internal readonly SimplexVertex[] V = new SimplexVertex[3];
+
+        internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB)
+        {
+            DebugTools.Assert(cache.Count <= 3);
+
+            // Copy data from cache.
+            Count = cache.Count;
+            for (int i = 0; i < Count; ++i)
+            {
+                SimplexVertex v = V[i];
+                unsafe
+                {
+                    v.IndexA = cache.IndexA[i];
+                    v.IndexB = cache.IndexB[i];
+                }
+
+                Vector2 wALocal = proxyA.Vertices[v.IndexA];
+                Vector2 wBLocal = proxyB.Vertices[v.IndexB];
+                v.WA = Transform.Mul(transformA, wALocal);
+                v.WB = Transform.Mul(transformB, wBLocal);
+                v.W = v.WB - v.WA;
+                v.A = 0.0f;
+                V[i] = v;
+            }
+
+            // Compute the new simplex metric, if it is substantially different than
+            // old metric then flush the simplex.
+            if (Count > 1)
+            {
+                float metric1 = cache.Metric;
+                float metric2 = GetMetric();
+                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon)
+                {
+                    // Reset the simplex.
+                    Count = 0;
+                }
+            }
+
+            // If the cache is empty or invalid ...
+            if (Count == 0)
+            {
+                SimplexVertex v = V[0];
+                v.IndexA = 0;
+                v.IndexB = 0;
+                Vector2 wALocal = proxyA.Vertices[0];
+                Vector2 wBLocal = proxyB.Vertices[0];
+                v.WA = Transform.Mul(transformA, wALocal);
+                v.WB = Transform.Mul(transformB, wBLocal);
+                v.W = v.WB - v.WA;
+                v.A = 1.0f;
+                V[0] = v;
+                Count = 1;
+            }
+        }
+
+        internal void WriteCache(ref SimplexCache cache)
+        {
+            cache.Metric = GetMetric();
+            cache.Count = (UInt16)Count;
+            for (var i = 0; i < Count; ++i)
+            {
+                unsafe
+                {
+                    cache.IndexA[i] = (byte) (V[i].IndexA);
+                    cache.IndexB[i] = (byte) (V[i].IndexB);
+                }
+            }
+        }
+
+        internal Vector2 GetSearchDirection()
+        {
+            switch (Count)
+            {
+                case 1:
+                    return -V[0].W;
+
+                case 2:
+                    {
+                        Vector2 e12 = V[1].W - V[0].W;
+                        float sgn = Vector2.Cross(e12, -V[0].W);
+                        if (sgn > 0.0f)
+                        {
+                            // Origin is left of e12.
+                            return new Vector2(-e12.Y, e12.X);
+                        }
+                        else
+                        {
+                            // Origin is right of e12.
+                            return new Vector2(e12.Y, -e12.X);
+                        }
+                    }
+
+                default:
+                    Debug.Assert(false);
+                    return Vector2.Zero;
+            }
+        }
+
+        internal Vector2 GetClosestPoint()
+        {
+            switch (Count)
+            {
+                case 0:
+                    Debug.Assert(false);
+                    return Vector2.Zero;
+
+                case 1:
+                    return V[0].W;
+
+                case 2:
+                    return V[0].W * V[0].A + V[1].W * V[1].A;
+
+                case 3:
+                    return Vector2.Zero;
+
+                default:
+                    Debug.Assert(false);
+                    return Vector2.Zero;
+            }
+        }
+
+        internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
+        {
+            switch (Count)
+            {
+                case 0:
+                    pA = Vector2.Zero;
+                    pB = Vector2.Zero;
+                    Debug.Assert(false);
+                    break;
+
+                case 1:
+                    pA = V[0].WA;
+                    pB = V[0].WB;
+                    break;
+
+                case 2:
+                    pA = V[0].WA * V[0].A + V[1].WA * V[1].A;
+                    pB = V[0].WB * V[0].A + V[1].WB * V[1].A;
+                    break;
+
+                case 3:
+                    pA = V[0].WA * V[0].A + V[1].WA * V[1].A + V[2].WA * V[2].A;
+                    pB = pA;
+                    break;
+
+                default:
+                    throw new Exception();
+            }
+        }
+
+        internal float GetMetric()
+        {
+            switch (Count)
+            {
+                case 0:
+                    Debug.Assert(false);
+                    return 0.0f;
+                case 1:
+                    return 0.0f;
+
+                case 2:
+                    return (V[0].W - V[1].W).Length;
+
+                case 3:
+                    return Vector2.Cross(V[1].W - V[0].W, V[2].W - V[0].W);
+
+                default:
+                    Debug.Assert(false);
+                    return 0.0f;
+            }
+        }
+
+        // Solve a line segment using barycentric coordinates.
+        //
+        // p = a1 * w1 + a2 * w2
+        // a1 + a2 = 1
+        //
+        // The vector from the origin to the closest point on the line is
+        // perpendicular to the line.
+        // e12 = w2 - w1
+        // dot(p, e) = 0
+        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
+        //
+        // 2-by-2 linear system
+        // [1      1     ][a1] = [1]
+        // [w1.e12 w2.e12][a2] = [0]
+        //
+        // Define
+        // d12_1 =  dot(w2, e12)
+        // d12_2 = -dot(w1, e12)
+        // d12 = d12_1 + d12_2
+        //
+        // Solution
+        // a1 = d12_1 / d12
+        // a2 = d12_2 / d12
+
+        internal void Solve2()
+        {
+            Vector2 w1 = V[0].W;
+            Vector2 w2 = V[1].W;
+            Vector2 e12 = w2 - w1;
+
+            // w1 region
+            float d12_2 = -Vector2.Dot(w1, e12);
+            if (d12_2 <= 0.0f)
+            {
+                // a2 <= 0, so we clamp it to 0
+                SimplexVertex v0 = V[0];
+                v0.A = 1.0f;
+                V[0] = v0;
+                Count = 1;
+                return;
+            }
+
+            // w2 region
+            float d12_1 = Vector2.Dot(w2, e12);
+            if (d12_1 <= 0.0f)
+            {
+                // a1 <= 0, so we clamp it to 0
+                SimplexVertex v1 = V[1];
+                v1.A = 1.0f;
+                V[1] = v1;
+                Count = 1;
+                V[0] = V[1];
+                return;
+            }
+
+            // Must be in e12 region.
+            float inv_d12 = 1.0f / (d12_1 + d12_2);
+            SimplexVertex v0_2 = V[0];
+            SimplexVertex v1_2 = V[1];
+            v0_2.A = d12_1 * inv_d12;
+            v1_2.A = d12_2 * inv_d12;
+            V[0] = v0_2;
+            V[1] = v1_2;
+            Count = 2;
+        }
+
+        // Possible regions:
+        // - points[2]
+        // - edge points[0]-points[2]
+        // - edge points[1]-points[2]
+        // - inside the triangle
+        internal void Solve3()
+        {
+            Vector2 w1 = V[0].W;
+            Vector2 w2 = V[1].W;
+            Vector2 w3 = V[2].W;
+
+            // Edge12
+            // [1      1     ][a1] = [1]
+            // [w1.e12 w2.e12][a2] = [0]
+            // a3 = 0
+            Vector2 e12 = w2 - w1;
+            float w1e12 = Vector2.Dot(w1, e12);
+            float w2e12 = Vector2.Dot(w2, e12);
+            float d12_1 = w2e12;
+            float d12_2 = -w1e12;
+
+            // Edge13
+            // [1      1     ][a1] = [1]
+            // [w1.e13 w3.e13][a3] = [0]
+            // a2 = 0
+            Vector2 e13 = w3 - w1;
+            float w1e13 = Vector2.Dot(w1, e13);
+            float w3e13 = Vector2.Dot(w3, e13);
+            float d13_1 = w3e13;
+            float d13_2 = -w1e13;
+
+            // Edge23
+            // [1      1     ][a2] = [1]
+            // [w2.e23 w3.e23][a3] = [0]
+            // a1 = 0
+            Vector2 e23 = w3 - w2;
+            float w2e23 = Vector2.Dot(w2, e23);
+            float w3e23 = Vector2.Dot(w3, e23);
+            float d23_1 = w3e23;
+            float d23_2 = -w2e23;
+
+            // Triangle123
+            float n123 = Vector2.Cross(e12, e13);
+
+            float d123_1 = n123 * Vector2.Cross(w2, w3);
+            float d123_2 = n123 * Vector2.Cross(w3, w1);
+            float d123_3 = n123 * Vector2.Cross(w1, w2);
+
+            // w1 region
+            if (d12_2 <= 0.0f && d13_2 <= 0.0f)
+            {
+                SimplexVertex v0_1 = V[0];
+                v0_1.A = 1.0f;
+                V[0] = v0_1;
+                Count = 1;
+                return;
+            }
+
+            // e12
+            if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
+            {
+                float inv_d12 = 1.0f / (d12_1 + d12_2);
+                SimplexVertex v0_2 = V[0];
+                SimplexVertex v1_2 = V[1];
+                v0_2.A = d12_1 * inv_d12;
+                v1_2.A = d12_2 * inv_d12;
+                V[0] = v0_2;
+                V[1] = v1_2;
+                Count = 2;
+                return;
+            }
+
+            // e13
+            if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
+            {
+                float inv_d13 = 1.0f / (d13_1 + d13_2);
+                SimplexVertex v0_3 = V[0];
+                SimplexVertex v2_3 = V[2];
+                v0_3.A = d13_1 * inv_d13;
+                v2_3.A = d13_2 * inv_d13;
+                V[0] = v0_3;
+                V[2] = v2_3;
+                Count = 2;
+                V[1] = V[2];
+                return;
+            }
+
+            // w2 region
+            if (d12_1 <= 0.0f && d23_2 <= 0.0f)
+            {
+                SimplexVertex v1_4 = V[1];
+                v1_4.A = 1.0f;
+                V[1] = v1_4;
+                Count = 1;
+                V[0] = V[1];
+                return;
+            }
+
+            // w3 region
+            if (d13_1 <= 0.0f && d23_1 <= 0.0f)
+            {
+                SimplexVertex v2_5 = V[2];
+                v2_5.A = 1.0f;
+                V[2] = v2_5;
+                Count = 1;
+                V[0] = V[2];
+                return;
+            }
+
+            // e23
+            if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
+            {
+                float inv_d23 = 1.0f / (d23_1 + d23_2);
+                SimplexVertex v1_6 = V[1];
+                SimplexVertex v2_6 = V[2];
+                v1_6.A = d23_1 * inv_d23;
+                v2_6.A = d23_2 * inv_d23;
+                V[1] = v1_6;
+                V[2] = v2_6;
+                Count = 2;
+                V[0] = V[2];
+                return;
+            }
+
+            // Must be in triangle123
+            float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
+            SimplexVertex v0_7 = V[0];
+            SimplexVertex v1_7 = V[1];
+            SimplexVertex v2_7 = V[2];
+            v0_7.A = d123_1 * inv_d123;
+            v1_7.A = d123_2 * inv_d123;
+            v2_7.A = d123_3 * inv_d123;
+            V[0] = v0_7;
+            V[1] = v1_7;
+            V[2] = v2_7;
+            Count = 3;
+        }
+    }
+}

Robust.Shared/Physics/Collision/Manifold.cs

@@ -0,0 +1,172 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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;
+using System.Runtime.InteropServices;
+using Robust.Shared.Maths;
+
+namespace Robust.Shared.Physics.Collision
+{
+    public enum ManifoldType : byte
+    {
+        Invalid = 0,
+        Circles,
+        FaceA,
+        FaceB,
+    }
+
+    internal enum ContactFeatureType : byte
+    {
+        Vertex = 0,
+        Face = 1,
+    }
+
+    /// <summary>
+    /// The features that intersect to form the contact point
+    /// This must be 4 bytes or less.
+    /// </summary>
+    public struct ContactFeature
+    {
+        /// <summary>
+        /// Feature index on ShapeA
+        /// </summary>
+        public byte IndexA;
+
+        /// <summary>
+        /// Feature index on ShapeB
+        /// </summary>
+        public byte IndexB;
+
+        /// <summary>
+        /// The feature type on ShapeA
+        /// </summary>
+        public byte TypeA;
+
+        /// <summary>
+        /// The feature type on ShapeB
+        /// </summary>
+        public byte TypeB;
+    }
+
+    /// <summary>
+    /// Contact ids to facilitate warm starting.
+    /// </summary>
+    [StructLayout(LayoutKind.Explicit)]
+    public struct ContactID
+    {
+        /// <summary>
+        /// The features that intersect to form the contact point
+        /// </summary>
+        [FieldOffset(0)]
+        public ContactFeature Features;
+
+        /// <summary>
+        /// Used to quickly compare contact ids.
+        /// </summary>
+        [FieldOffset(0)]
+        public uint Key;
+
+        public static bool operator ==(ContactID id, ContactID other)
+        {
+            return id.Key == other.Key;
+        }
+
+        public static bool operator !=(ContactID id, ContactID other)
+        {
+            return !(id == other);
+        }
+    }
+
+    // Made a class because A) It gets mutated bloody everywhere and B) unless you're careful you'll get solver issues (yay!)
+    // which I really could not be fucked dealing with
+    internal sealed class Manifold
+    {
+        public Vector2 LocalNormal;
+
+        /// <summary>
+        ///     Usage depends on manifold type.
+        /// </summary>
+        public Vector2 LocalPoint;
+
+        public int PointCount;
+
+        /// <summary>
+        ///     Points of contact, can only be 0 -> 2.
+        /// </summary>
+        public ManifoldPoint[] Points = new ManifoldPoint[2];
+
+        public ManifoldType Type;
+
+        public Manifold() {}
+
+        public Manifold(Vector2 localNormal, Vector2 localPoint, int pointCount, ManifoldPoint[] points, ManifoldType type)
+        {
+            LocalNormal = localNormal;
+            LocalPoint = localPoint;
+            PointCount = pointCount;
+            // Do not remove this copy or shit BREAKS
+            Array.Copy(points, Points, PointCount);
+            Type = type;
+        }
+
+        internal Manifold Clone()
+        {
+            return new(LocalNormal, LocalPoint, PointCount, Points, Type);
+        }
+    }
+
+    public struct ManifoldPoint
+    {
+        /// <summary>
+        ///     Unique identifier for the contact point between 2 shapes.
+        /// </summary>
+        public ContactID Id;
+
+        /// <summary>
+        ///     Usage depends on manifold type.
+        /// </summary>
+        public Vector2 LocalPoint;
+
+        /// <summary>
+        ///     The non-penetration impulse.
+        /// </summary>
+        public float NormalImpulse;
+
+        /// <summary>
+        ///     Friction impulse.
+        /// </summary>
+        public float TangentImpulse;
+
+        public static bool operator ==(ManifoldPoint point, ManifoldPoint other)
+        {
+            return point.Id == other.Id &&
+                   point.LocalPoint.Equals(other.LocalPoint) &&
+                   point.NormalImpulse.Equals(other.NormalImpulse) &&
+                   point.TangentImpulse.Equals(other.TangentImpulse);
+        }
+
+        public static bool operator !=(ManifoldPoint point, ManifoldPoint other)
+        {
+            return !(point == other);
+        }
+    }
+}

Robust.Shared/Physics/Collision/OrientedRectangle.cs

@@ -0,0 +1,213 @@
+using System;
+using Robust.Shared.Maths;
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// A rectangle that can be rotated.
+    /// </summary>
+    [Serializable]
+    internal readonly struct OrientedRectangle : IEquatable<OrientedRectangle>
+    {
+        /// <summary>
+        /// Center point of the rectangle in world space.
+        /// </summary>
+        public readonly Vector2 Center;
+
+        /// <summary>
+        /// Half of the total width and height of the rectangle.
+        /// </summary>
+        public readonly Vector2 HalfExtents;
+
+        /// <summary>
+        /// World rotation of the rectangle in radians.
+        /// </summary>
+        public readonly float Rotation;
+
+        /// <summary>
+        ///     A 1x1 unit box with the origin centered and identity rotation.
+        /// </summary>
+        public static readonly OrientedRectangle UnitCentered = new(Vector2.Zero, Vector2.One, 0);
+
+        public OrientedRectangle(Box2 worldBox)
+        {
+            Center = worldBox.Center;
+
+            var hWidth = MathF.Abs(worldBox.Right - worldBox.Left) * 0.5f;
+            var hHeight = MathF.Abs(worldBox.Bottom - worldBox.Top) * 0.5f;
+
+            HalfExtents = new Vector2(hWidth, hHeight);
+            Rotation = 0;
+        }
+
+        public OrientedRectangle(Vector2 halfExtents)
+        {
+            Center = default;
+            HalfExtents = halfExtents;
+            Rotation = default;
+        }
+
+        public OrientedRectangle(Vector2 center, Vector2 halfExtents)
+        {
+            Center = center;
+            HalfExtents = halfExtents;
+            Rotation = default;
+        }
+
+        public OrientedRectangle(Vector2 center, Vector2 halfExtents, float rotation)
+        {
+            Center = center;
+            HalfExtents = halfExtents;
+            Rotation = rotation;
+        }
+
+        public OrientedRectangle(in Vector2 center, in Box2 localBox, float rotation)
+        {
+            Center = center;
+            HalfExtents = new Vector2(localBox.Width / 2, localBox.Height / 2);
+            Rotation = rotation;
+        }
+
+        /// <summary>
+        /// calculates the smallest AABB that will encompass this rectangle. The AABB is in local space.
+        /// </summary>
+        public Box2 CalcBoundingBox()
+        {
+            var Fi = Rotation;
+
+            var CX = Center.X;
+            var CY = Center.Y;
+
+            var WX = HalfExtents.X;
+            var WY = HalfExtents.Y;
+
+            var SF = MathF.Sin(Fi);
+            var CF = MathF.Cos(Fi);
+
+            var NH = MathF.Abs(WX * SF) + MathF.Abs(WY * CF);  //boundrect half-height
+            var NW = MathF.Abs(WX * CF) + MathF.Abs(WY * SF);  //boundrect half-width
+
+            return new Box2((float)(CX - NW), (float)(CY - NH), (float)(CX + NW), (float)(CY + NH)); //draw bound rectangle
+        }
+
+        /// <summary>
+        /// Tests if a point is contained inside this rectangle.
+        /// </summary>
+        /// <param name="point">Point to test.</param>
+        /// <returns>True if the point is contained inside this rectangle.</returns>
+        public bool Contains(Vector2 point)
+        {
+            // rotate around rectangle center by -rectAngle
+            var s = MathF.Sin(-Rotation);
+            var c = MathF.Cos(-Rotation);
+
+            // set origin to rect center
+            var newPoint = point - Center;
+
+            // rotate
+            newPoint = new Vector2(newPoint.X * c - newPoint.Y * s, newPoint.X * s + newPoint.Y * c);
+
+            // put origin back
+            newPoint += Center;
+
+            // check if our transformed point is in the rectangle, which is no longer
+            // rotated relative to the point
+
+            var xMin = -HalfExtents.X;
+            var xMax =  HalfExtents.X;
+            var yMin = -HalfExtents.Y;
+            var yMax =  HalfExtents.Y;
+
+            return newPoint.X >= xMin && newPoint.X <= xMax && newPoint.Y >= yMin && newPoint.Y <= yMax;
+        }
+
+        /// <summary>
+        /// Returns the closest point inside the rectangle to the given point in world space.
+        /// </summary>
+        public Vector2 ClosestPointWorld(Vector2 worldPoint)
+        {
+            // inverse-transform the sphere's center into the box's local space.
+            var localPoint = InverseTransformPoint(worldPoint);
+
+            var xMin = -HalfExtents.X;
+            var xMax = HalfExtents.X;
+            var yMin = -HalfExtents.Y;
+            var yMax = HalfExtents.Y;
+
+            // clamp the point to the border of the box
+            var cx = MathHelper.Clamp(localPoint.X, xMin, xMax);
+            var cy = MathHelper.Clamp(localPoint.Y, yMin, yMax);
+
+            return TransformPoint(new Vector2(cx, cy));
+        }
+
+        /// <summary>
+        /// Transforms a point from the rectangle's local space to world space.
+        /// </summary>
+        public Vector2 TransformPoint(Vector2 localPoint)
+        {
+            var theta = Rotation;
+            var (x, y) = localPoint;
+            var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
+            var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
+            return new Vector2(dx, dy) + Center;
+        }
+
+        /// <summary>
+        /// Transforms a point from world space to the rectangle's local space.
+        /// </summary>
+        public Vector2 InverseTransformPoint(Vector2 worldPoint)
+        {
+            var theta = -Rotation;
+            var (x, y) = worldPoint + -Center;
+            var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
+            var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
+            return new Vector2(dx, dy);
+        }
+
+        #region Equality Members
+
+        /// <inheritdoc />
+        public bool Equals(OrientedRectangle other)
+        {
+            return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents) && Rotation.Equals(other.Rotation);
+        }
+
+        /// <inheritdoc />
+        public override bool Equals(object? obj)
+        {
+            return obj is OrientedRectangle other && Equals(other);
+        }
+
+        /// <inheritdoc />
+        public override int GetHashCode()
+        {
+            return HashCode.Combine(Center, HalfExtents, Rotation);
+        }
+
+        /// <summary>
+        ///     Check for equality by value between two <see cref="OrientedRectangle"/>.
+        /// </summary>
+        public static bool operator ==(OrientedRectangle left, OrientedRectangle right) {
+            return left.Equals(right);
+        }
+
+        /// <summary>
+        ///     Check for inequality by value between two <see cref="OrientedRectangle"/>.
+        /// </summary>
+        public static bool operator !=(OrientedRectangle left, OrientedRectangle right) {
+            return !left.Equals(right);
+        }
+
+        #endregion
+
+        /// <summary>
+        /// Returns the string representation of this object.
+        /// </summary>
+        public override string ToString()
+        {
+            var box = new Box2(-HalfExtents.X, -HalfExtents.Y, HalfExtents.X, HalfExtents.Y).Translated(Center);
+            return $"{box}, {Rotation}";
+        }
+    }
+}

Robust.Shared/Physics/Collision/PointState.cs

@@ -0,0 +1,50 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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.
+*/
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// This is used for determining the state of contact points.
+    /// </summary>
+    public enum PointState : byte
+    {
+        /// <summary>
+        /// Point does not exist
+        /// </summary>
+        Null,
+
+        /// <summary>
+        /// Point was added in the update
+        /// </summary>
+        Add,
+
+        /// <summary>
+        /// Point persisted across the update
+        /// </summary>
+        Persist,
+
+        /// <summary>
+        /// Point was removed in the update
+        /// </summary>
+        Remove,
+    }
+}

Robust.Shared/Physics/Collision/SimplexCache.cs

@@ -0,0 +1,48 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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.
+*/
+
+namespace Robust.Shared.Physics.Collision
+{
+    /// <summary>
+    /// Used to warm start ComputeDistance.
+    /// Set count to zero on first call.
+    /// </summary>
+    internal struct SimplexCache
+    {
+        /// <summary>
+        /// Length or area
+        /// </summary>
+        public ushort Count;
+
+        /// <summary>
+        /// Vertices on shape A
+        /// </summary>
+        public unsafe fixed byte IndexA[3];
+
+        /// <summary>
+        /// Vertices on shape B
+        /// </summary>
+        public unsafe fixed byte IndexB[3];
+
+        public float Metric;
+    }
+}

Robust.Shared/Physics/CollisionSolver.cs

@@ -1,688 +0,0 @@
-using System;
-using Robust.Shared.Map;
-using Robust.Shared.Maths;
-
-namespace Robust.Shared.Physics
-{
-    internal static class CollisionSolver
-    {
-        public static void CalculateFeatures(Manifold manifold, IPhysShape a, IPhysShape b, out CollisionFeatures features)
-        {
-            // 2D table of all possible PhysShape combinations
-            switch (a)
-            {
-                case PhysShapeCircle aCircle:
-                    switch (b)
-                    {
-                        case PhysShapeCircle bCircle:
-                            CircleCircle(manifold, aCircle, bCircle, 1, out features);
-                            return;
-                        case PhysShapeAabb bAabb:
-                            CircleBox(manifold, aCircle, bAabb, 1, out features);
-                            return;
-                        case PhysShapeRect bRect:
-                            RectCircle(manifold, bRect, aCircle, -1, out features);
-                            return;
-                        case PhysShapeGrid bGrid:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                    }
-                    break;
-                case PhysShapeAabb aAabb:
-                    switch (b)
-                    {
-                        case PhysShapeCircle bCircle:
-                            CircleBox(manifold, bCircle, aAabb, -1, out features);
-                            return;
-                        case PhysShapeAabb bAabb:
-                            BoxBox(manifold, aAabb, bAabb, 1, out features);
-                            return;
-                        case PhysShapeRect bRect:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeGrid bGrid:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                    }
-                    break;
-                case PhysShapeRect aRect:
-                    switch (b)
-                    {
-                        case PhysShapeCircle bCircle:
-                            RectCircle(manifold, aRect, bCircle, 1, out features);
-                            return;
-                        case PhysShapeAabb bAabb:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeRect bRect:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeGrid bGrid:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                    }
-                    break;
-                case PhysShapeGrid aGrid:
-                    switch (b)
-                    {
-                        case PhysShapeCircle bCircle:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeAabb bAabb:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeRect bRect:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                        case PhysShapeGrid bGrid:
-                            DummyBoundsFeatures(manifold, out features);
-                            return;
-                    }
-                    break;
-            }
-            features = default;
-        }
-
-        private static void DummyBoundsFeatures(Manifold manifold, out CollisionFeatures features)
-        {
-            var aRect = new AlignedRectangle(manifold.A.Entity.Transform.WorldPosition, manifold.A.AABB.Size / 2);
-            var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, manifold.B.AABB.Size / 2);
-            CalculateCollisionFeatures(aRect, bRect, 1, out features);
-        }
-
-        private static void CircleCircle(Manifold manifold, PhysShapeCircle a, PhysShapeCircle b, float flip,
-            out CollisionFeatures features)
-        {
-            var aRad = a.Radius;
-            var bRad = b.Radius;
-
-            var aPos = manifold.A.Entity.Transform.WorldPosition;
-            var bPos = manifold.B.Entity.Transform.WorldPosition;
-
-            CalculateCollisionFeatures(new Circle(aPos, aRad), new Circle(bPos, bRad), (float) flip, out features);
-        }
-
-        private static void CircleBox(Manifold manifold, PhysShapeCircle a, PhysShapeAabb b, float flip,
-            out CollisionFeatures features)
-        {
-            var aRad = a.Radius;
-            var aPos = manifold.A.Entity.Transform.WorldPosition;
-
-            var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, b.LocalBounds.Size / 2);
-
-            CalculateCollisionFeatures(bRect, new Circle(aPos, aRad), (float) flip * -1, out features);
-        }
-
-        private static void RectCircle(Manifold manifold, PhysShapeRect a, PhysShapeCircle b, float flip,
-            out CollisionFeatures features)
-        {
-            var aPos = manifold.A.Entity.Transform.WorldPosition;
-            var bPos = manifold.B.Entity.Transform.WorldPosition;
-
-            var aRot = (float)manifold.A.Entity.Transform.WorldRotation.Theta;
-
-            CalculateCollisionFeatures(new OrientedRectangle(aPos, a.Rectangle, aRot), new Circle(bPos, b.Radius), (float) flip, out features);
-        }
-
-        private static void BoxBox(Manifold manifold, PhysShapeAabb a, PhysShapeAabb b, float flip,
-            out CollisionFeatures features)
-        {
-            var aRect = new AlignedRectangle(manifold.A.Entity.Transform.WorldPosition, a.LocalBounds.Size / 2);
-            var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, b.LocalBounds.Size / 2);
-
-            CalculateCollisionFeatures(in aRect, in bRect, flip, out features);
-        }
-
-        public static void CalculateCollisionFeatures(in Circle A, in Circle B, float flip, out CollisionFeatures features)
-        {
-            var aRad = A.Radius;
-            var bRad = B.Radius;
-
-            var aPos = A.Position;
-            var bPos = B.Position;
-
-            // combined radius
-            var radiiSum = aRad + bRad;
-
-            // distance between circles
-            var dist = bPos - aPos;
-
-            // if the distance between two circles is larger than their combined radii,
-            // they are not colliding, otherwise they are
-            if (dist.LengthSquared > radiiSum * radiiSum)
-            {
-                features = default;
-                return;
-            }
-
-            // if dist between circles is zero, the circles are concentric, this collision cannot be resolved
-            if (dist.LengthSquared.Equals(0f))
-            {
-                features = default;
-                return;
-            }
-
-            // generate collision normal
-            var normal = dist.Normalized;
-
-            // half of the total
-            var penetraction = (radiiSum - dist.Length) * 0.5f;
-
-            var contacts = new Vector2[1];
-
-            // dtp - Distance to intersection point
-            var dtp = aRad - penetraction;
-            var contact = aPos + normal * dtp;
-            contacts[0] = contact;
-
-            features = new CollisionFeatures(true, normal, penetraction, contacts);
-        }
-
-        public static void CalculateCollisionFeatures(in AlignedRectangle A, in Circle B, float flip, out CollisionFeatures features)
-        {
-            // closest point inside the rectangle to the center of the sphere.
-            var closestPoint = A.ClosestPoint(in B.Position);
-
-            // If the point is outside the sphere, the sphere and OBB do not intersect.
-            var distanceSq = (closestPoint - B.Position).LengthSquared;
-            if (distanceSq > B.Radius * B.Radius)
-            {
-                features = default;
-                return;
-            }
-
-            Vector2 normal;
-            if (distanceSq.Equals(0.0f))
-            {
-                var mSq = (closestPoint - A.Center).LengthSquared;
-                if (mSq.Equals(0.0f))
-                {
-                    features = default;
-                    return;
-                }
-
-                // Closest point is at the center of the sphere
-                normal = (closestPoint - A.Center).Normalized;
-            }
-            else
-                normal = (B.Position - closestPoint).Normalized;
-
-            var outsidePoint = B.Position - normal * B.Radius;
-            var distance = (closestPoint - outsidePoint).Length;
-            var contacts = new Vector2[1];
-            contacts[0] = closestPoint + (outsidePoint - closestPoint) * 0.5f;
-            var depth = distance * 0.5f;
-
-            features = new CollisionFeatures(true, normal, depth, contacts);
-        }
-
-        public static void CalculateCollisionFeatures(in OrientedRectangle A, in Circle B, float flip, out CollisionFeatures features)
-        {
-            // closest point inside the rectangle to the center of the sphere.
-            var closestPoint = A.ClosestPointWorld(B.Position);
-
-            // If the point is outside the sphere, the sphere and OBB do not intersect.
-            var distanceSq = (closestPoint - B.Position).LengthSquared;
-            if (distanceSq > B.Radius * B.Radius)
-            {
-                features = default;
-                return;
-            }
-
-            Vector2 normal;
-            if (distanceSq.Equals(0.0f))
-            {
-                var mSq = (closestPoint - A.Center).LengthSquared;
-                if (mSq.Equals(0.0f))
-                {
-                    features = default;
-                    return;
-                }
-
-                // Closest point is at the center of the sphere
-                normal = (closestPoint - A.Center).Normalized;
-            }
-            else
-                normal = (B.Position - closestPoint).Normalized;
-
-            var outsidePoint = B.Position - normal * B.Radius;
-            var distance = (closestPoint - outsidePoint).Length;
-            var contacts = new Vector2[1];
-            contacts[0] = closestPoint + (outsidePoint - closestPoint) * 0.5f;
-            var depth = distance * 0.5f;
-
-            features = new CollisionFeatures(true, normal, depth, contacts);
-        }
-
-        public static void CalculateCollisionFeatures(in AlignedRectangle A, in AlignedRectangle B, float flip, out CollisionFeatures features)
-        {
-            // Vector from A to B
-            Vector2 n = B.Center - A.Center;
-
-            // Calculate half extents along x axis for each object
-            float a_extent_x = A.HalfExtents.X;
-            float b_extent_x = B.HalfExtents.X;
-
-            // Calculate overlap on x axis
-            float x_overlap = a_extent_x + b_extent_x - MathF.Abs(n.X);
-
-            // SAT test on x axis
-            if (!(x_overlap > 0))
-            {
-                features = default;
-                return;
-            }
-
-            // Calculate half extents along y axis for each object
-            float a_extent_y = A.HalfExtents.Y;
-            float b_extent_y = B.HalfExtents.Y;
-
-            // Calculate overlap on y axis
-            float y_overlap = a_extent_y + b_extent_y - MathF.Abs(n.Y);
-
-            // SAT test on y axis
-            if (!(y_overlap > 0))
-            {
-                features = default;
-                return;
-            }
-
-            Vector2 normal;
-            float penetration;
-            Vector2 contact;
-
-            // Find out which axis is axis of least penetration
-            if (x_overlap < y_overlap)
-            {
-                // Point towards B knowing that n points from A to B
-                if (n.X < 0)
-                    normal = new Vector2(-1, 0);
-                else
-                    normal = new Vector2(1, 0);
-                penetration = x_overlap / 2;
-                var hitx = A.Center.X + (a_extent_x * normal.X);
-                var hity = B.Center.Y;
-                contact = new Vector2(hitx, hity);
-            }
-            else
-            {
-                // Point toward B knowing that n points from A to B
-                if (n.Y < 0)
-                    normal = new Vector2(0, -1);
-                else
-                    normal = new Vector2(0, 1);
-                penetration = y_overlap / 2;
-                var hitx = B.Center.X;
-                var hity = A.Center.Y + (a_extent_y * normal.Y);
-                contact = new Vector2(hitx, hity);
-            }
-
-            features = new CollisionFeatures(true, normal, penetration, new[] {contact});
-        }
-    }
-
-    /// <summary>
-    /// Features of the collision.
-    /// </summary>
-    internal readonly struct CollisionFeatures
-    {
-        /// <summary>
-        /// Are the two shapes *actually* colliding? If this is false, the rest of the
-        /// values in this struct are default.
-        /// </summary>
-        public readonly bool Collided;
-
-        /// <summary>
-        /// Collision normal. If A moves in the negative direction of the normal and
-        /// B moves in the positive direction, the objects will no longer intersect.
-        /// </summary>
-        public readonly Vector2 Normal;
-
-        /// <summary>
-        /// Half of the total depth of penetration. Each object needs to move
-        /// by the penetration distance along the normal to resolve the collision.
-        /// </summary>
-        /// <remarks>
-        /// The penetration depth is the length of the minimum translation vector (MTV), which
-        /// is the smallest vector along which we can translate an intersecting shape to
-        /// separate it from the other shape.
-        /// </remarks>
-        public readonly float Penetration;
-
-        /// <summary>
-        /// all the points at which the two objects collide, projected onto a plane.The plane
-        /// these points are projected onto has the normal of the collision normal and is
-        /// located halfway between the colliding objects.
-        /// </summary>
-        public readonly Vector2[] Contacts;
-
-        /// <summary>
-        /// Constructs a new instance of <see cref="CollisionFeatures"/>.
-        /// </summary>
-        public CollisionFeatures(bool collided, Vector2 normal, float penetration, Vector2[] contacts)
-        {
-            Collided = collided;
-            Normal = normal;
-            Penetration = penetration;
-            Contacts = contacts;
-        }
-    }
-
-    /// <summary>
-    /// A rectangle that is always axis-aligned.
-    /// </summary>
-    [Serializable]
-    internal readonly struct AlignedRectangle : IEquatable<AlignedRectangle>
-    {
-        /// <summary>
-        /// Center point of the rectangle in world space.
-        /// </summary>
-        public readonly Vector2 Center;
-
-        /// <summary>
-        /// Half of the total width and height of the rectangle.
-        /// </summary>
-        public readonly Vector2 HalfExtents;
-
-        /// <summary>
-        ///     A 1x1 unit rectangle with the origin centered on the world origin.
-        /// </summary>
-        public static readonly AlignedRectangle UnitCentered = new(new Vector2(0.5f, 0.5f));
-
-        /// <summary>
-        ///     The lower X coordinate of the left edge of the box.
-        /// </summary>
-        public float Left => Center.X - HalfExtents.X;
-
-        /// <summary>
-        ///     The higher X coordinate of the right edge of the box.
-        /// </summary>
-        public float Right => Center.X + HalfExtents.X;
-
-        /// <summary>
-        ///     The lower Y coordinate of the top edge of the box.
-        /// </summary>
-        public float Bottom => Center.Y + HalfExtents.Y;
-
-        /// <summary>
-        ///     The higher Y coordinate of the bottom of the box.
-        /// </summary>
-        public float Top => Center.Y + HalfExtents.Y;
-
-        public AlignedRectangle(Box2 box)
-        {
-            var halfWidth = box.Width / 2;
-            var halfHeight = box.Height / 2;
-
-            HalfExtents = new Vector2(halfWidth, halfHeight);
-            Center = new Vector2(box.Left + halfWidth, box.Height + halfHeight);
-        }
-
-        public AlignedRectangle(Vector2 halfExtents)
-        {
-            Center = default;
-            HalfExtents = halfExtents;
-        }
-
-        public AlignedRectangle(Vector2 center, Vector2 halfExtents)
-        {
-            Center = center;
-            HalfExtents = halfExtents;
-        }
-
-        /// <summary>
-        /// Given a point, returns the closest point to it inside the box.
-        /// </summary>
-        public Vector2 ClosestPoint(in Vector2 position)
-        {
-            // clamp the point to the border of the box
-            var cx = MathHelper.Clamp(position.X, Left, Right);
-            var cy = MathHelper.Clamp(position.Y, Bottom, Top);
-
-            return new Vector2(cx, cy);
-        }
-
-        #region Equality members
-
-        public bool Equals(AlignedRectangle other)
-        {
-            return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents);
-        }
-
-        public override bool Equals(object? obj)
-        {
-            return obj is AlignedRectangle other && Equals(other);
-        }
-
-        public override int GetHashCode()
-        {
-            return HashCode.Combine(Center, HalfExtents);
-        }
-
-        public static bool operator ==(AlignedRectangle left, AlignedRectangle right) {
-            return left.Equals(right);
-        }
-
-        public static bool operator !=(AlignedRectangle left, AlignedRectangle right) {
-            return !left.Equals(right);
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Returns the string representation of this object.
-        /// </summary>
-        public override string ToString()
-        {
-            return $"({Left}, {Bottom}, {Right}, {Top})";
-        }
-    }
-
-    /// <summary>
-    /// A rectangle that can be rotated.
-    /// </summary>
-    [Serializable]
-    internal readonly struct OrientedRectangle : IEquatable<OrientedRectangle>
-    {
-        /// <summary>
-        /// Center point of the rectangle in world space.
-        /// </summary>
-        public readonly Vector2 Center;
-
-        /// <summary>
-        /// Half of the total width and height of the rectangle.
-        /// </summary>
-        public readonly Vector2 HalfExtents;
-
-        /// <summary>
-        /// World rotation of the rectangle in radians.
-        /// </summary>
-        public readonly float Rotation;
-
-        /// <summary>
-        ///     A 1x1 unit box with the origin centered and identity rotation.
-        /// </summary>
-        public static readonly OrientedRectangle UnitCentered = new(Vector2.Zero, Vector2.One, 0);
-
-        public OrientedRectangle(Box2 worldBox)
-        {
-            Center = worldBox.Center;
-
-            var hWidth = MathF.Abs(worldBox.Right - worldBox.Left) * 0.5f;
-            var hHeight = MathF.Abs(worldBox.Bottom - worldBox.Top) * 0.5f;
-
-            HalfExtents = new Vector2(hWidth, hHeight);
-            Rotation = 0;
-        }
-
-        public OrientedRectangle(Vector2 halfExtents)
-        {
-            Center = default;
-            HalfExtents = halfExtents;
-            Rotation = default;
-        }
-
-        public OrientedRectangle(Vector2 center, Vector2 halfExtents)
-        {
-            Center = center;
-            HalfExtents = halfExtents;
-            Rotation = default;
-        }
-
-        public OrientedRectangle(Vector2 center, Vector2 halfExtents, float rotation)
-        {
-            Center = center;
-            HalfExtents = halfExtents;
-            Rotation = rotation;
-        }
-
-        public OrientedRectangle(in Vector2 center, in Box2 localBox, float rotation)
-        {
-            Center = center;
-            HalfExtents = new Vector2(localBox.Width / 2, localBox.Height / 2);
-            Rotation = rotation;
-        }
-
-        /// <summary>
-        /// calculates the smallest AABB that will encompass this rectangle. The AABB is in local space.
-        /// </summary>
-        public Box2 CalcBoundingBox()
-        {
-            var Fi = Rotation;
-
-            var CX = Center.X;
-            var CY = Center.Y;
-
-            var WX = HalfExtents.X;
-            var WY = HalfExtents.Y;
-
-            var SF = MathF.Sin(Fi);
-            var CF = MathF.Cos(Fi);
-
-            var NH = MathF.Abs(WX * SF) + MathF.Abs(WY * CF);  //boundrect half-height
-            var NW = MathF.Abs(WX * CF) + MathF.Abs(WY * SF);  //boundrect half-width
-
-            return new Box2((float)(CX - NW), (float)(CY - NH), (float)(CX + NW), (float)(CY + NH)); //draw bound rectangle
-        }
-
-        /// <summary>
-        /// Tests if a point is contained inside this rectangle.
-        /// </summary>
-        /// <param name="point">Point to test.</param>
-        /// <returns>True if the point is contained inside this rectangle.</returns>
-        public bool Contains(Vector2 point)
-        {
-            // rotate around rectangle center by -rectAngle
-            var s = MathF.Sin(-Rotation);
-            var c = MathF.Cos(-Rotation);
-
-            // set origin to rect center
-            var newPoint = point - Center;
-
-            // rotate
-            newPoint = new Vector2(newPoint.X * c - newPoint.Y * s, newPoint.X * s + newPoint.Y * c);
-
-            // put origin back
-            newPoint += Center;
-
-            // check if our transformed point is in the rectangle, which is no longer
-            // rotated relative to the point
-
-            var xMin = -HalfExtents.X;
-            var xMax =  HalfExtents.X;
-            var yMin = -HalfExtents.Y;
-            var yMax =  HalfExtents.Y;
-
-            return newPoint.X >= xMin && newPoint.X <= xMax && newPoint.Y >= yMin && newPoint.Y <= yMax;
-        }
-
-        /// <summary>
-        /// Returns the closest point inside the rectangle to the given point in world space.
-        /// </summary>
-        public Vector2 ClosestPointWorld(Vector2 worldPoint)
-        {
-            // inverse-transform the sphere's center into the box's local space.
-            var localPoint = InverseTransformPoint(worldPoint);
-
-            var xMin = -HalfExtents.X;
-            var xMax = HalfExtents.X;
-            var yMin = -HalfExtents.Y;
-            var yMax = HalfExtents.Y;
-
-            // clamp the point to the border of the box
-            var cx = MathHelper.Clamp(localPoint.X, xMin, xMax);
-            var cy = MathHelper.Clamp(localPoint.Y, yMin, yMax);
-
-            return TransformPoint(new Vector2(cx, cy));
-        }
-
-        /// <summary>
-        /// Transforms a point from the rectangle's local space to world space.
-        /// </summary>
-        public Vector2 TransformPoint(Vector2 localPoint)
-        {
-            var theta = Rotation;
-            var (x, y) = localPoint;
-            var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
-            var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
-            return new Vector2(dx, dy) + Center;
-        }
-
-        /// <summary>
-        /// Transforms a point from world space to the rectangle's local space.
-        /// </summary>
-        public Vector2 InverseTransformPoint(Vector2 worldPoint)
-        {
-            var theta = -Rotation;
-            var (x, y) = worldPoint + -Center;
-            var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
-            var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
-            return new Vector2(dx, dy);
-        }
-
-        #region Equality Members
-
-        /// <inheritdoc />
-        public bool Equals(OrientedRectangle other)
-        {
-            return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents) && Rotation.Equals(other.Rotation);
-        }
-
-        /// <inheritdoc />
-        public override bool Equals(object? obj)
-        {
-            return obj is OrientedRectangle other && Equals(other);
-        }
-
-        /// <inheritdoc />
-        public override int GetHashCode()
-        {
-            return HashCode.Combine(Center, HalfExtents, Rotation);
-        }
-
-        /// <summary>
-        ///     Check for equality by value between two <see cref="OrientedRectangle"/>.
-        /// </summary>
-        public static bool operator ==(OrientedRectangle left, OrientedRectangle right) {
-            return left.Equals(right);
-        }
-
-        /// <summary>
-        ///     Check for inequality by value between two <see cref="OrientedRectangle"/>.
-        /// </summary>
-        public static bool operator !=(OrientedRectangle left, OrientedRectangle right) {
-            return !left.Equals(right);
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Returns the string representation of this object.
-        /// </summary>
-        public override string ToString()
-        {
-            var box = new Box2(-HalfExtents.X, -HalfExtents.Y, HalfExtents.X, HalfExtents.Y).Translated(Center);
-            return $"{box}, {Rotation}";
-        }
-    }
-}

Robust.Shared/Physics/Controllers/VirtualController.cs

@@ -0,0 +1,55 @@
+using System;
+using System.Collections.Generic;
+using JetBrains.Annotations;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Physics.Dynamics;
+
+namespace Robust.Shared.Physics.Controllers
+{
+    [MeansImplicitUse]
+    public abstract class VirtualController
+    {
+        [Dependency] protected readonly IComponentManager ComponentManager = default!;
+        [Dependency] protected readonly IEntityManager EntityManager = default!;
+
+        public virtual List<Type> UpdatesBefore => new();
+
+        public virtual List<Type> UpdatesAfter => new();
+
+        public virtual void Initialize()
+        {
+            IoCManager.InjectDependencies(this);
+        }
+
+        /// <summary>
+        ///     Run before any map processing starts.
+        /// </summary>
+        /// <param name="prediction"></param>
+        /// <param name="frameTime"></param>
+        public virtual void UpdateBeforeSolve(bool prediction, float frameTime) {}
+
+        /// <summary>
+        ///     Run after all map processing has finished.
+        /// </summary>
+        /// <param name="prediction"></param>
+        /// <param name="frameTime"></param>
+        public virtual void UpdateAfterSolve(bool prediction, float frameTime) {}
+
+        /// <summary>
+        ///     Run before a particular map starts.
+        /// </summary>
+        /// <param name="prediction"></param>
+        /// <param name="map"></param>
+        /// <param name="frameTime"></param>
+        public virtual void UpdateBeforeMapSolve(bool prediction, PhysicsMap map, float frameTime) {}
+
+        /// <summary>
+        ///     Run after a particular map finishes.
+        /// </summary>
+        /// <param name="prediction"></param>
+        /// <param name="map"></param>
+        /// <param name="frameTime"></param>
+        public virtual void UpdateAfterMapSolve(bool prediction, PhysicsMap map, float frameTime) {}
+    }
+}

Robust.Shared/Physics/DebugDrawRayMessage.cs

@@ -0,0 +1,14 @@
+using Robust.Shared.GameObjects;
+
+namespace Robust.Shared.Physics
+{
+    public sealed class DebugDrawRayMessage : EntitySystemMessage
+    {
+        public DebugRayData Data { get; }
+
+        public DebugDrawRayMessage(DebugRayData data)
+        {
+            Data = data;
+        }
+    }
+}

Robust.Shared/Physics/DebugDrawingHandle.cs

@@ -11,6 +11,8 @@ namespace Robust.Shared.Physics
         public abstract void DrawRect(in Box2 box, in Color color);
         public abstract void DrawRect(in Box2Rotated box, in Color color);
         public abstract void DrawCircle(Vector2 origin, float radius, in Color color);
+        public abstract void DrawPolygonShape(Vector2[] vertices, in Color color);
+        public abstract void DrawLine(Vector2 start, Vector2 end, in Color color);
 
         public abstract void SetTransform(in Matrix3 transform);
         public abstract Color CalcWakeColor(Color color, float wakePercent);

Robust.Shared/Physics/Dynamics/ContactManager.cs

@@ -0,0 +1,488 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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.Collections.Generic;
+using System.Linq;
+using Robust.Shared.Configuration;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Map;
+using Robust.Shared.Physics.Broadphase;
+using Robust.Shared.Physics.Dynamics.Contacts;
+
+namespace Robust.Shared.Physics.Dynamics
+{
+    internal sealed class ContactManager
+    {
+        // TODO: When a static body has no contacts left need to set it to sleep as otherwise it'll just show as awake
+        // for debug drawing (map never adds static bodies as awake so should be no problem there).
+
+        [Dependency] private readonly IConfigurationManager _configManager = default!;
+
+        internal MapId MapId { get; set; }
+
+        private SharedBroadPhaseSystem _broadPhaseSystem = default!;
+
+        /// <summary>
+        ///     Called when the broadphase finds two fixtures close to each other.
+        /// </summary>
+        public BroadPhaseDelegate OnBroadPhaseCollision;
+
+        /// <summary>
+        /// The set of active contacts.
+        /// </summary>
+        internal HashSet<Contact> ActiveContacts = new(128);
+
+        /// <summary>
+        /// A temporary copy of active contacts that is used during updates so
+        /// the hash set can have members added/removed during the update.
+        /// This list is cleared after every update.
+        /// </summary>
+        private List<Contact> ActiveList = new(128);
+
+        private List<ICollideBehavior> _collisionBehaviors = new();
+        private List<IPostCollide> _postCollideBehaviors = new();
+
+        public ContactManager()
+        {
+            OnBroadPhaseCollision = AddPair;
+        }
+
+        public void Initialize()
+        {
+            IoCManager.InjectDependencies(this);
+            _broadPhaseSystem = EntitySystem.Get<SharedBroadPhaseSystem>();
+        }
+
+        public void FindNewContacts(MapId mapId)
+        {
+            foreach (var broadPhase in _broadPhaseSystem.GetBroadPhases(mapId))
+            {
+                broadPhase.UpdatePairs(OnBroadPhaseCollision);
+            }
+        }
+
+        internal void UpdateContacts(ContactEdge? contactEdge, bool value)
+        {
+            if (value)
+            {
+                while (contactEdge != null)
+                {
+                    var contact = contactEdge.Contact!;
+
+                    if (!ActiveContacts.Contains(contact))
+                    {
+                        ActiveContacts.Add(contact);
+                    }
+
+                    contactEdge = contactEdge.Next;
+                }
+            }
+            else
+            {
+                while (contactEdge != null)
+                {
+                    var contact = contactEdge.Contact!;
+
+                    if (!contactEdge.Other!.Awake)
+                    {
+                        if (ActiveContacts.Contains(contact))
+                        {
+                            ActiveContacts.Remove(contact);
+                        }
+                    }
+
+                    contactEdge = contactEdge.Next;
+                }
+            }
+        }
+
+        internal void RemoveActiveContact(Contact contact)
+        {
+            if (!ActiveContacts.Contains(contact))
+            {
+                ActiveContacts.Remove(contact);
+            }
+        }
+
+        /// <summary>
+        ///     Go through the cached broadphase movement and update contacts.
+        /// </summary>
+        /// <param name="gridId"></param>
+        /// <param name="proxyA"></param>
+        /// <param name="proxyB"></param>
+        private void AddPair(GridId gridId, in FixtureProxy proxyA, in FixtureProxy proxyB)
+        {
+            Fixture fixtureA = proxyA.Fixture;
+            Fixture fixtureB = proxyB.Fixture;
+
+            int indexA = proxyA.ChildIndex;
+            int indexB = proxyB.ChildIndex;
+
+            PhysicsComponent bodyA = fixtureA.Body;
+            PhysicsComponent bodyB = fixtureB.Body;
+
+            // Are the fixtures on the same body?
+            if (bodyA.Owner.Uid.Equals(bodyB.Owner.Uid)) return;
+
+            // Box2D checks the mask / layer below but IMO doing it before contact is better.
+            // Check default filter
+            if (!ShouldCollide(fixtureA, fixtureB))
+                return;
+
+            // Does a contact already exist?
+            var edge = bodyB.ContactEdges;
+
+            while (edge != null)
+            {
+                if (edge.Other == bodyA)
+                {
+                    Fixture fA = edge.Contact?.FixtureA!;
+                    Fixture fB = edge.Contact?.FixtureB!;
+                    var iA = edge.Contact!.ChildIndexA;
+                    var iB = edge.Contact!.ChildIndexB;
+
+                    if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
+                    {
+                        // A contact already exists.
+                        return;
+                    }
+
+                    if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
+                    {
+                        // A contact already exists.
+                        return;
+                    }
+                }
+
+                edge = edge.Next;
+            }
+
+            // Does a joint override collision? Is at least one body dynamic?
+            if (!bodyB.ShouldCollide(bodyA))
+                return;
+
+            //FPE feature: BeforeCollision delegate
+            /*
+            if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
+                return;
+
+            if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
+                return;
+            */
+
+            // Call the factory.
+            Contact c = Contact.Create(gridId, fixtureA, indexA, fixtureB, indexB);
+
+            // Contact creation may swap fixtures.
+            fixtureA = c.FixtureA!;
+            fixtureB = c.FixtureB!;
+            bodyA = fixtureA.Body;
+            bodyB = fixtureB.Body;
+
+            // Insert into the world.
+            ActiveContacts.Add(c);
+
+            // Connect to island graph.
+
+            // Connect to body A
+            c.NodeA.Contact = c;
+            c.NodeA.Other = bodyB;
+
+            c.NodeA.Previous = null;
+            c.NodeA.Next = bodyA.ContactEdges;
+
+            if (bodyA.ContactEdges != null)
+            {
+                bodyA.ContactEdges.Previous = c.NodeA;
+            }
+            bodyA.ContactEdges = c.NodeA;
+
+            // Connect to body B
+            c.NodeB.Contact = c;
+            c.NodeB.Other = bodyA;
+
+            c.NodeB.Previous = null;
+            c.NodeB.Next = bodyB.ContactEdges;
+
+            if (bodyB.ContactEdges != null)
+            {
+                bodyB.ContactEdges.Previous = c.NodeB;
+            }
+            bodyB.ContactEdges = c.NodeB;
+
+            // Wake up the bodies
+            if (fixtureA.Hard && fixtureB.Hard)
+            {
+                bodyA.Awake = true;
+                bodyB.Awake = true;
+            }
+        }
+
+        private bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
+        {
+            // TODO: Should we only be checking one side's mask? I think maybe fixtureB? IDK
+            return !((fixtureA.CollisionMask & fixtureB.CollisionLayer) == 0x0 &&
+                     (fixtureB.CollisionMask & fixtureA.CollisionLayer) == 0x0);
+        }
+
+        public void Destroy(Contact contact)
+        {
+            Fixture fixtureA = contact.FixtureA!;
+            Fixture fixtureB = contact.FixtureB!;
+            PhysicsComponent bodyA = fixtureA.Body;
+            PhysicsComponent bodyB = fixtureB.Body;
+
+            if (contact.IsTouching)
+            {
+                //Report the separation to both participants:
+                // TODO: Needs to do like a comp message and system message
+                // fixtureA?.OnSeparation(fixtureA, fixtureB);
+
+                //Reverse the order of the reported fixtures. The first fixture is always the one that the
+                //user subscribed to.
+                // fixtureB.OnSeparation(fixtureB, fixtureA);
+
+                // EndContact(contact);
+            }
+
+            // Remove from body 1
+            if (contact.NodeA.Previous != null)
+            {
+                contact.NodeA.Previous.Next = contact.NodeA.Next;
+            }
+
+            if (contact.NodeA.Next != null)
+            {
+                contact.NodeA.Next.Previous = contact.NodeA.Previous;
+            }
+
+            if (contact.NodeA == bodyA.ContactEdges)
+            {
+                bodyA.ContactEdges = contact.NodeA.Next;
+            }
+
+            // Remove from body 2
+            if (contact.NodeB.Previous != null)
+            {
+                contact.NodeB.Previous.Next = contact.NodeB.Next;
+            }
+
+            if (contact.NodeB.Next != null)
+            {
+                contact.NodeB.Next.Previous = contact.NodeB.Previous;
+            }
+
+            if (contact.NodeB == bodyB.ContactEdges)
+            {
+                bodyB.ContactEdges = contact.NodeB.Next;
+            }
+
+            ActiveContacts.Remove(contact);
+
+            contact.Destroy();
+        }
+
+        internal void Collide()
+        {
+            // TODO: We need to handle collisions during prediction but also handle the start / stop colliding shit during sim ONLY
+
+            // Update awake contacts
+            ActiveList.AddRange(ActiveContacts);
+
+            // Can be changed while enumerating
+            foreach (var contact in ActiveList)
+            {
+                Fixture fixtureA = contact.FixtureA!;
+                Fixture fixtureB = contact.FixtureB!;
+                int indexA = contact.ChildIndexA;
+                int indexB = contact.ChildIndexB;
+                PhysicsComponent bodyA = fixtureA.Body;
+                PhysicsComponent bodyB = fixtureB.Body;
+
+                // Do not try to collide disabled bodies
+                // FPE just continues here but in our case I think it's better to also destroy the contact.
+                if (!bodyA.CanCollide || !bodyB.CanCollide)
+                {
+                    Contact cNuke = contact;
+                    Destroy(cNuke);
+                    continue;
+                }
+
+                // Is this contact flagged for filtering?
+                if (contact.FilterFlag)
+                {
+                    // Should these bodies collide?
+                    if (!bodyB.ShouldCollide(bodyA))
+                    {
+                        Contact cNuke = contact;
+                        Destroy(cNuke);
+                        continue;
+                    }
+
+                    // Check default filtering
+                    if (!ShouldCollide(fixtureA, fixtureB))
+                    {
+                        Contact cNuke = contact;
+                        Destroy(cNuke);
+                        continue;
+                    }
+
+                    // Check user filtering.
+                    /*
+                    if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
+                    {
+                        Contact cNuke = c;
+                        Destroy(cNuke);
+                        continue;
+                    }
+                    */
+
+                    // Clear the filtering flag.
+                    contact.FilterFlag = false;
+                }
+
+                var activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static;
+                var activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static;
+
+                // At least one body must be awake and it must be dynamic or kinematic.
+                if (!activeA && !activeB)
+                {
+                    ActiveContacts.Remove(contact);
+                    continue;
+                }
+
+                // TODO: Need to handle moving grids
+                bool? overlap = false;
+
+                // Sloth addition: Kind of hacky and might need to be removed at some point.
+                // One of the bodies was probably put into nullspace so we need to remove I think.
+                if (fixtureA.Proxies.ContainsKey(contact.GridId) && fixtureB.Proxies.ContainsKey(contact.GridId))
+                {
+                    var proxyIdA = fixtureA.Proxies[contact.GridId][indexA].ProxyId;
+                    var proxyIdB = fixtureB.Proxies[contact.GridId][indexB].ProxyId;
+
+                    var broadPhase = _broadPhaseSystem.GetBroadPhase(MapId, contact.GridId);
+
+                    overlap = broadPhase?.TestOverlap(proxyIdA, proxyIdB);
+                }
+
+                // Here we destroy contacts that cease to overlap in the broad-phase.
+                if (overlap == false)
+                {
+                    Contact cNuke = contact;
+                    Destroy(cNuke);
+                    continue;
+                }
+
+                // The contact persists.
+                contact.Update(this);
+            }
+
+            ActiveList.Clear();
+        }
+
+        public void PreSolve()
+        {
+            // We'll do pre and post-solve around all islands rather than each specific island as it seems cleaner with race conditions.
+            foreach (var contact in ActiveContacts)
+            {
+                if (!contact.IsTouching) continue;
+
+                // God this area's hard to read but tl;dr run ICollideBehavior and IPostCollide and try to optimise it a little.
+                var bodyA = contact.FixtureA!.Body;
+                var bodyB = contact.FixtureB!.Body;
+
+                if (!bodyA.Entity.Deleted)
+                {
+                    foreach (var behavior in bodyA.Owner.GetAllComponents<ICollideBehavior>())
+                    {
+                        _collisionBehaviors.Add(behavior);
+                    }
+
+                    foreach (var behavior in _collisionBehaviors)
+                    {
+                        if (bodyB.Deleted) break;
+                        behavior.CollideWith(bodyA, bodyB);
+                    }
+
+                    _collisionBehaviors.Clear();
+                }
+
+                if (!bodyB.Entity.Deleted)
+                {
+                    foreach (var behavior in bodyB.Owner.GetAllComponents<ICollideBehavior>())
+                    {
+                        _collisionBehaviors.Add(behavior);
+                    }
+
+                    foreach (var behavior in _collisionBehaviors)
+                    {
+                        if (bodyA.Deleted) break;
+                        behavior.CollideWith(bodyB, bodyA);
+                    }
+
+                    _collisionBehaviors.Clear();
+                }
+
+                if (!bodyA.Entity.Deleted)
+                {
+                    foreach (var behavior in bodyA.Owner.GetAllComponents<IPostCollide>())
+                    {
+                        _postCollideBehaviors.Add(behavior);
+                    }
+
+                    foreach (var behavior in _postCollideBehaviors)
+                    {
+                        behavior.PostCollide(bodyA, bodyB);
+                        if (bodyB.Deleted) break;
+                    }
+
+                    _postCollideBehaviors.Clear();
+                }
+
+                if (!bodyB.Entity.Deleted)
+                {
+                    foreach (var behavior in bodyB.Owner.GetAllComponents<IPostCollide>())
+                    {
+                        _postCollideBehaviors.Add(behavior);
+                    }
+
+                    foreach (var behavior in _postCollideBehaviors)
+                    {
+                        behavior.PostCollide(bodyB, bodyA);
+                        if (bodyA.Deleted) break;
+                    }
+
+                    _postCollideBehaviors.Clear();
+                }
+            }
+        }
+
+        public void PostSolve()
+        {
+
+        }
+    }
+
+    public delegate void BroadPhaseDelegate(GridId gridId, in FixtureProxy proxyA, in FixtureProxy proxyB);
+}

Robust.Shared/Physics/Dynamics/Contacts/Contact.cs

@@ -0,0 +1,505 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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;
+using System.Collections.Generic;
+using Robust.Shared.GameObjects;
+using Robust.Shared.IoC;
+using Robust.Shared.Map;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Collision;
+using Robust.Shared.Physics.Dynamics.Shapes;
+using Robust.Shared.Utility;
+
+namespace Robust.Shared.Physics.Dynamics.Contacts
+{
+    internal sealed class Contact
+    {
+        [Dependency] private readonly ICollisionManager _collisionManager = default!;
+        [Dependency] private readonly IEntityManager _entityManager = default!;
+
+        public ContactEdge NodeA = new();
+        public ContactEdge NodeB = new();
+
+        public Fixture? FixtureA;
+        public Fixture? FixtureB;
+
+        public Manifold Manifold = new();
+
+        private ContactType _type;
+
+        /// <summary>
+        ///     Ordering is under <see cref="ShapeType"/>
+        ///     uses enum to work out which collision evaluation to use.
+        /// </summary>
+        private static ContactType[,] _registers = {
+                                                           {
+                                                               // Circle register
+                                                               ContactType.Circle,
+                                                               ContactType.EdgeAndCircle,
+                                                               ContactType.PolygonAndCircle,
+                                                               ContactType.ChainAndCircle,
+                                                               ContactType.AabbAndCircle,
+                                                               ContactType.RectAndCircle,
+                                                           },
+                                                           {
+                                                               // Edge register
+                                                               ContactType.EdgeAndCircle,
+                                                               ContactType.NotSupported, // Edge
+                                                               ContactType.EdgeAndPolygon,
+                                                               ContactType.NotSupported, // Chain
+                                                               ContactType.NotSupported, // Aabb
+                                                               ContactType.NotSupported, // Rect
+                                                           },
+                                                           {
+                                                               // Polygon register
+                                                               ContactType.PolygonAndCircle,
+                                                               ContactType.EdgeAndPolygon,
+                                                               ContactType.Polygon,
+                                                               ContactType.ChainAndPolygon,
+                                                               ContactType.AabbAndPolygon,
+                                                               ContactType.RectAndPolygon,
+                                                           },
+                                                           {
+                                                               // Chain register
+                                                               ContactType.ChainAndCircle,
+                                                               ContactType.NotSupported, // Edge
+                                                               ContactType.ChainAndPolygon,
+                                                               ContactType.NotSupported, // Chain
+                                                               ContactType.NotSupported, // Aabb - TODO Just cast to poly
+                                                               ContactType.NotSupported, // Rect - TODO Just cast to poly
+                                                           },
+                                                           {
+                                                               // Aabb register
+                                                               ContactType.AabbAndCircle,
+                                                               ContactType.NotSupported, // Edge - TODO Just cast to poly
+                                                               ContactType.AabbAndPolygon,
+                                                               ContactType.NotSupported, // Chain - TODO Just cast to poly
+                                                               ContactType.Aabb,
+                                                               ContactType.AabbAndRect,
+                                                           },
+                                                           {
+                                                               // Rectangle register
+                                                               ContactType.RectAndCircle,
+                                                               ContactType.NotSupported, // Edge - TODO Just cast to poly
+                                                               ContactType.RectAndPolygon,
+                                                               ContactType.NotSupported, // Chain - TODO Just cast to poly
+                                                               ContactType.AabbAndRect,
+                                                               ContactType.Rect,
+                                                           }
+                                                       };
+
+        /// <summary>
+        ///     Has this contact already been added to an island?
+        /// </summary>
+        public bool IslandFlag { get; set; }
+
+        public bool FilterFlag { get; set; }
+
+        /// <summary>
+        ///     Determines whether the contact is touching.
+        /// </summary>
+        public bool IsTouching { get; set; }
+
+        // Some day we'll refactor it to be more like EntityCoordinates
+        public GridId GridId { get; set; } = GridId.Invalid;
+
+        /// Enable/disable this contact. This can be used inside the pre-solve
+        /// contact listener. The contact is only disabled for the current
+        /// time step (or sub-step in continuous collisions).
+        /// NOTE: If you are setting Enabled to a constant true or false,
+        /// use the explicit Enable() or Disable() functions instead to
+        /// save the CPU from doing a branch operation.
+        public bool Enabled { get; set; }
+
+        /// <summary>
+        ///     Get the child primitive index for fixture A.
+        /// </summary>
+        public int ChildIndexA { get; internal set; }
+
+        /// <summary>
+        ///     Get the child primitive index for fixture B.
+        /// </summary>
+        public int ChildIndexB { get; internal set; }
+
+        /// <summary>
+        ///     The mixed friction of the 2 fixtures.
+        /// </summary>
+        public float Friction { get; set; }
+
+        /// <summary>
+        ///     The mixed restitution of the 2 fixtures.
+        /// </summary>
+        public float Restitution { get; set; }
+
+        /// <summary>
+        ///     Used for conveyor belt behavior in m/s.
+        /// </summary>
+        public float TangentSpeed { get; set; }
+
+        public Contact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
+        {
+            IoCManager.InjectDependencies(this);
+            Reset(fixtureA, indexA, fixtureB, indexB);
+        }
+
+        /// <summary>
+        ///     Gets a new contact to use, using the contact pool if relevant.
+        /// </summary>
+        /// <param name="fixtureA"></param>
+        /// <param name="indexA"></param>
+        /// <param name="fixtureB"></param>
+        /// <param name="indexB"></param>
+        /// <returns></returns>
+        internal static Contact Create(GridId gridId, Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
+        {
+            var type1 = fixtureA.Shape.ShapeType;
+            var type2 = fixtureB.Shape.ShapeType;
+
+            DebugTools.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
+            DebugTools.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);
+
+            Queue<Contact> pool = fixtureA.Body.PhysicsMap.ContactPool;
+            if (pool.TryDequeue(out var contact))
+            {
+                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
+                {
+                    contact.Reset(fixtureA, indexA, fixtureB, indexB);
+                }
+                else
+                {
+                    contact.Reset(fixtureB, indexB, fixtureA, indexA);
+                }
+            }
+            else
+            {
+                // Edge+Polygon is non-symmetrical due to the way Erin handles collision type registration.
+                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
+                {
+                    contact = new Contact(fixtureA, indexA, fixtureB, indexB);
+                }
+                else
+                {
+                    contact = new Contact(fixtureB, indexB, fixtureA, indexA);
+                }
+            }
+
+            contact.GridId = gridId;
+            contact._type = _registers[(int) type1, (int) type2];
+
+            return contact;
+        }
+
+        public void ResetRestitution()
+        {
+            Restitution = MathF.Max(FixtureA?.Restitution ?? 0.0f, FixtureB?.Restitution ?? 0.0f);
+        }
+
+        public void ResetFriction()
+        {
+            Friction = MathF.Sqrt(FixtureA?.Friction ?? 0.0f * FixtureB?.Friction ?? 0.0f);
+        }
+
+        private void Reset(Fixture? fixtureA, int indexA, Fixture? fixtureB, int indexB)
+        {
+            Enabled = true;
+            IsTouching = false;
+            IslandFlag = false;
+            FilterFlag = false;
+            // TOIFlag = false;
+
+            FixtureA = fixtureA;
+            FixtureB = fixtureB;
+
+            ChildIndexA = indexA;
+            ChildIndexB = indexB;
+
+            Manifold.PointCount = 0;
+
+            NodeA.Contact = null;
+            NodeA.Previous = null;
+            NodeA.Next = null;
+            NodeA.Other = null;
+
+            NodeB.Contact = null;
+            NodeB.Previous = null;
+            NodeB.Next = null;
+            NodeB.Other = null;
+
+            // _toiCount = 0;
+
+            //FPE: We only set the friction and restitution if we are not destroying the contact
+            if (FixtureA != null && FixtureB != null)
+            {
+                Friction = MathF.Sqrt(FixtureA.Friction * FixtureB.Friction);
+                Restitution = MathF.Max(FixtureA.Restitution, FixtureB.Restitution);
+            }
+
+            TangentSpeed = 0;
+        }
+
+        /// <summary>
+        /// Gets the world manifold.
+        /// </summary>
+        public void GetWorldManifold(out Vector2 normal, out Vector2[] points)
+        {
+            PhysicsComponent bodyA = FixtureA?.Body!;
+            PhysicsComponent bodyB = FixtureB?.Body!;
+            IPhysShape shapeA = FixtureA?.Shape!;
+            IPhysShape shapeB = FixtureB?.Shape!;
+
+            ContactSolver.InitializeManifold(Manifold, bodyA.GetTransform(), bodyB.GetTransform(), shapeA.Radius, shapeB.Radius, out normal, out points);
+        }
+
+        /// <summary>
+        /// Update the contact manifold and touching status.
+        /// Note: do not assume the fixture AABBs are overlapping or are valid.
+        /// </summary>
+        /// <param name="contactManager">The contact manager.</param>
+        internal void Update(ContactManager contactManager)
+        {
+            PhysicsComponent bodyA = FixtureA!.Body;
+            PhysicsComponent bodyB = FixtureB!.Body;
+
+            if (FixtureA == null || FixtureB == null)
+                return;
+
+            Manifold oldManifold = Manifold.Clone();
+
+            // Re-enable this contact.
+            Enabled = true;
+
+            bool touching;
+            bool wasTouching = IsTouching;
+
+            bool sensor = !FixtureA.Hard || !FixtureB.Hard;
+
+            // Is this contact a sensor?
+            if (sensor)
+            {
+                IPhysShape shapeA = FixtureA.Shape;
+                IPhysShape shapeB = FixtureB.Shape;
+                touching = _collisionManager.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, bodyA.GetTransform(), bodyB.GetTransform());
+
+                // Sensors don't generate manifolds.
+                Manifold.PointCount = 0;
+            }
+            else
+            {
+                Evaluate(Manifold, bodyA.GetTransform(), bodyB.GetTransform());
+                touching = Manifold.PointCount > 0;
+
+                // Match old contact ids to new contact ids and copy the
+                // stored impulses to warm start the solver.
+                for (int i = 0; i < Manifold.PointCount; ++i)
+                {
+                    ManifoldPoint mp2 = Manifold.Points[i];
+                    mp2.NormalImpulse = 0.0f;
+                    mp2.TangentImpulse = 0.0f;
+                    ContactID id2 = mp2.Id;
+
+                    for (int j = 0; j < oldManifold.PointCount; ++j)
+                    {
+                        ManifoldPoint mp1 = oldManifold.Points[j];
+
+                        if (mp1.Id.Key == id2.Key)
+                        {
+                            mp2.NormalImpulse = mp1.NormalImpulse;
+                            mp2.TangentImpulse = mp1.TangentImpulse;
+                            break;
+                        }
+                    }
+
+                    Manifold.Points[i] = mp2;
+                }
+
+                if (touching != wasTouching)
+                {
+                    bodyA.Awake = true;
+                    bodyB.Awake = true;
+                }
+            }
+
+            IsTouching = touching;
+            // TODO: Need to do collision behaviors around here.
+
+            if (!wasTouching)
+            {
+                if (touching)
+                {
+                    var enabledA = true;
+                    var enabledB = true;
+
+                    /*
+                    // Report the collision to both participants. Track which ones returned true so we can
+                    // later call OnSeparation if the contact is disabled for a different reason.
+                    if (FixtureA.OnCollision != null)
+                        foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
+                            enabledA = handler(FixtureA, FixtureB, this) && enabledA;
+
+                    // Reverse the order of the reported fixtures. The first fixture is always the one that the
+                    // user subscribed to.
+                    if (FixtureB.OnCollision != null)
+                        foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
+                            enabledB = handler(FixtureB, FixtureA, this) && enabledB;
+                    */
+
+                    Enabled = enabledA && enabledB;
+
+                    // BeginContact can also return false and disable the contact
+                    /*
+                    if (enabledA && enabledB && contactManager.BeginContact != null)
+                        Enabled = contactManager.BeginContact(this);
+                    */
+                }
+            }
+            else
+            {
+                if (!touching)
+                {
+                    /*
+                    //Report the separation to both participants:
+                    if (FixtureA != null && FixtureA.OnSeparation != null)
+                        FixtureA.OnSeparation(FixtureA, FixtureB);
+
+                    //Reverse the order of the reported fixtures. The first fixture is always the one that the
+                    //user subscribed to.
+                    if (FixtureB != null && FixtureB.OnSeparation != null)
+                        FixtureB.OnSeparation(FixtureB, FixtureA);
+
+                    if (contactManager.EndContact != null)
+                        contactManager.EndContact(this);
+                    */
+                }
+            }
+
+            if (sensor)
+                return;
+
+            _entityManager.EventBus.RaiseEvent(EventSource.Local, new PreSolveMessage(this, oldManifold));
+        }
+
+        /// <summary>
+        ///     Evaluate this contact with your own manifold and transforms.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="transformA">The first transform.</param>
+        /// <param name="transformB">The second transform.</param>
+        private void Evaluate(Manifold manifold, in Transform transformA, in Transform transformB)
+        {
+            // This is expensive and shitcodey, see below.
+            switch (_type)
+            {
+                // TODO: Need a unit test for these.
+                case ContactType.Polygon:
+                    _collisionManager.CollidePolygons(manifold, new PolygonShape(FixtureA!.Shape), transformA, new PolygonShape(FixtureB!.Shape), transformB);
+                    break;
+                case ContactType.PolygonAndCircle:
+                    _collisionManager.CollidePolygonAndCircle(manifold, new PolygonShape(FixtureA!.Shape), transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.EdgeAndCircle:
+                    _collisionManager.CollideEdgeAndCircle(manifold, (EdgeShape) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.EdgeAndPolygon:
+                    _collisionManager.CollideEdgeAndPolygon(manifold, (EdgeShape) FixtureA!.Shape, transformA, new PolygonShape(FixtureB!.Shape), transformB);
+                    break;
+                case ContactType.ChainAndCircle:
+                    throw new NotImplementedException();
+                    /*
+                    ChainShape chain = (ChainShape)FixtureA.Shape;
+                    chain.GetChildEdge(_edge, ChildIndexA);
+                    Collision.CollisionManager.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
+                    */
+                    break;
+                case ContactType.ChainAndPolygon:
+                    throw new NotImplementedException();
+                    /*
+                    ChainShape loop2 = (ChainShape)FixtureA.Shape;
+                    loop2.GetChildEdge(_edge, ChildIndexA);
+                    Collision.CollisionManager.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB);
+                    */
+                    break;
+                case ContactType.Circle:
+                    _collisionManager.CollideCircles(manifold, (PhysShapeCircle) FixtureA!.Shape, in transformA, (PhysShapeCircle) FixtureB!.Shape, in transformB);
+                    break;
+                // Custom ones
+                // This is kind of shitcodey and originally I just had the poly version but if we get an AABB -> whatever version directly you'll get good optimisations over a cast.
+                case ContactType.Aabb:
+                    _collisionManager.CollideAabbs(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeAabb) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.AabbAndCircle:
+                    _collisionManager.CollideAabbAndCircle(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.AabbAndPolygon:
+                    _collisionManager.CollideAabbAndPolygon(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PolygonShape) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.AabbAndRect:
+                    _collisionManager.CollideAabbAndRect(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeRect) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.Rect:
+                    _collisionManager.CollideRects(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PhysShapeRect) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.RectAndCircle:
+                    _collisionManager.CollideRectAndCircle(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
+                    break;
+                case ContactType.RectAndPolygon:
+                    _collisionManager.CollideRectAndPolygon(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PolygonShape) FixtureB!.Shape, transformB);
+                    break;
+            }
+        }
+
+        internal void Destroy()
+        {
+            // Seems like active contacts were never used in farseer anyway
+            // FixtureA?.Body.PhysicsMap.ContactManager.RemoveActiveContact(this);
+            FixtureA?.Body.PhysicsMap.ContactPool.Enqueue(this);
+
+            if (Manifold.PointCount > 0 && FixtureA?.Hard == true && FixtureB?.Hard == true)
+            {
+                FixtureA.Body.Awake = true;
+                FixtureB.Body.Awake = true;
+            }
+
+            Reset(null, 0, null, 0);
+        }
+
+        private enum ContactType : byte
+        {
+            NotSupported,
+            Polygon,
+            PolygonAndCircle,
+            Circle,
+            EdgeAndPolygon,
+            EdgeAndCircle,
+            ChainAndPolygon,
+            ChainAndCircle,
+            // Custom
+            Aabb,
+            AabbAndPolygon,
+            AabbAndCircle,
+            AabbAndRect,
+            Rect,
+            RectAndCircle,
+            RectAndPolygon,
+        }
+    }
+}

Robust.Shared/Physics/Dynamics/Contacts/ContactEdge.cs

@@ -0,0 +1,50 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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 Robust.Shared.GameObjects;
+
+namespace Robust.Shared.Physics.Dynamics.Contacts
+{
+    internal sealed class ContactEdge
+    {
+        /// <summary>
+        ///     This contact in the chain.
+        /// </summary>
+        public Contact? Contact { get; set; } = default!;
+
+        public ContactEdge? Next { get; set; }
+
+        public ContactEdge? Previous { get; set; } = default!;
+
+        // Subject to change
+        public PhysicsComponent? Other { get; set; } = default!;
+
+        public ContactEdge() {}
+
+        public ContactEdge(Contact contact, ContactEdge previous, PhysicsComponent other)
+        {
+            Contact = contact;
+            Previous = previous;
+            Other = other;
+        }
+    }
+}

Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs

@@ -0,0 +1,66 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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 Robust.Shared.Maths;
+using Robust.Shared.Physics.Collision;
+
+namespace Robust.Shared.Physics.Dynamics.Contacts
+{
+    internal sealed class ContactPositionConstraint
+    {
+        /// <summary>
+        ///     Index of BodyA in the island.
+        /// </summary>
+        public int IndexA { get; set; }
+
+        /// <summary>
+        ///     Index of BodyB in the island.
+        /// </summary>
+        public int IndexB { get; set; }
+
+        public Vector2[] LocalPoints = new Vector2[2];
+
+        public Vector2 LocalNormal;
+
+        public Vector2 LocalPoint;
+
+        public float InvMassA;
+
+        public float InvMassB;
+
+        public Vector2 LocalCenterA;
+
+        public Vector2 LocalCenterB;
+
+        public float InvIA;
+
+        public float InvIB;
+
+        public ManifoldType Type;
+
+        public float RadiusA;
+
+        public float RadiusB;
+
+        public int PointCount;
+    }
+}

Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs

@@ -0,0 +1,861 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 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;
+using Robust.Shared.Configuration;
+using Robust.Shared.IoC;
+using Robust.Shared.Maths;
+using Robust.Shared.Physics.Collision;
+using Robust.Shared.Utility;
+
+namespace Robust.Shared.Physics.Dynamics.Contacts
+{
+    internal sealed class ContactSolver
+    {
+        [Dependency] private readonly IConfigurationManager _configManager = default!;
+
+        private bool _warmStarting;
+        private float _velocityThreshold;
+        private float _baumgarte;
+        private float _linearSlop;
+        private float _maxLinearCorrection;
+
+        private Vector2[] _linearVelocities = Array.Empty<Vector2>();
+        private float[] _angularVelocities = Array.Empty<float>();
+
+        private Vector2[] _positions = Array.Empty<Vector2>();
+        private float[] _angles = Array.Empty<float>();
+
+        private Contact[] _contacts = Array.Empty<Contact>();
+        private int _contactCount;
+
+        private ContactVelocityConstraint[] _velocityConstraints = Array.Empty<ContactVelocityConstraint>();
+        private ContactPositionConstraint[] _positionConstraints = Array.Empty<ContactPositionConstraint>();
+
+        public void Initialize()
+        {
+            IoCManager.InjectDependencies(this);
+
+            _warmStarting = _configManager.GetCVar(CVars.WarmStarting);
+            _configManager.OnValueChanged(CVars.WarmStarting, value => _warmStarting = value);
+
+            _velocityThreshold = _configManager.GetCVar(CVars.VelocityThreshold);
+            _configManager.OnValueChanged(CVars.VelocityThreshold, value => _velocityThreshold = value);
+
+            _baumgarte = _configManager.GetCVar(CVars.Baumgarte);
+            _configManager.OnValueChanged(CVars.Baumgarte, value => _baumgarte = value);
+
+            _linearSlop = _configManager.GetCVar(CVars.LinearSlop);
+            _configManager.OnValueChanged(CVars.LinearSlop, value => _linearSlop = value);
+
+            _maxLinearCorrection = _configManager.GetCVar(CVars.MaxLinearCorrection);
+            _configManager.OnValueChanged(CVars.MaxLinearCorrection, value => _maxLinearCorrection = value);
+        }
+
+        public void Reset(SolverData data, int contactCount, Contact[] contacts)
+        {
+            _linearVelocities = data.LinearVelocities;
+            _angularVelocities = data.AngularVelocities;
+
+            _positions = data.Positions;
+            _positions = data.Positions;
+            _angles = data.Angles;
+
+            _contactCount = contactCount;
+            _contacts = contacts;
+
+            // If we need more constraints then grow the cached arrays
+            if (_velocityConstraints.Length < contactCount)
+            {
+                var oldLength = _velocityConstraints.Length;
+
+                Array.Resize(ref _velocityConstraints, contactCount * 2);
+                Array.Resize(ref _positionConstraints, contactCount * 2);
+
+                for (var i = oldLength; i < _velocityConstraints.Length; i++)
+                {
+                    _velocityConstraints[i] = new ContactVelocityConstraint();
+                    _positionConstraints[i] = new ContactPositionConstraint();
+                }
+            }
+
+            // Build constraints
+            // For now these are going to be bare but will change
+            for (var i = 0; i < _contactCount; i++)
+            {
+                var contact = contacts[i];
+                Fixture fixtureA = contact.FixtureA!;
+                Fixture fixtureB = contact.FixtureB!;
+                var shapeA = fixtureA.Shape;
+                var shapeB = fixtureB.Shape;
+                float radiusA = shapeA.Radius;
+                float radiusB = shapeB.Radius;
+                var bodyA = fixtureA.Body;
+                var bodyB = fixtureB.Body;
+                var manifold = contact.Manifold;
+
+                int pointCount = manifold.PointCount;
+                DebugTools.Assert(pointCount > 0);
+
+                var velocityConstraint = _velocityConstraints[i];
+                velocityConstraint.Friction = contact.Friction;
+                velocityConstraint.Restitution = contact.Restitution;
+                velocityConstraint.TangentSpeed = contact.TangentSpeed;
+                velocityConstraint.IndexA = bodyA.IslandIndex;
+                velocityConstraint.IndexB = bodyB.IslandIndex;
+                velocityConstraint.InvMassA = bodyA.InvMass;
+                velocityConstraint.InvMassB = bodyB.InvMass;
+                velocityConstraint.InvIA = bodyA.InvI;
+                velocityConstraint.InvIB = bodyB.InvI;
+                velocityConstraint.ContactIndex = i;
+                velocityConstraint.PointCount = pointCount;
+
+                for (var x = 0; x < 2; x++)
+                {
+                    velocityConstraint.K[x] = Vector2.Zero;
+                    velocityConstraint.NormalMass[x] = Vector2.Zero;
+                }
+
+                var positionConstraint = _positionConstraints[i];
+                positionConstraint.IndexA = bodyA.IslandIndex;
+                positionConstraint.IndexB = bodyB.IslandIndex;
+                positionConstraint.InvMassA = bodyA.InvMass;
+                positionConstraint.InvMassB = bodyB.InvMass;
+                // TODO: Dis
+                // positionConstraint.LocalCenterA = bodyA._sweep.LocalCenter;
+                // positionConstraint.LocalCenterB = bodyB._sweep.LocalCenter;
+                positionConstraint.LocalCenterA = bodyA.LocalCenter;
+                positionConstraint.LocalCenterB = bodyB.LocalCenter;
+
+                positionConstraint.InvIA = bodyA.InvI;
+                positionConstraint.InvIB = bodyB.InvI;
+                positionConstraint.LocalNormal = manifold.LocalNormal;
+                positionConstraint.LocalPoint = manifold.LocalPoint;
+                positionConstraint.PointCount = pointCount;
+                positionConstraint.RadiusA = radiusA;
+                positionConstraint.RadiusB = radiusB;
+                positionConstraint.Type = manifold.Type;
+
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    var contactPoint = manifold.Points[j];
+                    var constraintPoint = velocityConstraint.Points[j];
+
+                    if (_warmStarting)
+                    {
+                        constraintPoint.NormalImpulse = data.DtRatio * contactPoint.NormalImpulse;
+                        constraintPoint.TangentImpulse = data.DtRatio * contactPoint.TangentImpulse;
+                    }
+                    else
+                    {
+                        constraintPoint.NormalImpulse = 0.0f;
+                        constraintPoint.TangentImpulse = 0.0f;
+                    }
+
+                    constraintPoint.RelativeVelocityA = Vector2.Zero;
+                    constraintPoint.RelativeVelocityB = Vector2.Zero;
+                    constraintPoint.NormalMass = 0.0f;
+                    constraintPoint.TangentMass = 0.0f;
+                    constraintPoint.VelocityBias = 0.0f;
+
+                    positionConstraint.LocalPoints[j] = contactPoint.LocalPoint;
+                }
+            }
+        }
+
+        public void InitializeVelocityConstraints()
+        {
+            for (var i = 0; i < _contactCount; ++i)
+            {
+                var velocityConstraint = _velocityConstraints[i];
+                var positionConstraint = _positionConstraints[i];
+
+                var radiusA = positionConstraint.RadiusA;
+                var radiusB = positionConstraint.RadiusB;
+                var manifold = _contacts[velocityConstraint.ContactIndex].Manifold;
+
+                var indexA = velocityConstraint.IndexA;
+                var indexB = velocityConstraint.IndexB;
+
+                var invMassA = velocityConstraint.InvMassA;
+                var invMassB = velocityConstraint.InvMassB;
+                var invIA = velocityConstraint.InvIA;
+                var invIB = velocityConstraint.InvIB;
+                var localCenterA = positionConstraint.LocalCenterA;
+                var localCenterB = positionConstraint.LocalCenterB;
+
+                var centerA = _positions[indexA];
+                var angleA = _angles[indexA];
+                var linVelocityA = _linearVelocities[indexA];
+                var angVelocityA = _angularVelocities[indexA];
+
+                var centerB = _positions[indexB];
+                var angleB = _angles[indexB];
+                var linVelocityB = _linearVelocities[indexB];
+                var angVelocityB = _angularVelocities[indexB];
+
+                DebugTools.Assert(manifold.PointCount > 0);
+
+                Transform xfA = new Transform(angleA);
+                Transform xfB = new Transform(angleB);
+                xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA);
+                xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB);
+
+                Vector2 normal;
+                var points = new Vector2[2];
+                InitializeManifold(manifold, xfA, xfB, radiusA, radiusB, out normal, out points);
+
+                velocityConstraint.Normal = normal;
+
+                int pointCount = velocityConstraint.PointCount;
+
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    VelocityConstraintPoint vcp = velocityConstraint.Points[j];
+
+                    vcp.RelativeVelocityA = points[j] - centerA;
+                    vcp.RelativeVelocityB = points[j] - centerB;
+
+                    float rnA = Vector2.Cross(vcp.RelativeVelocityA, velocityConstraint.Normal);
+                    float rnB = Vector2.Cross(vcp.RelativeVelocityB, velocityConstraint.Normal);
+
+                    float kNormal = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
+
+                    vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+
+                    Vector2 tangent = Vector2.Cross(velocityConstraint.Normal, 1.0f);
+
+                    float rtA = Vector2.Cross(vcp.RelativeVelocityA, tangent);
+                    float rtB = Vector2.Cross(vcp.RelativeVelocityB, tangent);
+
+                    float kTangent = invMassA + invMassB + invIA * rtA * rtA + invIB * rtB * rtB;
+
+                    vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
+
+                    // Setup a velocity bias for restitution.
+                    vcp.VelocityBias = 0.0f;
+                    float vRel = Vector2.Dot(velocityConstraint.Normal, linVelocityB + Vector2.Cross(angVelocityB, vcp.RelativeVelocityB) - linVelocityA - Vector2.Cross(angVelocityA, vcp.RelativeVelocityA));
+                    if (vRel < -_velocityThreshold)
+                    {
+                        vcp.VelocityBias = -velocityConstraint.Restitution * vRel;
+                    }
+                }
+
+                // If we have two points, then prepare the block solver.
+                if (velocityConstraint.PointCount == 2)
+                {
+                    var vcp1 = velocityConstraint.Points[0];
+                    var vcp2 = velocityConstraint.Points[1];
+
+                    var rn1A = Vector2.Cross(vcp1.RelativeVelocityA, velocityConstraint.Normal);
+                    var rn1B = Vector2.Cross(vcp1.RelativeVelocityB, velocityConstraint.Normal);
+                    var rn2A = Vector2.Cross(vcp2.RelativeVelocityA, velocityConstraint.Normal);
+                    var rn2B = Vector2.Cross(vcp2.RelativeVelocityB, velocityConstraint.Normal);
+
+                    var k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
+                    var k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
+                    var k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
+
+                    // Ensure a reasonable condition number.
+                    const float k_maxConditionNumber = 1000.0f;
+                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
+                    {
+                        // K is safe to invert.
+                        velocityConstraint.K[0] = new Vector2(k11, k12);
+                        velocityConstraint.K[1] = new Vector2(k12, k22);
+                        velocityConstraint.NormalMass = velocityConstraint.K.Inverse();
+                    }
+                    else
+                    {
+                        // The constraints are redundant, just use one.
+                        // TODO_ERIN use deepest?
+                        velocityConstraint.PointCount = 1;
+                    }
+                }
+            }
+        }
+
+        public void WarmStart()
+        {
+            for (var i = 0; i < _contactCount; ++i)
+            {
+                var velocityConstraint = _velocityConstraints[i];
+
+                var indexA = velocityConstraint.IndexA;
+                var indexB = velocityConstraint.IndexB;
+                var invMassA = velocityConstraint.InvMassA;
+                var invIA = velocityConstraint.InvIA;
+                var invMassB = velocityConstraint.InvMassB;
+                var invIB = velocityConstraint.InvIB;
+                var pointCount = velocityConstraint.PointCount;
+
+                var linVelocityA = _linearVelocities[indexA];
+                var angVelocityA = _angularVelocities[indexA];
+                var linVelocityB = _linearVelocities[indexB];
+                var angVelocityB = _angularVelocities[indexB];
+
+                var normal = velocityConstraint.Normal;
+                var tangent = Vector2.Cross(normal, 1.0f);
+
+                for (var j = 0; j < pointCount; ++j)
+                {
+                    var constraintPoint = velocityConstraint.Points[j];
+                    var P = normal * constraintPoint.NormalImpulse + tangent * constraintPoint.TangentImpulse;
+                    angVelocityA -= invIA * Vector2.Cross(constraintPoint.RelativeVelocityA, P);
+                    linVelocityA -= P * invMassA;
+                    angVelocityB += invIB * Vector2.Cross(constraintPoint.RelativeVelocityB, P);
+                    linVelocityB += P * invMassB;
+                }
+
+                _linearVelocities[indexA] = linVelocityA;
+                _angularVelocities[indexA] = angVelocityA;
+                _linearVelocities[indexB] = linVelocityB;
+                _angularVelocities[indexB] = angVelocityB;
+            }
+        }
+
+        public void SolveVelocityConstraints()
+        {
+            // Here be dragons
+            for (var i = 0; i < _contactCount; ++i)
+            {
+                var velocityConstraint = _velocityConstraints[i];
+
+                var indexA = velocityConstraint.IndexA;
+                var indexB = velocityConstraint.IndexB;
+                var mA = velocityConstraint.InvMassA;
+                var iA = velocityConstraint.InvIA;
+                var mB = velocityConstraint.InvMassB;
+                var iB = velocityConstraint.InvIB;
+                var pointCount = velocityConstraint.PointCount;
+
+                var vA = _linearVelocities[indexA];
+                var wA = _angularVelocities[indexA];
+                var vB = _linearVelocities[indexB];
+                var wB = _angularVelocities[indexB];
+
+                var normal = velocityConstraint.Normal;
+                var tangent = Vector2.Cross(normal, 1.0f);
+                var friction = velocityConstraint.Friction;
+
+                DebugTools.Assert(pointCount == 1 || pointCount == 2);
+
+                // Solve tangent constraints first because non-penetration is more important
+                // than friction.
+                for (var j = 0; j < pointCount; ++j)
+                {
+                    VelocityConstraintPoint velConstraintPoint = velocityConstraint.Points[j];
+
+                    // Relative velocity at contact
+                    var dv = vB + Vector2.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2.Cross(wA, velConstraintPoint.RelativeVelocityA);
+
+                    // Compute tangent force
+                    float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed;
+                    float lambda = velConstraintPoint.TangentMass * (-vt);
+
+                    // b2Clamp the accumulated force
+                    var maxFriction = friction * velConstraintPoint.NormalImpulse;
+                    var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction);
+                    lambda = newImpulse - velConstraintPoint.TangentImpulse;
+                    velConstraintPoint.TangentImpulse = newImpulse;
+
+                    // Apply contact impulse
+                    Vector2 P = tangent * lambda;
+
+                    vA -= P * mA;
+                    wA -= iA * Vector2.Cross(velConstraintPoint.RelativeVelocityA, P);
+
+                    vB += P * mB;
+                    wB += iB * Vector2.Cross(velConstraintPoint.RelativeVelocityB, P);
+                }
+
+                // Solve normal constraints
+                if (velocityConstraint.PointCount == 1)
+                {
+                    VelocityConstraintPoint vcp = velocityConstraint.Points[0];
+
+                    // Relative velocity at contact
+                    Vector2 dv = vB + Vector2.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2.Cross(wA, vcp.RelativeVelocityA);
+
+                    // Compute normal impulse
+                    float vn = Vector2.Dot(dv, normal);
+                    float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias);
+
+                    // b2Clamp the accumulated impulse
+                    float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f);
+                    lambda = newImpulse - vcp.NormalImpulse;
+                    vcp.NormalImpulse = newImpulse;
+
+                    // Apply contact impulse
+                    Vector2 P = normal * lambda;
+                    vA -= P * mA;
+                    wA -= iA * Vector2.Cross(vcp.RelativeVelocityA, P);
+
+                    vB += P * mB;
+                    wB += iB * Vector2.Cross(vcp.RelativeVelocityB, P);
+                }
+                else
+                {
+                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
+                    // Build the mini LCP for this contact patch
+                    //
+                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+                    //
+                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
+                    // b = vn0 - velocityBias
+                    //
+                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
+                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
+                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
+                    // solution that satisfies the problem is chosen.
+                    //
+                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
+                    // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
+                    //
+                    // Substitute:
+                    //
+                    // x = a + d
+                    //
+                    // a := old total impulse
+                    // x := new total impulse
+                    // d := incremental impulse
+                    //
+                    // For the current iteration we extend the formula for the incremental impulse
+                    // to compute the new total impulse:
+                    //
+                    // vn = A * d + b
+                    //    = A * (x - a) + b
+                    //    = A * x + b - A * a
+                    //    = A * x + b'
+                    // b' = b - A * a;
+
+                    VelocityConstraintPoint cp1 = velocityConstraint.Points[0];
+                    VelocityConstraintPoint cp2 = velocityConstraint.Points[1];
+
+                    Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
+                    DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f);
+
+                    // Relative velocity at contact
+                    Vector2 dv1 = vB + Vector2.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2.Cross(wA, cp1.RelativeVelocityA);
+                    Vector2 dv2 = vB + Vector2.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2.Cross(wA, cp2.RelativeVelocityA);
+
+                    // Compute normal velocity
+                    float vn1 = Vector2.Dot(dv1, normal);
+                    float vn2 = Vector2.Dot(dv2, normal);
+
+                    Vector2 b = new Vector2
+                    {
+                        X = vn1 - cp1.VelocityBias,
+                        Y = vn2 - cp2.VelocityBias
+                    };
+
+                    // Compute b'
+                    b -= Transform.Mul(velocityConstraint.K, a);
+
+                    //const float k_errorTol = 1e-3f;
+                    //B2_NOT_USED(k_errorTol);
+
+                    for (; ; )
+                    {
+                        //
+                        // Case 1: vn = 0
+                        //
+                        // 0 = A * x + b'
+                        //
+                        // Solve for x:
+                        //
+                        // x = - inv(A) * b'
+                        //
+                        Vector2 x = -Transform.Mul(velocityConstraint.NormalMass, b);
+
+                        if (x.X >= 0.0f && x.Y >= 0.0f)
+                        {
+                            // Get the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = normal * d.X;
+                            Vector2 P2 = normal * d.Y;
+                            vA -= (P1 + P2) * mA;
+                            wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2));
+
+                            vB += (P1 + P2) * mB;
+                            wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2));
+
+                            // Accumulate
+                            cp1.NormalImpulse = x.X;
+                            cp2.NormalImpulse = x.Y;
+
+                            break;
+                        }
+
+                        //
+                        // Case 2: vn1 = 0 and x2 = 0
+                        //
+                        //   0 = a11 * x1 + a12 * 0 + b1'
+                        // vn2 = a21 * x1 + a22 * 0 + b2'
+                        //
+                        x.X = -cp1.NormalMass * b.X;
+                        x.Y = 0.0f;
+                        vn1 = 0.0f;
+                        vn2 = velocityConstraint.K[0].Y * x.X + b.Y;
+
+                        if (x.X >= 0.0f && vn2 >= 0.0f)
+                        {
+                            // Get the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = normal * d.X;
+                            Vector2 P2 = normal * d.Y;
+                            vA -= (P1 + P2) * mA;
+                            wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2));
+
+                            vB += (P1 + P2) * mB;
+                            wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2));
+
+                            // Accumulate
+                            cp1.NormalImpulse = x.X;
+                            cp2.NormalImpulse = x.Y;
+
+                            break;
+                        }
+
+
+                        //
+                        // Case 3: vn2 = 0 and x1 = 0
+                        //
+                        // vn1 = a11 * 0 + a12 * x2 + b1'
+                        //   0 = a21 * 0 + a22 * x2 + b2'
+                        //
+                        x.X = 0.0f;
+                        x.Y = -cp2.NormalMass * b.Y;
+                        vn1 = velocityConstraint.K[1].X * x.Y + b.X;
+                        vn2 = 0.0f;
+
+                        if (x.Y >= 0.0f && vn1 >= 0.0f)
+                        {
+                            // Resubstitute for the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = normal * d.X;
+                            Vector2 P2 = normal * d.Y;
+                            vA -= (P1 + P2) * mA;
+                            wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2));
+
+                            vB += (P1 + P2) * mB;
+                            wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2));
+
+                            // Accumulate
+                            cp1.NormalImpulse = x.X;
+                            cp2.NormalImpulse = x.Y;
+
+                            break;
+                        }
+
+                        //
+                        // Case 4: x1 = 0 and x2 = 0
+                        //
+                        // vn1 = b1
+                        // vn2 = b2;
+                        x.X = 0.0f;
+                        x.Y = 0.0f;
+                        vn1 = b.X;
+                        vn2 = b.Y;
+
+                        if (vn1 >= 0.0f && vn2 >= 0.0f)
+                        {
+                            // Resubstitute for the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = normal * d.X;
+                            Vector2 P2 = normal * d.Y;
+                            vA -= (P1 + P2) * mA;
+                            wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2));
+
+                            vB += (P1 + P2) * mB;
+                            wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2));
+
+                            // Accumulate
+                            cp1.NormalImpulse = x.X;
+                            cp2.NormalImpulse = x.Y;
+
+                            break;
+                        }
+
+                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+                        break;
+                    }
+                }
+
+                _linearVelocities[indexA] = vA;
+                _angularVelocities[indexA] = wA;
+                _linearVelocities[indexB] = vB;
+                _angularVelocities[indexB] = wB;
+            }
+        }
+
+        public void StoreImpulses()
+        {
+            for (int i = 0; i < _contactCount; ++i)
+            {
+                ContactVelocityConstraint velocityConstraint = _velocityConstraints[i];
+                Collision.Manifold manifold = _contacts[velocityConstraint.ContactIndex].Manifold;
+
+                for (int j = 0; j < velocityConstraint.PointCount; ++j)
+                {
+                    ManifoldPoint point = manifold.Points[j];
+                    point.NormalImpulse = velocityConstraint.Points[j].NormalImpulse;
+                    point.TangentImpulse = velocityConstraint.Points[j].TangentImpulse;
+                    manifold.Points[j] = point;
+                }
+
+                _contacts[velocityConstraint.ContactIndex].Manifold = manifold;
+            }
+        }
+
+        /// <summary>
+        ///     Tries to solve positions for all contacts specified.
+        /// </summary>
+        /// <returns>true if all positions solved</returns>
+        public bool SolvePositionConstraints()
+        {
+            float minSeparation = 0.0f;
+
+            for (int i = 0; i < _contactCount; ++i)
+            {
+                ContactPositionConstraint pc = _positionConstraints[i];
+
+                int indexA = pc.IndexA;
+                int indexB = pc.IndexB;
+                Vector2 localCenterA = pc.LocalCenterA;
+                float mA = pc.InvMassA;
+                float iA = pc.InvIA;
+                Vector2 localCenterB = pc.LocalCenterB;
+                float mB = pc.InvMassB;
+                float iB = pc.InvIB;
+                int pointCount = pc.PointCount;
+
+                Vector2 centerA = _positions[indexA];
+                float angleA = _angles[indexA];
+
+                Vector2 centerB = _positions[indexB];
+                float angleB = _angles[indexB];
+
+                // Solve normal constraints
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    Transform xfA = new Transform(angleA);
+                    Transform xfB = new Transform(angleB);
+                    xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA);
+                    xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB);
+
+                    Vector2 normal;
+                    Vector2 point;
+                    float separation;
+
+                    PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation);
+
+                    Vector2 rA = point - centerA;
+                    Vector2 rB = point - centerB;
+
+                    // Track max constraint error.
+                    minSeparation = Math.Min(minSeparation, separation);
+
+                    // Prevent large corrections and allow slop.
+                    float C = Math.Clamp(_baumgarte * (separation + _linearSlop), -_maxLinearCorrection, 0.0f);
+
+                    // Compute the effective mass.
+                    float rnA = Vector2.Cross(rA, normal);
+                    float rnB = Vector2.Cross(rB, normal);
+                    float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+                    // Compute normal impulse
+                    float impulse = K > 0.0f ? -C / K : 0.0f;
+
+                    Vector2 P = normal * impulse;
+
+                    centerA -= P * mA;
+                    angleA -= iA * Vector2.Cross(rA, P);
+
+                    centerB += P * mB;
+                    angleB += iB * Vector2.Cross(rB, P);
+                }
+
+                _positions[indexA] = centerA;
+                _angles[indexA] = angleA;
+
+                _positions[indexB] = centerB;
+                _angles[indexB] = angleB;
+            }
+
+            // We can't expect minSpeparation >= -b2_linearSlop because we don't
+            // push the separation above -b2_linearSlop.
+            return minSeparation >= -3.0f * _linearSlop;
+        }
+
+        /// <summary>
+        /// Evaluate the manifold with supplied transforms. This assumes
+        /// modest motion from the original state. This does not change the
+        /// point count, impulses, etc. The radii must come from the Shapes
+        /// that generated the manifold.
+        /// </summary>
+        internal static void InitializeManifold(
+            in Collision.Manifold manifold,
+            in Transform xfA,
+            in Transform xfB,
+            float radiusA,
+            float radiusB,
+            out Vector2 normal,
+            out Vector2[] points)
+        {
+            normal = Vector2.Zero;
+            points = new Vector2[2];
+
+            if (manifold.PointCount == 0)
+            {
+                return;
+            }
+
+            switch (manifold.Type)
+            {
+                case ManifoldType.Circles:
+                {
+                    normal = new Vector2(1.0f, 0.0f);
+                    Vector2 pointA = Transform.Mul(xfA, manifold.LocalPoint);
+                    Vector2 pointB = Transform.Mul(xfB, manifold.Points[0].LocalPoint);
+
+                    if ((pointA - pointB).LengthSquared > float.Epsilon * float.Epsilon)
+                    {
+                        normal = pointB - pointA;
+                        normal = normal.Normalized;
+                    }
+
+                    Vector2 cA = pointA + normal * radiusA;
+                    Vector2 cB = pointB - normal * radiusB;
+                    points[0] = (cA + cB) * 0.5f;
+                }
+                break;
+
+                case ManifoldType.FaceA:
+                {
+                    normal = Transform.Mul(xfA.Quaternion2D, manifold.LocalNormal);
+                    Vector2 planePoint = Transform.Mul(xfA, manifold.LocalPoint);
+
+                    for (int i = 0; i < manifold.PointCount; ++i)
+                    {
+                        Vector2 clipPoint = Transform.Mul(xfB, manifold.Points[i].LocalPoint);
+                        Vector2 cA = clipPoint + normal * (radiusA - Vector2.Dot(clipPoint - planePoint, normal));
+                        Vector2 cB = clipPoint - normal * radiusB;
+                        points[i] = (cA + cB) * 0.5f;
+                    }
+                }
+                break;
+
+                case ManifoldType.FaceB:
+                {
+                    normal = Transform.Mul(xfB.Quaternion2D, manifold.LocalNormal);
+                    Vector2 planePoint = Transform.Mul(xfB, manifold.LocalPoint);
+
+                    for (int i = 0; i < manifold.PointCount; ++i)
+                    {
+                        Vector2 clipPoint = Transform.Mul(xfA, manifold.Points[i].LocalPoint);
+                        Vector2 cB = clipPoint + normal * (radiusB - Vector2.Dot(clipPoint - planePoint, normal));
+                        Vector2 cA = clipPoint - normal * radiusA;
+                        points[i] = (cA + cB) * 0.5f;
+                    }
+
+                    // Ensure normal points from A to B.
+                    normal = -normal;
+                }
+                break;
+                default:
+                    // Shouldn't happentm
+                    throw new InvalidOperationException();
+
+            }
+        }
+
+        private static void PositionSolverManifoldInitialize(
+            in ContactPositionConstraint pc,
+            int index,
+            in Transform xfA,
+            in Transform xfB,
+            out Vector2 normal,
+            out Vector2 point,
+            out float separation)
+        {
+            DebugTools.Assert(pc.PointCount > 0);
+
+                switch (pc.Type)
+                {
+                    case ManifoldType.Circles:
+                        {
+                            Vector2 pointA = Transform.Mul(xfA, pc.LocalPoint);
+                            Vector2 pointB = Transform.Mul(xfB, pc.LocalPoints[0]);
+                            normal = pointB - pointA;
+
+                            //FPE: Fix to handle zero normalization
+                            if (normal != Vector2.Zero)
+                                normal = normal.Normalized;
+
+                            point = (pointA + pointB) * 0.5f;
+                            separation = Vector2.Dot(pointB - pointA, normal) - pc.RadiusA - pc.RadiusB;
+                        }
+                        break;
+
+                    case ManifoldType.FaceA:
+                        {
+                            normal = Transform.Mul(xfA.Quaternion2D, pc.LocalNormal);
+                            Vector2 planePoint = Transform.Mul(xfA, pc.LocalPoint);
+
+                            Vector2 clipPoint = Transform.Mul(xfB, pc.LocalPoints[index]);
+                            separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB;
+                            point = clipPoint;
+                        }
+                        break;
+
+                    case ManifoldType.FaceB:
+                        {
+                            normal = Transform.Mul(xfB.Quaternion2D, pc.LocalNormal);
+                            Vector2 planePoint = Transform.Mul(xfB, pc.LocalPoint);
+
+                            Vector2 clipPoint = Transform.Mul(xfA, pc.LocalPoints[index]);
+                            separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB;
+                            point = clipPoint;
+
+                            // Ensure normal points from A to B
+                            normal = -normal;
+                        }
+                        break;
+                    default:
+                        normal = Vector2.Zero;
+                        point = Vector2.Zero;
+                        separation = 0;
+                        break;
+
+                }
+        }
+    }
+}