Add Breadth-First Search pathfinder (#1283)

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

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Accessible/BFSPathfinder.cs

@@ -0,0 +1,70 @@
+using System.Collections.Generic;
+using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders;
+using Content.Server.GameObjects.EntitySystems.Pathfinding;
+using Robust.Shared.GameObjects.Systems;
+using Robust.Shared.Map;
+
+namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Accessible
+{
+    /// <summary>
+    /// The simplest pathfinder
+    /// </summary>
+    public sealed class BFSPathfinder
+    {
+        /// <summary>
+        /// Gets all of the tiles in range that can we access
+        /// </summary>
+        /// If you want Dikstra then add distances.
+        /// Doesn't use the JobQueue as it will generally be encapsulated by other jobs
+        /// <param name="pathfindingArgs"></param>
+        /// <param name="range"></param>
+        /// <param name="fromStart">Whether we traverse from the starting tile or the end tile</param>
+        /// <returns></returns>
+        public static IEnumerable<PathfindingNode> GetNodesInRange(PathfindingArgs pathfindingArgs, bool fromStart = true)
+        {
+            var pathfindingSystem = EntitySystem.Get<PathfindingSystem>();
+            // Don't need a priority queue given not looking for shortest path
+            var openTiles = new Queue<PathfindingNode>();
+            var closedTiles = new HashSet<TileRef>();
+            PathfindingNode startNode;
+
+            if (fromStart)
+            {
+                startNode = pathfindingSystem.GetNode(pathfindingArgs.Start);
+            }
+            else
+            {
+                startNode = pathfindingSystem.GetNode(pathfindingArgs.End);
+            }
+            
+            PathfindingNode currentNode;
+            openTiles.Enqueue(startNode);
+
+            while (openTiles.Count > 0)
+            {
+                currentNode = openTiles.Dequeue();
+
+                foreach (var neighbor in currentNode.GetNeighbors())
+                {
+                    // No distances stored so can just check closed tiles here
+                    if (closedTiles.Contains(neighbor.TileRef)) continue;
+                    closedTiles.Add(currentNode.TileRef);
+                    
+                    // So currently tileCost gets the octile distance between the 2 so we'll also use that for our range check
+                    var tileCost = PathfindingHelpers.GetTileCost(pathfindingArgs, startNode, neighbor);
+                    var direction = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
+                    
+                    if (tileCost == null || 
+                        tileCost > pathfindingArgs.Proximity || 
+                        !PathfindingHelpers.DirectionTraversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, currentNode, direction))
+                    {
+                        continue;
+                    }
+
+                    openTiles.Enqueue(neighbor);
+                    yield return neighbor;
+                }
+            }
+        }
+    }
+}

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

@@ -1,5 +1,7 @@
 using System;
 using System.Collections.Generic;
+using Content.Server.GameObjects.Components.Access;
+using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Accessible;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders;
 using Content.Server.GameObjects.EntitySystems.Pathfinding;
 using Robust.Shared.Interfaces.GameObjects;
@@ -18,17 +20,12 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             {
                 if (pathfindingArgs.Proximity > 0.0f)
                 {
-                    // TODO: Should make this account for proximities,
+                    foreach (var node in BFSPathfinder.GetNodesInRange(pathfindingArgs, false))
-                    // probably some kind of breadth-first search to find a valid one
-                    foreach (var node in endNode.GetNeighbors())
-                    {
-                        if (Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, node))
                     {
                         endNode = node;
                         return true;
                     }
                 }
-                }
 
                 return false;
             }