diff --git a/Content.Client/GameObjects/EntitySystems/AI/ClientAiDebugSystem.cs b/Content.Client/GameObjects/EntitySystems/AI/ClientAiDebugSystem.cs index e0071b5116..eb2fa68567 100644 --- a/Content.Client/GameObjects/EntitySystems/AI/ClientAiDebugSystem.cs +++ b/Content.Client/GameObjects/EntitySystems/AI/ClientAiDebugSystem.cs @@ -103,8 +103,8 @@ namespace Content.Client.GameObjects.EntitySystems.AI var label = (Label) _aiBoxes[entity].GetChild(0).GetChild(1); label.Text = $"Pathfinding time (ms): {message.TimeTaken * 1000:0.0000}\n" + - $"Nodes traversed: {message.ClosedTiles.Count}\n" + - $"Nodes per ms: {message.ClosedTiles.Count / (message.TimeTaken * 1000)}"; + $"Nodes traversed: {message.CameFrom.Count}\n" + + $"Nodes per ms: {message.CameFrom.Count / (message.TimeTaken * 1000)}"; } } diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs index 445569e44f..0f9f4deacc 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs @@ -46,70 +46,73 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders return null; } - var openTiles = new PriorityQueue>(new PathfindingComparer()); - var gScores = new Dictionary(); + var frontier = new PriorityQueue>(new PathfindingComparer()); + var costSoFar = new Dictionary(); var cameFrom = new Dictionary(); - var closedTiles = new HashSet(); PathfindingNode currentNode = null; - openTiles.Add((0.0f, _startNode)); - gScores[_startNode] = 0.0f; + frontier.Add((0.0f, _startNode)); + costSoFar[_startNode] = 0.0f; var routeFound = false; var count = 0; - - while (openTiles.Count > 0) + + while (frontier.Count > 0) { + // Handle whether we need to pause if we've taken too long count++; - if (count % 20 == 0 && count > 0) { await SuspendIfOutOfTime(); - } - if (_startNode == null || _endNode == null) - { - return null; + if (_startNode == null || _endNode == null) + { + return null; + } } - - (_, currentNode) = openTiles.Take(); + + // Actual pathfinding here + (_, currentNode) = frontier.Take(); if (currentNode.Equals(_endNode)) { routeFound = true; break; } - closedTiles.Add(currentNode); - foreach (var nextNode in currentNode.GetNeighbors()) { - if (closedTiles.Contains(nextNode)) - { - continue; - } - // If tile is untraversable it'll be null var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode); - var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode); - - if (tileCost == null || !PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction)) + if (tileCost == null) { continue; } - var gScore = gScores[currentNode] + tileCost.Value; + // So if we're going NE then that means either N or E needs to be free to actually get there + var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode); + if (!PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction)) + { + continue; + } - if (gScores.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue) + // f = g + h + // gScore is distance to the start node + // hScore is distance to the end node + var gScore = costSoFar[currentNode] + tileCost.Value; + if (costSoFar.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue) { continue; } cameFrom[nextNode] = currentNode; - gScores[nextNode] = gScore; + costSoFar[nextNode] = gScore; // 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] + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f); - openTiles.Add((fScore, nextNode)); + // The closer the fScore is to the actual distance then the better the pathfinder will be + // (i.e. somewhere between 1 and infinite) + // Can use hierarchical pathfinder or whatever to improve the heuristic but this is fine for now. + var fScore = gScore + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f); + frontier.Add((fScore, nextNode)); } } @@ -130,30 +133,22 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders if (DebugRoute != null && route.Count > 0) { var debugCameFrom = new Dictionary(cameFrom.Count); - var debugGScores = new Dictionary(gScores.Count); - var debugClosedTiles = new HashSet(closedTiles.Count); - + var debugGScores = new Dictionary(costSoFar.Count); foreach (var (node, parent) in cameFrom) { debugCameFrom.Add(node.TileRef, parent.TileRef); } - foreach (var (node, score) in gScores) + foreach (var (node, score) in costSoFar) { debugGScores.Add(node.TileRef, score); } - foreach (var node in closedTiles) - { - debugClosedTiles.Add(node.TileRef); - } - var debugRoute = new SharedAiDebug.AStarRouteDebug( _pathfindingArgs.Uid, route, debugCameFrom, debugGScores, - debugClosedTiles, DebugTime); DebugRoute.Invoke(debugRoute); diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/ServerPathfindingDebugSystem.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/ServerPathfindingDebugSystem.cs index f67b441ff5..cc93480280 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/ServerPathfindingDebugSystem.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/ServerPathfindingDebugSystem.cs @@ -55,19 +55,11 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding gScores.Add(mapManager.GetGrid(tile.GridIndex).LocalToWorld(tileGrid).Position, score); } - var closedTiles = new List(); - foreach (var tile in routeDebug.ClosedTiles) - { - var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices); - closedTiles.Add(mapManager.GetGrid(tile.GridIndex).LocalToWorld(tileGrid).Position); - } - var systemMessage = new SharedAiDebug.AStarRouteMessage( routeDebug.EntityUid, route, cameFrom, gScores, - closedTiles, routeDebug.TimeTaken ); diff --git a/Content.Shared/AI/SharedAiDebug.cs b/Content.Shared/AI/SharedAiDebug.cs index 23c838315d..7033901aa0 100644 --- a/Content.Shared/AI/SharedAiDebug.cs +++ b/Content.Shared/AI/SharedAiDebug.cs @@ -58,7 +58,6 @@ namespace Content.Shared.AI public Queue Route { get; } public Dictionary CameFrom { get; } public Dictionary GScores { get; } - public HashSet ClosedTiles { get; } public double TimeTaken { get; } public AStarRouteDebug( @@ -66,14 +65,12 @@ namespace Content.Shared.AI Queue route, Dictionary cameFrom, Dictionary gScores, - HashSet closedTiles, double timeTaken) { EntityUid = uid; Route = route; CameFrom = cameFrom; GScores = gScores; - ClosedTiles = closedTiles; TimeTaken = timeTaken; } } @@ -105,7 +102,6 @@ namespace Content.Shared.AI public readonly IEnumerable Route; public readonly Dictionary CameFrom; public readonly Dictionary GScores; - public readonly List ClosedTiles; public double TimeTaken; public AStarRouteMessage( @@ -113,14 +109,12 @@ namespace Content.Shared.AI IEnumerable route, Dictionary cameFrom, Dictionary gScores, - List closedTiles, double timeTaken) { EntityUid = uid; Route = route; CameFrom = cameFrom; GScores = gScores; - ClosedTiles = closedTiles; TimeTaken = timeTaken; } }