Refactor pathfinding updates and add AccessReader support (#1183)

There was some extra bloat in the path graph updates.
Now the queue should also just run if it gets too big regardless.
Un-anchored physics objects are no longer a hard fail for pathfinding.
Add AccessReader support so open / close doors show up for pathfinding
AI also ensure they call the operator's shutdown when they're shutdown so that should cancel the pathfinding job.

I tried to split these into 2 commits but they were kinda coupled together

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

Content.Server/AI/Operators/Movement/BaseMover.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Threading;
+using Content.Server.GameObjects.Components.Access;
 using Content.Server.GameObjects.Components.Movement;
 using Content.Server.GameObjects.EntitySystems;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding;
@@ -240,10 +241,11 @@ namespace Content.Server.AI.Operators.Movement
 
             var startGrid = _mapManager.GetGrid(Owner.Transform.GridID).GetTileRef(Owner.Transform.GridPosition);
             var endGrid = _mapManager.GetGrid(TargetGrid.GridID).GetTileRef(TargetGrid);;
-            // _routeCancelToken = new CancellationTokenSource();
+            var access = AccessReader.FindAccessTags(Owner);
 
             RouteJob = _pathfinder.RequestPath(new PathfindingArgs(
                 Owner.Uid,
+                access,
                 collisionMask,
                 startGrid,
                 endGrid,

Content.Server/AI/Utility/Actions/Combat/Melee/MeleeAttackEntity.cs

@@ -29,11 +29,17 @@ namespace Content.Server.AI.Utility.Actions.Combat.Melee
 
         public override void SetupOperators(Blackboard context)
         {
-            var moveOperator = new MoveToEntityOperator(Owner, _entity);
             var equipped = context.GetState<EquippedEntityState>().GetValue();
+            MoveToEntityOperator moveOperator;
             if (equipped != null && equipped.TryGetComponent(out MeleeWeaponComponent meleeWeaponComponent))
             {
-                moveOperator.DesiredRange = meleeWeaponComponent.Range - 0.01f;
+                moveOperator = new MoveToEntityOperator(Owner, _entity, meleeWeaponComponent.Range - 0.01f);
+            }
+            // I think it's possible for this to happen given planning is time-sliced?
+            // TODO: At this point we should abort
+            else
+            {
+                moveOperator = new MoveToEntityOperator(Owner, _entity);
             }
 
             ActionOperators = new Queue<AiOperator>(new AiOperator[]

Content.Server/AI/Utility/AiLogic/UtilityAI.cs

@@ -126,6 +126,9 @@ namespace Content.Server.AI.Utility.AiLogic
             {
                 damageableComponent.DamageThresholdPassed -= DeathHandle;
             }
+
+            var currentOp = CurrentAction?.ActionOperators.Peek();
+            currentOp?.Shutdown(Outcome.Failed);
         }
 
         private void DeathHandle(object sender, DamageThresholdPassedEventArgs eventArgs)

Content.Server/GameObjects/Components/Access/AccessReaderChangeMessage.cs

@@ -0,0 +1,16 @@
+using Robust.Shared.GameObjects;
+
+namespace Content.Server.GameObjects.Components.Access
+{
+    public sealed class AccessReaderChangeMessage : EntitySystemMessage
+    {
+        public EntityUid Uid { get; }
+        public bool Enabled { get; }
+
+        public AccessReaderChangeMessage(EntityUid uid, bool enabled)
+        {
+            Uid = uid;
+            Enabled = enabled;
+        }
+    }
+}

Content.Server/GameObjects/Components/Access/AccessReaderComponent.cs

@@ -69,7 +69,7 @@ namespace Content.Server.GameObjects.Components.Access
         }
 
         [CanBeNull]
-        private static ICollection<string> FindAccessTags(IEntity entity)
+        public static ICollection<string> FindAccessTags(IEntity entity)
         {
             if (entity.TryGetComponent(out IAccess accessComponent))
             {

Content.Server/GameObjects/Components/Doors/ServerDoorComponent.cs

@@ -154,6 +154,8 @@ namespace Content.Server.GameObjects
                 State = DoorState.Open;
                 SetAppearance(DoorVisualState.Open);
             }, _cancellationTokenSource.Token);
+            
+            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new AccessReaderChangeMessage(Owner.Uid, false));
         }
 
         public virtual bool CanClose()
@@ -203,6 +205,7 @@ namespace Content.Server.GameObjects
                     occluder.Enabled = true;
                 }
             }, _cancellationTokenSource.Token);
+            Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new AccessReaderChangeMessage(Owner.Uid, true));
             return true;
         }
 

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/GraphUpdates/CollidableMove.cs

