"Fix" conveyors (#6301)

Content.Server/Physics/Controllers/ConveyorController.cs

@@ -1,19 +1,24 @@
 using System;
 using System.Collections.Generic;
 using Content.Server.Conveyor;
-using Content.Server.Recycling.Components;
+using Content.Shared.Conveyor;
 using Content.Shared.Movement.Components;
+using Robust.Shared.Containers;
 using Robust.Shared.GameObjects;
 using Robust.Shared.IoC;
+using Robust.Shared.Map;
 using Robust.Shared.Maths;
 using Robust.Shared.Physics;
 using Robust.Shared.Physics.Controllers;
 
 namespace Content.Server.Physics.Controllers
 {
-    internal sealed class ConveyorController : VirtualController
+    public sealed class ConveyorController : VirtualController
     {
+        [Dependency] private readonly IEntityLookup _lookup = default!;
+        [Dependency] private readonly IMapManager _mapManager = default!;
         [Dependency] private readonly ConveyorSystem _conveyor = default!;
+        [Dependency] private readonly SharedContainerSystem _container = default!;
 
         public override void Initialize()
         {
@@ -25,43 +30,110 @@ namespace Content.Server.Physics.Controllers
         public override void UpdateBeforeSolve(bool prediction, float frameTime)
         {
             base.UpdateBeforeSolve(prediction, frameTime);
-            foreach (var comp in EntityManager.EntityQuery<ConveyorComponent>())
+
+            var conveyed = new HashSet<EntityUid>();
+
+            // TODO: This won't work if someone wants a massive fuckoff conveyor so look at using StartCollide or something.
+            foreach (var (comp, xform) in EntityManager.EntityQuery<ConveyorComponent, TransformComponent>())
             {
-                Convey(_conveyor, comp, frameTime);
+                Convey(comp, xform, conveyed, frameTime);
             }
         }
 
-        private void Convey(ConveyorSystem system, ConveyorComponent comp, float frameTime)
+        private void Convey(ConveyorComponent comp, TransformComponent xform, HashSet<EntityUid> conveyed, float frameTime)
         {
             // Use an event for conveyors to know what needs to run
-            if (!system.CanRun(comp))
+            if (!_conveyor.CanRun(comp))
             {
                 return;
             }
 
-            var direction = system.GetAngle(comp).ToVec();
-            var entMan = IoCManager.Resolve<IEntityManager>();
-                         var ownerPos = entMan.GetComponent<TransformComponent>(comp.Owner).WorldPosition;
+            var speed = comp.Speed;
 
-            foreach (var (entity, physics) in EntitySystem.Get<ConveyorSystem>().GetEntitiesToMove(comp))
+            if (speed <= 0f) return;
+
+            var (conveyorPos, conveyorRot) = xform.GetWorldPositionRotation();
+
+            conveyorRot += comp.Angle;
+
+            if (comp.State == ConveyorState.Reversed)
             {
-                var itemRelativeToConveyor = entMan.GetComponent<TransformComponent>(entity).WorldPosition - ownerPos;
-                physics.LinearVelocity += Convey(direction, comp.Speed, frameTime, itemRelativeToConveyor);
+                conveyorRot += MathF.PI;
+            }
+
+            var direction = conveyorRot.ToWorldVec();
+
+            foreach (var (entity, transform) in GetEntitiesToMove(comp, xform))
+            {
+                if (!conveyed.Add(entity)) continue;
+
+                var worldPos = transform.WorldPosition;
+                var itemRelative = conveyorPos - worldPos;
+
+                worldPos += Convey(direction, speed, frameTime, itemRelative);
+                transform.WorldPosition = worldPos;
             }
         }
 
-        private Vector2 Convey(Vector2 direction, float speed, float frameTime, Vector2 itemRelativeToConveyor)
+        private static Vector2 Convey(Vector2 direction, float speed, float frameTime, Vector2 itemRelative)
         {
-            if(speed == 0 || direction.Length == 0) return Vector2.Zero;
+            if (speed == 0 || direction.Length == 0) return Vector2.Zero;
+
+            /* TODO: Figure out how to fix corner cuts.
             direction = direction.Normalized;
 
             var dirNormal = new Vector2(direction.Y, direction.X);
-            var dot = Vector2.Dot(itemRelativeToConveyor, dirNormal);
+            var dot = Vector2.Dot(itemRelative, dirNormal);
+            */
 
-            var velocity = direction * speed * 5;
+            var velocity = direction * speed;
+
+            return velocity * frameTime;
+
+            /*
             velocity += dirNormal * speed * -dot;
 
             return velocity * frameTime;
+            */
+        }
+
+        public IEnumerable<(EntityUid, TransformComponent)> GetEntitiesToMove(ConveyorComponent comp, TransformComponent xform)
+        {
+            if (!_mapManager.TryGetGrid(xform.GridID, out var grid) ||
+                !grid.TryGetTileRef(xform.Coordinates, out var tile)) yield break;
+
+            var tileAABB = _lookup.GetLocalBounds(tile, grid.TileSize).Enlarged(0.01f);
+            var gridMatrix = Transform(grid.GridEntityId).InvWorldMatrix;
+
+            foreach (var entity in _lookup.GetEntitiesIntersecting(tile))
+            {
+                if (entity == comp.Owner ||
+                    Deleted(entity) ||
+                    HasComp<IMapGridComponent>(entity)) continue;
+
+                if (!TryComp(entity, out PhysicsComponent? physics) ||
+                    physics.BodyType == BodyType.Static ||
+                    physics.BodyStatus == BodyStatus.InAir ||
+                    entity.IsWeightless(physics, entityManager: EntityManager))
+                {
+                    continue;
+                }
+
+                if (_container.IsEntityInContainer(entity))
+                {
+                    continue;
+                }
+
+                // Yes there's still going to be the occasional rounding issue where it stops getting conveyed
+                // When you fix the corner issue that will fix this anyway.
+                var transform = Transform(entity);
+                var gridPos = gridMatrix.Transform(transform.WorldPosition);
+                var gridAABB = new Box2(gridPos - 0.1f, gridPos + 0.1f);
+
+                if (!tileAABB.Intersects(gridAABB)) continue;
+
+                yield return (entity, transform);
+            }
         }
     }
 }