From 1d96adcc2cc5978381510397476adc66d4da97d6 Mon Sep 17 00:00:00 2001
From: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com>
Date: Wed, 8 Jul 2020 09:41:41 +1000
Subject: [PATCH] Add Breadth-First Search pathfinder (#1283)

Co-authored-by: Metal Gear Sloth <metalgearsloth@gmail.com>
---
 .../Pathfinding/Accessible/BFSPathfinder.cs   | 70 +++++++++++++++++++
 .../AI/Pathfinding/PathfindingHelpers.cs      | 13 ++--
 2 files changed, 75 insertions(+), 8 deletions(-)
 create mode 100644 Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Accessible/BFSPathfinder.cs

diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Accessible/BFSPathfinder.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Accessible/BFSPathfinder.cs
new file mode 100644
index 0000000000..ddb5628a02
--- /dev/null
+++ b/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;
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs
index e4c04d5a94..4c5e25390a 100644
--- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs
+++ b/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,15 +20,10 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding
             {
                 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 node in endNode.GetNeighbors())
+                    foreach (var node in BFSPathfinder.GetNodesInRange(pathfindingArgs, false))
                     {
-                        if (Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, node))
-                        {
-                            endNode = node;
-                            return true;
-                        }
+                        endNode = node;
+                        return true;
                     }
                 }