@@ -1,14 +0,0 @@
-using Robust.Shared.GameObjects.Components.Transform;
-
-namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates
-{
-    public struct CollidableMove : IPathfindingGraphUpdate
-    {
-        public MoveEvent MoveEvent { get; }
-
-        public CollidableMove(MoveEvent moveEvent)
-        {
-            MoveEvent = moveEvent;
-        }
-    }
-}

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/GraphUpdates/CollisionChange.cs

@@ -1,16 +0,0 @@
-using Robust.Shared.Interfaces.GameObjects;
-
-namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates
-{
-    public class CollisionChange : IPathfindingGraphUpdate
-    {
-        public IEntity Owner { get; }
-        public bool Value { get; }
-
-        public CollisionChange(IEntity owner, bool value)
-        {
-            Owner = owner;
-            Value = value;
-        }
-    }
-}

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/GraphUpdates/GridRemoval.cs

@@ -1,14 +0,0 @@
-using Robust.Shared.Map;
-
-namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates
-{
-    public struct GridRemoval : IPathfindingGraphUpdate
-    {
-        public GridId GridId { get; }
-
-        public GridRemoval(GridId gridId)
-        {
-            GridId = gridId;
-        }
-    }
-}

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/GraphUpdates/IPathfindingGraphUpdate.cs

@@ -1,7 +0,0 @@
-namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates
-{
-    public interface IPathfindingGraphUpdate
-    {
-
-    }
-}

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/GraphUpdates/TileUpdate.cs

@@ -1,14 +0,0 @@
-using Robust.Shared.Map;
-
-namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates
-{
-    public struct TileUpdate : IPathfindingGraphUpdate
-    {
-        public TileUpdate(TileRef tile)
-        {
-            Tile = tile;
-        }
-
-        public TileRef Tile { get; }
-    }
-}

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs

@@ -41,7 +41,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
             }
 
             // If we couldn't get a nearby node that's good enough
-            if (!Utils.TryEndNode(ref _endNode, _pathfindingArgs))
+            if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
             {
                 return null;
             }
@@ -88,9 +88,9 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                     }
 
                     // If tile is untraversable it'll be null
-                    var tileCost = Utils.GetTileCost(_pathfindingArgs, currentNode, nextNode);
+                    var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
 
-                    if (tileCost == null || !Utils.DirectionTraversable(_pathfindingArgs.CollisionMask, currentNode, direction))
+                    if (tileCost == null || !PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
                     {
                         continue;
                     }
@@ -107,7 +107,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                     // pFactor is tie-breaker where the fscore is otherwise equal.
                     // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
                     // There's other ways to do it but future consideration
-                    var fScore = gScores[nextNode] + Utils.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
+                    var fScore = gScores[nextNode] + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
                     openTiles.Add((fScore, nextNode));
                 }
             }
@@ -117,7 +117,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                 return null;
             }
 
-            var route = Utils.ReconstructPath(cameFrom, currentNode);
+            var route = PathfindingHelpers.ReconstructPath(cameFrom, currentNode);
 
             if (route.Count == 1)
             {

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/JpsPathfindingJob.cs

@@ -41,7 +41,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
             }
 
             // If we couldn't get a nearby node that's good enough
-            if (!Utils.TryEndNode(ref _endNode, _pathfindingArgs))
+            if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
             {
                 return null;
             }
@@ -89,7 +89,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                         jumpNodes.Add(jumpNode);
 #endif
                         // GetJumpPoint should already check if we can traverse to the node
-                        var tileCost = Utils.GetTileCost(_pathfindingArgs, currentNode, jumpNode);
+                        var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, jumpNode);
 
                         if (tileCost == null)
                         {
@@ -108,7 +108,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                         // pFactor is tie-breaker where the fscore is otherwise equal.
                         // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
                         // There's other ways to do it but future consideration
-                        var fScore = gScores[jumpNode] + Utils.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
+                        var fScore = gScores[jumpNode] + PathfindingHelpers.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
                         openTiles.Add((fScore, jumpNode));
                     }
                 }
@@ -119,7 +119,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                 return null;
             }
 
-            var route = Utils.ReconstructJumpPath(cameFrom, currentNode);
+            var route = PathfindingHelpers.ReconstructJumpPath(cameFrom, currentNode);
             if (route.Count == 1)
             {
                 return null;
@@ -161,7 +161,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                 // We'll do opposite DirectionTraversable just because of how the method's setup
                 // Nodes should be 2-way anyway.
                 if (nextNode == null ||
-                    Utils.GetTileCost(_pathfindingArgs, currentNode, nextNode) == null)
+                    PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode) == null)
                 {
                     return null;
                 }
@@ -312,14 +312,14 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                     throw new ArgumentOutOfRangeException();
             }
 
-            if ((closedNeighborOne == null || Utils.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null)
+            if ((closedNeighborOne == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null)
-                && openNeighborOne != null && Utils.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null)
+                && openNeighborOne != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null)
             {
                 return true;
             }
 
-            if ((closedNeighborTwo == null || Utils.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null)
+            if ((closedNeighborTwo == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null)
-                && openNeighborTwo != null && Utils.GetTileCost(_pathfindingArgs, currentNode, openNeighborTwo) != null)
+                && openNeighborTwo != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborTwo) != null)
             {
                 return true;
             }
@@ -371,14 +371,14 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
                     throw new ArgumentOutOfRangeException();
             }
 
-            if ((closedNeighborOne == null || !Utils.Traversable(_pathfindingArgs.CollisionMask, closedNeighborOne.CollisionMask)) &&
+            if ((closedNeighborOne == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborOne)) &&
-                (openNeighborOne != null && Utils.Traversable(_pathfindingArgs.CollisionMask, openNeighborOne.CollisionMask)))
+                (openNeighborOne != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborOne)))
             {
                 return true;
             }
 
-            if ((closedNeighborTwo == null || !Utils.Traversable(_pathfindingArgs.CollisionMask, closedNeighborTwo.CollisionMask)) &&
+            if ((closedNeighborTwo == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborTwo)) &&
-                (openNeighborTwo != null && Utils.Traversable(_pathfindingArgs.CollisionMask, openNeighborTwo.CollisionMask)))
+                (openNeighborTwo != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborTwo)))
             {
                 return true;
             }

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/PathfindingArgs.cs

@@ -1,3 +1,4 @@
+using System.Collections.Generic;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Map;
 
@@ -6,6 +7,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
     public struct PathfindingArgs
     {
         public EntityUid Uid { get; }
+        public ICollection<string> Access { get; }
         public int CollisionMask { get; }
         public TileRef Start { get; }
         public TileRef End { get; }
@@ -20,6 +22,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
 
         public PathfindingArgs(
             EntityUid entityUid,
+            ICollection<string> access,
             int collisionMask,
             TileRef start,
             TileRef end,
@@ -29,6 +32,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
             bool allowSpace = false)
         {
             Uid = entityUid;
+            Access = access;
             CollisionMask = collisionMask;
             Start = start;
             End = end;

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using Content.Server.GameObjects.Components.Access;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders;
 using Content.Server.GameObjects.EntitySystems.Pathfinding;
 using Robust.Shared.Interfaces.GameObjects;
@@ -10,19 +11,19 @@ using Robust.Shared.Maths;
 
 namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 {
-    public static class Utils
+    public static class PathfindingHelpers
     {
         public static bool TryEndNode(ref PathfindingNode endNode, PathfindingArgs pathfindingArgs)
         {
-            if (!Traversable(pathfindingArgs.CollisionMask, endNode.CollisionMask))
+            if (!Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, endNode))
             {
                 if (pathfindingArgs.Proximity > 0.0f)
                 {
                     // TODO: Should make this account for proximities,
                     // probably some kind of breadth-first search to find a valid one
-                    foreach (var (direction, node) in endNode.Neighbors)
+                    foreach (var (_, node) in endNode.Neighbors)
                     {
-                        if (Traversable(pathfindingArgs.CollisionMask, node.CollisionMask))
+                        if (Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, node))
                         {
                             endNode = node;
                             return true;
@@ -36,7 +37,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             return true;
         }
 
-        public static bool DirectionTraversable(int collisionMask, PathfindingNode currentNode, Direction direction)
+        public static bool DirectionTraversable(int collisionMask, ICollection<string> access, PathfindingNode currentNode, Direction direction)
          {
             // If it's a diagonal we need to check NSEW to see if we can get to it and stop corner cutting, NE needs N and E etc.
             // Given there's different collision layers stored for each node in the graph it's probably not worth it to cache this
@@ -51,32 +52,32 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             {
                 case Direction.NorthEast:
                     if (northNeighbor == null || eastNeighbor == null) return false;
-                    if (!Traversable(collisionMask, northNeighbor.CollisionMask) ||
+                    if (!Traversable(collisionMask, access, northNeighbor) ||
-                        !Traversable(collisionMask, eastNeighbor.CollisionMask))
+                        !Traversable(collisionMask, access, eastNeighbor))
                     {
                         return false;
                     }
                     break;
                 case Direction.NorthWest:
                     if (northNeighbor == null || westNeighbor == null) return false;
-                    if (!Traversable(collisionMask, northNeighbor.CollisionMask) ||
+                    if (!Traversable(collisionMask, access, northNeighbor) ||
-                        !Traversable(collisionMask, westNeighbor.CollisionMask))
+                        !Traversable(collisionMask, access, westNeighbor))
                     {
                         return false;
                     }
                     break;
                 case Direction.SouthWest:
                     if (southNeighbor == null || westNeighbor == null) return false;
-                    if (!Traversable(collisionMask, southNeighbor.CollisionMask) ||
+                    if (!Traversable(collisionMask, access, southNeighbor) ||
-                        !Traversable(collisionMask, westNeighbor.CollisionMask))
+                        !Traversable(collisionMask, access, westNeighbor))
                     {
                         return false;
                     }
                     break;
                 case Direction.SouthEast:
                     if (southNeighbor == null || eastNeighbor == null) return false;
-                    if (!Traversable(collisionMask, southNeighbor.CollisionMask) ||
+                    if (!Traversable(collisionMask, access, southNeighbor) ||
-                        !Traversable(collisionMask, eastNeighbor.CollisionMask))
+                        !Traversable(collisionMask, access, eastNeighbor))
                     {
                         return false;
                     }
@@ -86,11 +87,24 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             return true;
         }
 
-        public static bool Traversable(int collisionMask, int nodeMask)
+        public static bool Traversable(int collisionMask, ICollection<string> access, PathfindingNode node)
         {
-            return (collisionMask & nodeMask) == 0;
+            if ((collisionMask & node.BlockedCollisionMask) != 0)
-        }
+            {
+                return false;
+            }
 
+            foreach (var reader in node.AccessReaders)
+            {
+                if (!reader.IsAllowed(access))
+                {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+        
         public static Queue<TileRef> ReconstructPath(Dictionary<PathfindingNode, PathfindingNode> cameFrom, PathfindingNode current)
         {
             var running = new Stack<TileRef>();
@@ -194,6 +208,20 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 
             return 1.4f * dstX + (dstY - dstX);
         }
+        
+        public static float OctileDistance(TileRef endTile, TileRef startTile)
+        {
+            // "Fast Euclidean" / octile.
+            // This implementation is written down in a few sources; it just saves doing sqrt.
+            int dstX = Math.Abs(startTile.X - endTile.X);
+            int dstY = Math.Abs(startTile.Y - endTile.Y);
+            if (dstX > dstY)
+            {
+                return 1.4f * dstY + (dstX - dstY);
+            }
+
+            return 1.4f * dstX + (dstY - dstX);
+        }
 
         public static float ManhattanDistance(PathfindingNode endNode, PathfindingNode currentNode)
         {
@@ -202,7 +230,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 
         public static float? GetTileCost(PathfindingArgs pathfindingArgs, PathfindingNode start, PathfindingNode end)
         {
-            if (!pathfindingArgs.NoClip && !Traversable(pathfindingArgs.CollisionMask, end.CollisionMask))
+            if (!pathfindingArgs.NoClip && !Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, end))
             {
                 return null;
             }

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs

@@ -1,6 +1,12 @@
 using System;
 using System.Collections.Generic;
+using Content.Server.GameObjects.Components.Access;
+using Content.Server.GameObjects.Components.Doors;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding;
+using Robust.Server.GameObjects;
+using Robust.Shared.GameObjects;
+using Robust.Shared.GameObjects.Components;
+using Robust.Shared.Interfaces.GameObjects;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
 
@@ -8,27 +14,34 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding
 {
     public class PathfindingNode
     {
-        // TODO: Add access ID here
         public PathfindingChunk ParentChunk => _parentChunk;
         private readonly PathfindingChunk _parentChunk;
-        public TileRef TileRef { get; private set; }
+        
-        public List<int> CollisionLayers { get; }
-        public int CollisionMask { get; private set; }
         public Dictionary<Direction, PathfindingNode> Neighbors => _neighbors;
         private Dictionary<Direction, PathfindingNode> _neighbors = new Dictionary<Direction, PathfindingNode>();
+        
+        public TileRef TileRef { get; private set; }
+        
+        /// <summary>
+        /// Whenever there's a change in the collision layers we update the mask as the graph has more reads than writes
+        /// </summary>
+        public int BlockedCollisionMask { get; private set; }
+        private readonly Dictionary<EntityUid, int> _blockedCollidables = new Dictionary<EntityUid, int>(0);
 
-        public PathfindingNode(PathfindingChunk parent, TileRef tileRef, List<int> collisionLayers = null)
+        public IReadOnlyCollection<EntityUid> PhysicsUids => _physicsUids;
+        private readonly HashSet<EntityUid> _physicsUids = new HashSet<EntityUid>(0);
+
+        /// <summary>
+        /// The entities on this tile that require access to traverse
+        /// </summary>
+        /// We don't store the ICollection, at least for now, as we'd need to replicate the access code here
+        public IReadOnlyCollection<AccessReader> AccessReaders => _accessReaders.Values;
+        private readonly Dictionary<EntityUid, AccessReader> _accessReaders = new Dictionary<EntityUid, AccessReader>(0);
+
+        public PathfindingNode(PathfindingChunk parent, TileRef tileRef)
         {
             _parentChunk = parent;
             TileRef = tileRef;
-            if (collisionLayers == null)
-            {
-                CollisionLayers = new List<int>();
-            }
-            else
-            {
-                CollisionLayers = collisionLayers;
-            }
             GenerateMask();
         }
 
@@ -105,25 +118,70 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding
             TileRef = newTile;
         }
 
-        public void AddCollisionLayer(int layer)
+        /// <summary>
+        /// Call if this entity is relevant for the pathfinder
+        /// </summary>
+        /// <param name="entity"></param>
+        /// TODO: These 2 methods currently don't account for a bunch of changes (e.g. airlock unpowered, wrenching, etc.)
+        public void AddEntity(IEntity entity)
         {
-            CollisionLayers.Add(layer);
+            // If we're a door
-            GenerateMask();
+            if (entity.HasComponent<AirlockComponent>() || entity.HasComponent<ServerDoorComponent>())
+            {
+                // If we need access to traverse this then add to readers, otherwise no point adding it (except for maybe tile costs in future)
+                // TODO: Check for powered I think (also need an event for when it's depowered
+                // AccessReader calls this whenever opening / closing but it can seem to get called multiple times
+                // Which may or may not be intended?
+                if (entity.TryGetComponent(out AccessReader accessReader) && !_accessReaders.ContainsKey(entity.Uid))
+                {
+                    _accessReaders.Add(entity.Uid, accessReader);
+                }
+                return;
+            }
+            
+            if (entity.TryGetComponent(out CollidableComponent collidableComponent))
+            {
+                if (entity.TryGetComponent(out PhysicsComponent physicsComponent) && !physicsComponent.Anchored)
+                {
+                    _physicsUids.Add(entity.Uid);
+                }
+                else
+                {
+                    _blockedCollidables.TryAdd(entity.Uid, collidableComponent.CollisionLayer);
+                    GenerateMask();
+                }
+            }
         }
 
-        public void RemoveCollisionLayer(int layer)
+        public void RemoveEntity(IEntity entity)
         {
-            CollisionLayers.Remove(layer);
+            if (_accessReaders.ContainsKey(entity.Uid))
-            GenerateMask();
+            {
+                _accessReaders.Remove(entity.Uid);
+                return;
+            }
+
+            if (entity.HasComponent<CollidableComponent>())
+            {
+                if (entity.TryGetComponent(out PhysicsComponent physicsComponent) && physicsComponent.Anchored)
+                {
+                    _blockedCollidables.Remove(entity.Uid);
+                    GenerateMask();
+                }
+                else
+                {
+                    _physicsUids.Remove(entity.Uid);
+                }
+            }
         }
 
         private void GenerateMask()
         {
-            CollisionMask = 0x0;
+            BlockedCollisionMask = 0x0;
 
-            foreach (var layer in CollisionLayers)
+            foreach (var layer in _blockedCollidables.Values)
             {
-                CollisionMask |= layer;
+                BlockedCollisionMask |= layer;
             }
         }
     }

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingSystem.cs

@@ -1,12 +1,12 @@
 using System;
 using System.Collections.Generic;
 using System.Threading;
-using Content.Server.GameObjects.Components.Doors;
+using Content.Server.GameObjects.Components.Access;
-using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.GraphUpdates;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders;
 using Content.Server.GameObjects.EntitySystems.JobQueues;
 using Content.Server.GameObjects.EntitySystems.JobQueues.Queues;
 using Content.Server.GameObjects.EntitySystems.Pathfinding;
+using Content.Shared.Physics;
 using Robust.Shared.GameObjects.Components;
 using Robust.Shared.GameObjects.Components.Transform;
 using Robust.Shared.GameObjects.Systems;
@@ -14,6 +14,7 @@ using Robust.Shared.Interfaces.GameObjects;
 using Robust.Shared.Interfaces.Map;
 using Robust.Shared.IoC;
 using Robust.Shared.Map;
+using Robust.Shared.Utility;
 
 namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 {
@@ -29,18 +30,30 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
     public class PathfindingSystem : EntitySystem
     {
 #pragma warning disable 649
+        [Dependency] private readonly IEntityManager _entitymanager;
         [Dependency] private readonly IMapManager _mapManager;
 #pragma warning restore 649
 
         public IReadOnlyDictionary<GridId, Dictionary<MapIndices, PathfindingChunk>> Graph => _graph;
         private readonly Dictionary<GridId, Dictionary<MapIndices, PathfindingChunk>> _graph = new Dictionary<GridId, Dictionary<MapIndices, PathfindingChunk>>();
-        // Every tick we queue up all the changes and do them at once
+        
-        private readonly Queue<IPathfindingGraphUpdate> _queuedGraphUpdates = new Queue<IPathfindingGraphUpdate>();
         private readonly PathfindingJobQueue _pathfindingQueue = new PathfindingJobQueue();
+        
+        // Queued pathfinding graph updates
+        private readonly Queue<CollisionChangeEvent> _collidableUpdateQueue = new Queue<CollisionChangeEvent>();
+        private readonly Queue<MoveEvent> _moveUpdateQueue = new Queue<MoveEvent>();
+        private readonly Queue<AccessReaderChangeMessage> _accessReaderUpdateQueue = new Queue<AccessReaderChangeMessage>();
+        private readonly Queue<TileRef> _tileUpdateQueue = new Queue<TileRef>();
 
         // Need to store previously known entity positions for collidables for when they move
         private readonly Dictionary<IEntity, TileRef> _lastKnownPositions = new Dictionary<IEntity, TileRef>();
 
+        public const int TrackedCollisionLayers = (int)
+            (CollisionGroup.Impassable | 
+             CollisionGroup.MobImpassable |
+             CollisionGroup.SmallImpassable | 
+             CollisionGroup.VaultImpassable);
+        
         /// <summary>
         /// Ask for the pathfinder to gimme somethin
         /// </summary>
@@ -68,51 +81,66 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 
         private void ProcessGraphUpdates()
         {
-            for (var i = 0; i < Math.Min(50, _queuedGraphUpdates.Count); i++)
+            var totalUpdates = 0;
+            
+            foreach (var update in _collidableUpdateQueue)
             {
-                var update = _queuedGraphUpdates.Dequeue();
+                var entity = _entitymanager.GetEntity(update.Owner);
-                switch (update)
+                if (update.CanCollide)
                 {
-                    case CollidableMove move:
+                    HandleCollidableAdd(entity);
-                        HandleCollidableMove(move);
+                }
-                        break;
+                else
-                    case CollisionChange change:
+                {
-                        if (change.Value)
+                    HandleAccessRemove(entity);
-                        {
-                            HandleCollidableAdd(change.Owner);
-                        }
-                        else
-                        {
-                            HandleCollidableRemove(change.Owner);
-                        }
-
-                        break;
-                    case GridRemoval removal:
-                        HandleGridRemoval(removal);
-                        break;
-                    case TileUpdate tile:
-                        HandleTileUpdate(tile);
-                        break;
-                    default:
-                        throw new ArgumentOutOfRangeException();
                 }
-            }
-        }
 
-        private void HandleGridRemoval(GridRemoval removal)
+                totalUpdates++;
-        {
+            }
-            if (!_graph.ContainsKey(removal.GridId))
+            
+            _collidableUpdateQueue.Clear();
+
+            foreach (var update in _accessReaderUpdateQueue)
             {
-                throw new InvalidOperationException();
+                var entity = _entitymanager.GetEntity(update.Uid);
+                if (update.Enabled)
+                {
+                    HandleAccessAdd(entity);
+                }
+                else
+                {
+                    HandleAccessRemove(entity);
+                }
+
+                totalUpdates++;
+            }
+            
+            _accessReaderUpdateQueue.Clear();
+
+            foreach (var tile in _tileUpdateQueue)
+            {
+                HandleTileUpdate(tile);
+                totalUpdates++;
+            }
+            
+            _tileUpdateQueue.Clear();
+            var moveUpdateCount = Math.Max(50 - totalUpdates, 0);
+            
+            // Other updates are high priority so for this we'll just defer it if there's a spike (explosion, etc.)
+            // If the move updates grow too large then we'll just do it
+            if (_moveUpdateQueue.Count > 100)
+            {
+                moveUpdateCount = _moveUpdateQueue.Count - 100;
             }
 
-            _graph.Remove(removal.GridId);
+            moveUpdateCount = Math.Min(moveUpdateCount, _moveUpdateQueue.Count);
-        }
+            
-
+            for (var i = 0; i < moveUpdateCount; i++)
-        private void HandleTileUpdate(TileUpdate tile)
+            {
-        {
+                HandleCollidableMove(_moveUpdateQueue.Dequeue());
-            var chunk = GetChunk(tile.Tile);
+            }
-            chunk.UpdateNode(tile.Tile);
+            
+            DebugTools.Assert(_moveUpdateQueue.Count < 1000);
         }
 
         public PathfindingChunk GetChunk(TileRef tile)
@@ -132,7 +160,6 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             }
 
             var newChunk = CreateChunk(tile.GridIndex, mapIndices);
-
             return newChunk;
         }
 
@@ -179,13 +206,13 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 
         public override void Initialize()
         {
-            IoCManager.InjectDependencies(this);
             SubscribeLocalEvent<CollisionChangeEvent>(QueueCollisionEnabledEvent);
             SubscribeLocalEvent<MoveEvent>(QueueCollidableMove);
+            SubscribeLocalEvent<AccessReaderChangeMessage>(QueueAccessChangeEvent);
 
             // Handle all the base grid changes
             // Anything that affects traversal (i.e. collision layer) is handled separately.
-            _mapManager.OnGridRemoved += QueueGridRemoval;
+            _mapManager.OnGridRemoved += HandleGridRemoval;
             _mapManager.GridChanged += QueueGridChange;
             _mapManager.TileChanged += QueueTileChange;
         }
@@ -193,32 +220,85 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
         public override void Shutdown()
         {
             base.Shutdown();
-            _mapManager.OnGridRemoved -= QueueGridRemoval;
+            UnsubscribeLocalEvent<CollisionChangeEvent>();
+            UnsubscribeLocalEvent<MoveEvent>();
+            UnsubscribeLocalEvent<AccessReaderChangeMessage>();
+            
+            _mapManager.OnGridRemoved -= HandleGridRemoval;
             _mapManager.GridChanged -= QueueGridChange;
             _mapManager.TileChanged -= QueueTileChange;
         }
+        
+        private void HandleTileUpdate(TileRef tile)
+        {
+            var node = GetNode(tile);
+            node.UpdateTile(tile);
+        }
 
         public void ResettingCleanup()
         {
-            _queuedGraphUpdates.Clear();
+            _graph.Clear();
+            _collidableUpdateQueue.Clear();
+            _moveUpdateQueue.Clear();
+            _accessReaderUpdateQueue.Clear();
+            _tileUpdateQueue.Clear();
+            _lastKnownPositions.Clear();
         }
 
-        private void QueueGridRemoval(GridId gridId)
+        private void HandleGridRemoval(GridId gridId)
         {
-            _queuedGraphUpdates.Enqueue(new GridRemoval(gridId));
+            if (_graph.ContainsKey(gridId))
+            {
+                _graph.Remove(gridId);
+            }
         }
 
         private void QueueGridChange(object sender, GridChangedEventArgs eventArgs)
         {
             foreach (var (position, _) in eventArgs.Modified)
             {
-                _queuedGraphUpdates.Enqueue(new TileUpdate(eventArgs.Grid.GetTileRef(position)));
+                _tileUpdateQueue.Enqueue(eventArgs.Grid.GetTileRef(position));
             }
         }
 
         private void QueueTileChange(object sender, TileChangedEventArgs eventArgs)
         {
-            _queuedGraphUpdates.Enqueue(new TileUpdate(eventArgs.NewTile));
+            _tileUpdateQueue.Enqueue(eventArgs.NewTile);
+        }
+
+        private void QueueAccessChangeEvent(AccessReaderChangeMessage message)
+        {
+            _accessReaderUpdateQueue.Enqueue(message);
+        }
+
+        private void HandleAccessAdd(IEntity entity)
+        {
+            if (entity.Deleted || !entity.HasComponent<AccessReader>())
+            {
+                return;
+            }
+            
+            var grid = _mapManager.GetGrid(entity.Transform.GridID);
+            var tileRef = grid.GetTileRef(entity.Transform.GridPosition);
+
+            var chunk = GetChunk(tileRef);
+            var node = chunk.GetNode(tileRef);
+            node.AddEntity(entity);
+        }
+
+        private void HandleAccessRemove(IEntity entity)
+        {
+            if (entity.Deleted || !entity.HasComponent<AccessReader>())
+            {
+                return;
+            }
+            
+            var grid = _mapManager.GetGrid(entity.Transform.GridID);
+            var tileRef = grid.GetTileRef(entity.Transform.GridPosition);
+
+            var chunk = GetChunk(tileRef);
+            var node = chunk.GetNode(tileRef);
+            node.RemoveEntity(entity);
         }
 
         #region collidable
@@ -228,25 +308,22 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
         /// <param name="entity"></param>
         private void HandleCollidableAdd(IEntity entity)
         {
-            // It's a grid / gone / a door / we already have it (which probably shouldn't happen)
             if (entity.Prototype == null ||
                 entity.Deleted ||
-                entity.HasComponent<ServerDoorComponent>() ||
+                _lastKnownPositions.ContainsKey(entity) || 
-                entity.HasComponent<AirlockComponent>() ||
+                !entity.TryGetComponent(out CollidableComponent collidableComponent) || 
-                _lastKnownPositions.ContainsKey(entity))
+                !collidableComponent.CanCollide || 
+                (TrackedCollisionLayers & collidableComponent.CollisionLayer) == 0)
             {
                 return;
             }
 
             var grid = _mapManager.GetGrid(entity.Transform.GridID);
             var tileRef = grid.GetTileRef(entity.Transform.GridPosition);
-
-            var collisionLayer = entity.GetComponent<CollidableComponent>().CollisionLayer;
-
             var chunk = GetChunk(tileRef);
             var node = chunk.GetNode(tileRef);
-            node.AddCollisionLayer(collisionLayer);
 
+            node.AddEntity(entity);
             _lastKnownPositions.Add(entity, tileRef);
         }
 
@@ -258,46 +335,37 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
         {
             if (entity.Prototype == null ||
                 entity.Deleted ||
-                entity.HasComponent<ServerDoorComponent>() ||
+                !_lastKnownPositions.ContainsKey(entity) || 
-                entity.HasComponent<AirlockComponent>() ||
+                !entity.TryGetComponent(out CollidableComponent collidableComponent) || 
-                !_lastKnownPositions.ContainsKey(entity))
+                !collidableComponent.CanCollide || 
+                (TrackedCollisionLayers & collidableComponent.CollisionLayer) == 0)
             {
                 return;
             }
 
-            _lastKnownPositions.Remove(entity);
-
             var grid = _mapManager.GetGrid(entity.Transform.GridID);
             var tileRef = grid.GetTileRef(entity.Transform.GridPosition);
-
-            if (!entity.TryGetComponent(out CollidableComponent collidableComponent))
-            {
-                return;
-            }
-
-            var collisionLayer = collidableComponent.CollisionLayer;
-
             var chunk = GetChunk(tileRef);
             var node = chunk.GetNode(tileRef);
-            node.RemoveCollisionLayer(collisionLayer);
+
+            node.RemoveEntity(entity);
+            _lastKnownPositions.Remove(entity);
         }
 
         private void QueueCollidableMove(MoveEvent moveEvent)
         {
-            _queuedGraphUpdates.Enqueue(new CollidableMove(moveEvent));
+            _moveUpdateQueue.Enqueue(moveEvent);
         }
 
-        private void HandleCollidableMove(CollidableMove move)
+        private void HandleCollidableMove(MoveEvent moveEvent)
         {
-            if (!_lastKnownPositions.ContainsKey(move.MoveEvent.Sender))
+            if (!_lastKnownPositions.ContainsKey(moveEvent.Sender))
             {
                 return;
             }
 
             // The pathfinding graph is tile-based so first we'll check if they're on a different tile and if we need to update.
             // If you get entities bigger than 1 tile wide you'll need some other system so god help you.
-            var moveEvent = move.MoveEvent;
-
             if (moveEvent.Sender.Deleted)
             {
                 HandleCollidableRemove(moveEvent.Sender);
@@ -314,14 +382,12 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
 
             _lastKnownPositions[moveEvent.Sender] = newTile;
 
-            if (!moveEvent.Sender.TryGetComponent(out CollidableComponent collidableComponent))
+            if (!moveEvent.Sender.HasComponent<CollidableComponent>())
             {
                 HandleCollidableRemove(moveEvent.Sender);
                 return;
             }
 
-            var collisionLayer = collidableComponent.CollisionLayer;
-
             var gridIds = new HashSet<GridId>(2) {oldTile.GridIndex, newTile.GridIndex};
 
             foreach (var gridId in gridIds)
@@ -330,33 +396,53 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
                 {
                     var oldChunk = GetChunk(oldTile);
                     var oldNode = oldChunk.GetNode(oldTile);
-                    oldNode.RemoveCollisionLayer(collisionLayer);
+                    oldNode.RemoveEntity(moveEvent.Sender);
                 }
 
                 if (newTile.GridIndex == gridId)
                 {
                     var newChunk = GetChunk(newTile);
                     var newNode = newChunk.GetNode(newTile);
-                    newNode.RemoveCollisionLayer(collisionLayer);
+                    newNode.AddEntity(moveEvent.Sender);
                 }
             }
         }
 
         private void QueueCollisionEnabledEvent(CollisionChangeEvent collisionEvent)
         {
-            // TODO: Handle containers
+            _collidableUpdateQueue.Enqueue(collisionEvent);
-            var entityManager = IoCManager.Resolve<IEntityManager>();
-            var entity = entityManager.GetEntity(collisionEvent.Owner);
-            switch (collisionEvent.CanCollide)
-            {
-                case true:
-                    _queuedGraphUpdates.Enqueue(new CollisionChange(entity, true));
-                    break;
-                case false:
-                    _queuedGraphUpdates.Enqueue(new CollisionChange(entity, false));
-                    break;
-            }
         }
         #endregion
+
+        // TODO: Need to rethink the pathfinder utils (traversable etc.). Maybe just chuck them all in PathfindingSystem
+        // Otherwise you get the steerer using this and the pathfinders using a different traversable.
+        // Also look at increasing tile cost the more physics entities are on it
+        public bool CanTraverse(IEntity entity, GridCoordinates grid)
+        {
+            var tile = _mapManager.GetGrid(grid.GridID).GetTileRef(grid);
+            var node = GetNode(tile);
+            return CanTraverse(entity, node);
+        }
+
+        public bool CanTraverse(IEntity entity, PathfindingNode node)
+        {
+            if (entity.TryGetComponent(out CollidableComponent collidableComponent) &&
+                (collidableComponent.CollisionMask & node.BlockedCollisionMask) != 0)
+            {
+                return false;
+            }
+
+            var access = AccessReader.FindAccessTags(entity);
+            
+            foreach (var reader in node.AccessReaders)
+            {
+                if (!reader.IsAllowed(access))
+                {
+                    return false;
+                }
+            }
+
+            return true;
+        }
     }
 }