diff --git a/Content.IntegrationTests/Tests/Pathfinding/PathfindingChunkTest.cs b/Content.IntegrationTests/Tests/Pathfinding/PathfindingChunkTest.cs new file mode 100644 index 0000000000..8c6d42a5c6 --- /dev/null +++ b/Content.IntegrationTests/Tests/Pathfinding/PathfindingChunkTest.cs @@ -0,0 +1,65 @@ +using System.Linq; +using System.Threading.Tasks; +using Content.Server.GameObjects.EntitySystems.AI.Pathfinding; +using NUnit.Framework; +using Robust.Shared.GameObjects.Systems; +using Robust.Shared.Interfaces.Map; +using Robust.Shared.IoC; +using Robust.Shared.Map; +using Robust.Shared.Maths; + +namespace Content.IntegrationTests.Tests.Pathfinding +{ + [TestFixture] + [TestOf(typeof(PathfindingChunk))] + public class PathfindingChunkTest : ContentIntegrationTest + { + [Test] + public async Task Test() + { + var server = StartServerDummyTicker(); + + server.Assert(() => + { + var pathfindingSystem = EntitySystem.Get(); + var mapMan = IoCManager.Resolve(); + + // Setup + var mapId = mapMan.CreateMap(new MapId(1)); + var gridId = new GridId(2); + mapMan.CreateGrid(mapId, gridId); + var chunkTile = mapMan.GetGrid(gridId).GetTileRef(new MapIndices(0, 0)); + var chunk = pathfindingSystem.GetChunk(chunkTile); + Assert.That(chunk.Nodes.Length == PathfindingChunk.ChunkSize * PathfindingChunk.ChunkSize); + + // Neighbors + var chunkNeighbors = chunk.GetNeighbors().ToList(); + Assert.That(chunkNeighbors.Count == 0); + var neighborChunkTile = mapMan.GetGrid(gridId).GetTileRef(new MapIndices(PathfindingChunk.ChunkSize, PathfindingChunk.ChunkSize)); + var neighborChunk = pathfindingSystem.GetChunk(neighborChunkTile); + chunkNeighbors = chunk.GetNeighbors().ToList(); + Assert.That(chunkNeighbors.Count == 1); + + // Directions + Assert.That(PathfindingHelpers.RelativeDirection(neighborChunk, chunk) == Direction.NorthEast); + Assert.That(PathfindingHelpers.RelativeDirection(chunk.Nodes[0, 1], chunk.Nodes[0, 0]) == Direction.North); + + // Nodes + var node = chunk.Nodes[1, 1]; + var nodeNeighbors = node.GetNeighbors().ToList(); + Assert.That(nodeNeighbors.Count == 8); + + // Bottom-left corner with no chunk neighbor + node = chunk.Nodes[0, 0]; + nodeNeighbors = node.GetNeighbors().ToList(); + Assert.That(nodeNeighbors.Count == 3); + + // Given we have 1 NE neighbor then NE corner should have 4 neighbors due to the 1 extra from the neighbor chunk + node = chunk.Nodes[PathfindingChunk.ChunkSize - 1, PathfindingChunk.ChunkSize - 1]; + nodeNeighbors = node.GetNeighbors().ToList(); + Assert.That(nodeNeighbors.Count == 4); + }); + await server.WaitIdleAsync(); + } + } +} \ No newline at end of file diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs index e69b9e06d8..445569e44f 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/AStarPathfindingJob.cs @@ -80,7 +80,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders closedTiles.Add(currentNode); - foreach (var (direction, nextNode) in currentNode.Neighbors) + foreach (var nextNode in currentNode.GetNeighbors()) { if (closedTiles.Contains(nextNode)) { @@ -89,6 +89,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders // 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)) { diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/JpsPathfindingJob.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/JpsPathfindingJob.cs index ffae0d7d89..0d47f0671b 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/JpsPathfindingJob.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/Pathfinders/JpsPathfindingJob.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Linq; using System.Threading; using System.Threading.Tasks; using Content.Server.GameObjects.EntitySystems.JobQueues; @@ -14,6 +15,8 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders { public class JpsPathfindingJob : Job> { + // Some of this is probably fugly due to other structural changes in pathfinding so it could do with optimisation + // Realistically it's probably not getting used given it doesn't support tile costs which can be very useful public static event Action DebugRoute; private PathfindingNode _startNode; @@ -78,8 +81,9 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders break; } - foreach (var (direction, _) in currentNode.Neighbors) + foreach (var node in currentNode.GetNeighbors()) { + var direction = PathfindingHelpers.RelativeDirection(node, currentNode); var jumpNode = GetJumpPoint(currentNode, direction, _endNode); if (jumpNode != null && !closedTiles.Contains(jumpNode)) @@ -156,7 +160,15 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders while (count < 1000) { count++; - var nextNode = currentNode.GetNeighbor(direction); + PathfindingNode nextNode = null; + foreach (var node in currentNode.GetNeighbors()) + { + if (PathfindingHelpers.RelativeDirection(node, currentNode) == direction) + { + nextNode = node; + break; + } + } // We'll do opposite DirectionTraversable just because of how the method's setup // Nodes should be 2-way anyway. @@ -270,43 +282,99 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders private bool IsDiagonalJumpPoint(Direction direction, PathfindingNode currentNode) { // If we're going diagonally need to check all cardinals. - // I just just using casts int casts and offset to make it smaller but brain no workyand it wasn't working. + // I tried just casting direction ints and offsets to make it smaller but brain no worky. // From NorthEast we check (Closed / Open) S - SE, W - NW - - PathfindingNode openNeighborOne; - PathfindingNode closedNeighborOne; - PathfindingNode openNeighborTwo; - PathfindingNode closedNeighborTwo; + + PathfindingNode openNeighborOne = null; + PathfindingNode closedNeighborOne = null; + PathfindingNode openNeighborTwo = null; + PathfindingNode closedNeighborTwo = null; switch (direction) { case Direction.NorthEast: - openNeighborOne = currentNode.GetNeighbor(Direction.SouthEast); - closedNeighborOne = currentNode.GetNeighbor(Direction.South); - - openNeighborTwo = currentNode.GetNeighbor(Direction.NorthWest); - closedNeighborTwo = currentNode.GetNeighbor(Direction.West); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.SouthEast: + openNeighborOne = neighbor; + break; + case Direction.South: + closedNeighborOne = neighbor; + break; + case Direction.NorthWest: + openNeighborTwo = neighbor; + break; + case Direction.West: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.SouthEast: - openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast); - closedNeighborOne = currentNode.GetNeighbor(Direction.North); - - openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest); - closedNeighborTwo = currentNode.GetNeighbor(Direction.West); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.NorthEast: + openNeighborOne = neighbor; + break; + case Direction.North: + closedNeighborOne = neighbor; + break; + case Direction.SouthWest: + openNeighborTwo = neighbor; + break; + case Direction.West: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.SouthWest: - openNeighborOne = currentNode.GetNeighbor(Direction.NorthWest); - closedNeighborOne = currentNode.GetNeighbor(Direction.North); - - openNeighborTwo = currentNode.GetNeighbor(Direction.SouthEast); - closedNeighborTwo = currentNode.GetNeighbor(Direction.East); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.NorthWest: + openNeighborOne = neighbor; + break; + case Direction.North: + closedNeighborOne = neighbor; + break; + case Direction.SouthEast: + openNeighborTwo = neighbor; + break; + case Direction.East: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.NorthWest: - openNeighborOne = currentNode.GetNeighbor(Direction.SouthWest); - closedNeighborOne = currentNode.GetNeighbor(Direction.South); - - openNeighborTwo = currentNode.GetNeighbor(Direction.NorthEast); - closedNeighborTwo = currentNode.GetNeighbor(Direction.East); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.SouthWest: + openNeighborOne = neighbor; + break; + case Direction.South: + closedNeighborOne = neighbor; + break; + case Direction.NorthEast: + openNeighborTwo = neighbor; + break; + case Direction.East: + closedNeighborTwo = neighbor; + break; + } + } break; default: throw new ArgumentOutOfRangeException(); @@ -332,40 +400,96 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders /// private bool IsCardinalJumpPoint(Direction direction, PathfindingNode currentNode) { - PathfindingNode openNeighborOne; - PathfindingNode closedNeighborOne; - PathfindingNode openNeighborTwo; - PathfindingNode closedNeighborTwo; + PathfindingNode openNeighborOne = null; + PathfindingNode closedNeighborOne = null; + PathfindingNode openNeighborTwo = null; + PathfindingNode closedNeighborTwo = null; switch (direction) { case Direction.North: - openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast); - closedNeighborOne = currentNode.GetNeighbor(Direction.East); - - openNeighborTwo = currentNode.GetNeighbor(Direction.NorthWest); - closedNeighborTwo = currentNode.GetNeighbor(Direction.West); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.NorthEast: + openNeighborOne = neighbor; + break; + case Direction.East: + closedNeighborOne = neighbor; + break; + case Direction.NorthWest: + openNeighborTwo = neighbor; + break; + case Direction.West: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.East: - openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast); - closedNeighborOne = currentNode.GetNeighbor(Direction.North); - - openNeighborTwo = currentNode.GetNeighbor(Direction.SouthEast); - closedNeighborTwo = currentNode.GetNeighbor(Direction.South); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.NorthEast: + openNeighborOne = neighbor; + break; + case Direction.North: + closedNeighborOne = neighbor; + break; + case Direction.SouthEast: + openNeighborTwo = neighbor; + break; + case Direction.South: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.South: - openNeighborOne = currentNode.GetNeighbor(Direction.SouthEast); - closedNeighborOne = currentNode.GetNeighbor(Direction.East); - - openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest); - closedNeighborTwo = currentNode.GetNeighbor(Direction.West); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.SouthEast: + openNeighborOne = neighbor; + break; + case Direction.East: + closedNeighborOne = neighbor; + break; + case Direction.SouthWest: + openNeighborTwo = neighbor; + break; + case Direction.West: + closedNeighborTwo = neighbor; + break; + } + } break; case Direction.West: - openNeighborOne = currentNode.GetNeighbor(Direction.NorthWest); - closedNeighborOne = currentNode.GetNeighbor(Direction.North); - - openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest); - closedNeighborTwo = currentNode.GetNeighbor(Direction.South); + foreach (var neighbor in currentNode.GetNeighbors()) + { + var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); + switch (neighborDirection) + { + case Direction.NorthWest: + openNeighborOne = neighbor; + break; + case Direction.North: + closedNeighborOne = neighbor; + break; + case Direction.SouthWest: + openNeighborTwo = neighbor; + break; + case Direction.South: + closedNeighborTwo = neighbor; + break; + } + } break; default: throw new ArgumentOutOfRangeException(); diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingChunk.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingChunk.cs index 74b58fd5b8..202961dc3f 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingChunk.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingChunk.cs @@ -1,6 +1,9 @@ using System; using System.Collections.Generic; +using System.IO; +using System.Linq; using Content.Server.GameObjects.EntitySystems.Pathfinding; +using Robust.Shared.GameObjects.Systems; using Robust.Shared.Interfaces.Map; using Robust.Shared.IoC; using Robust.Shared.Map; @@ -19,7 +22,6 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding public static int ChunkSize => 16; public PathfindingNode[,] Nodes => _nodes; private PathfindingNode[,] _nodes = new PathfindingNode[ChunkSize,ChunkSize]; - public Dictionary Neighbors { get; } = new Dictionary(8); public PathfindingChunk(GridId gridId, MapIndices indices) { @@ -38,244 +40,31 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding CreateNode(tileRef); } } - - RefreshNodeNeighbors(); } - /// - /// Updates all internal nodes with references to every other internal node - /// - private void RefreshNodeNeighbors() + public IEnumerable GetNeighbors() { - for (var x = 0; x < ChunkSize; x++) + var pathfindingSystem = EntitySystem.Get(); + var chunkGrid = pathfindingSystem.Graph[GridId]; + + for (var x = -1; x <= 1; x++) { - for (var y = 0; y < ChunkSize; y++) + for (var y = -1; y <= 1; y++) { - var node = _nodes[x, y]; - // West - if (x != 0) + if (x == 0 && y == 0) continue; + var (neighborX, neighborY) = (_indices.X + ChunkSize * x, _indices.Y + ChunkSize * y); + if (chunkGrid.TryGetValue(new MapIndices(neighborX, neighborY), out var neighbor)) { - if (y != ChunkSize - 1) - { - node.AddNeighbor(Direction.NorthWest, _nodes[x - 1, y + 1]); - } - node.AddNeighbor(Direction.West, _nodes[x - 1, y]); - if (y != 0) - { - node.AddNeighbor(Direction.SouthWest, _nodes[x - 1, y - 1]); - } - } - - // Same column - if (y != ChunkSize - 1) - { - node.AddNeighbor(Direction.North, _nodes[x, y + 1]); - } - - if (y != 0) - { - node.AddNeighbor(Direction.South, _nodes[x, y - 1]); - } - - // East - if (x != ChunkSize - 1) - { - if (y != ChunkSize - 1) - { - node.AddNeighbor(Direction.NorthEast, _nodes[x + 1, y + 1]); - } - node.AddNeighbor(Direction.East, _nodes[x + 1, y]); - if (y != 0) - { - node.AddNeighbor(Direction.SouthEast, _nodes[x + 1, y - 1]); - } + yield return neighbor; } } } } - /// - /// This will work both ways - /// - /// - /// - public void AddNeighbor(PathfindingChunk chunk) + public bool InBounds(MapIndices mapIndices) { - if (chunk == this) return; - if (Neighbors.ContainsValue(chunk)) - { - return; - } - - Direction direction; - if (chunk.Indices.X < _indices.X) - { - if (chunk.Indices.Y > _indices.Y) - { - direction = Direction.NorthWest; - } else if (chunk.Indices.Y < _indices.Y) - { - direction = Direction.SouthWest; - } - else - { - direction = Direction.West; - } - } - else if (chunk.Indices.X > _indices.X) - { - if (chunk.Indices.Y > _indices.Y) - { - direction = Direction.NorthEast; - } else if (chunk.Indices.Y < _indices.Y) - { - direction = Direction.SouthEast; - } - else - { - direction = Direction.East; - } - } - else - { - if (chunk.Indices.Y > _indices.Y) - { - direction = Direction.North; - } else if (chunk.Indices.Y < _indices.Y) - { - direction = Direction.South; - } - else - { - throw new InvalidOperationException(); - } - } - - Neighbors.TryAdd(direction, chunk); - - foreach (var node in GetBorderNodes(direction)) - { - foreach (var counter in chunk.GetCounterpartNodes(direction)) - { - var xDiff = node.TileRef.X - counter.TileRef.X; - var yDiff = node.TileRef.Y - counter.TileRef.Y; - - if (Math.Abs(xDiff) <= 1 && Math.Abs(yDiff) <= 1) - { - node.AddNeighbor(counter); - counter.AddNeighbor(node); - } - } - } - - chunk.Neighbors.TryAdd(OppositeDirection(direction), this); - - if (Neighbors.Count > 8) - { - throw new InvalidOperationException(); - } - } - - private Direction OppositeDirection(Direction direction) - { - return (Direction) (((int) direction + 4) % 8); - } - - // TODO I was too tired to think of an easier system. Could probably just google an array wraparound - private IEnumerable GetCounterpartNodes(Direction direction) - { - switch (direction) - { - case Direction.West: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[ChunkSize - 1, i]; - } - break; - case Direction.SouthWest: - yield return _nodes[ChunkSize - 1, ChunkSize - 1]; - break; - case Direction.South: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[i, ChunkSize - 1]; - } - break; - case Direction.SouthEast: - yield return _nodes[0, ChunkSize - 1]; - break; - case Direction.East: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[0, i]; - } - break; - case Direction.NorthEast: - yield return _nodes[0, 0]; - break; - case Direction.North: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[i, 0]; - } - break; - case Direction.NorthWest: - yield return _nodes[ChunkSize - 1, 0]; - break; - default: - throw new ArgumentOutOfRangeException(nameof(direction), direction, null); - } - } - - public IEnumerable GetBorderNodes(Direction direction) - { - switch (direction) - { - case Direction.East: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[ChunkSize - 1, i]; - } - break; - case Direction.NorthEast: - yield return _nodes[ChunkSize - 1, ChunkSize - 1]; - break; - case Direction.North: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[i, ChunkSize - 1]; - } - break; - case Direction.NorthWest: - yield return _nodes[0, ChunkSize - 1]; - break; - case Direction.West: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[0, i]; - } - break; - case Direction.SouthWest: - yield return _nodes[0, 0]; - break; - case Direction.South: - for (var i = 0; i < ChunkSize; i++) - { - yield return _nodes[i, 0]; - } - break; - case Direction.SouthEast: - yield return _nodes[ChunkSize - 1, 0]; - break; - default: - throw new ArgumentOutOfRangeException(nameof(direction), direction, null); - } - } - - public bool InBounds(TileRef tile) - { - if (tile.X < _indices.X || tile.Y < _indices.Y) return false; - if (tile.X >= _indices.X + ChunkSize || tile.Y >= _indices.Y + ChunkSize) return false; + if (mapIndices.X < _indices.X || mapIndices.Y < _indices.Y) return false; + if (mapIndices.X >= _indices.X + ChunkSize || mapIndices.Y >= _indices.Y + ChunkSize) return false; return true; } @@ -293,6 +82,73 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding return false; } + /// + /// Gets our neighbors that are relevant for the node to retrieve its own neighbors + /// + /// + /// + public IEnumerable RelevantChunks(PathfindingNode node) + { + var relevantDirections = GetEdges(node).ToList(); + + foreach (var chunk in GetNeighbors()) + { + var chunkDirection = PathfindingHelpers.RelativeDirection(chunk, this); + if (relevantDirections.Contains(chunkDirection)) + { + yield return chunk; + } + } + } + + private IEnumerable GetEdges(PathfindingNode node) + { + // West Edge + if (node.TileRef.X == _indices.X) + { + yield return Direction.West; + if (node.TileRef.Y == _indices.Y) + { + yield return Direction.SouthWest; + yield return Direction.South; + } else if (node.TileRef.Y == _indices.Y + ChunkSize - 1) + { + yield return Direction.NorthWest; + yield return Direction.North; + } + + yield break; + } + // East edge + if (node.TileRef.X == _indices.X + ChunkSize - 1) + { + yield return Direction.East; + if (node.TileRef.Y == _indices.Y) + { + yield return Direction.SouthEast; + yield return Direction.South; + } else if (node.TileRef.Y == _indices.Y + ChunkSize - 1) + { + yield return Direction.NorthEast; + yield return Direction.North; + } + + yield break; + + } + // South edge + if (node.TileRef.Y == _indices.Y) + { + yield return Direction.South; + // Given we already checked south-west and south-east above shouldn't need any more + } + // North edge + if (node.TileRef.Y == _indices.Y + ChunkSize - 1) + { + yield return Direction.North; + } + } + public PathfindingNode GetNode(TileRef tile) { var chunkX = tile.X - _indices.X; diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs index 127c43a8d4..e4c04d5a94 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingHelpers.cs @@ -1,6 +1,5 @@ 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; @@ -21,7 +20,7 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding { // TODO: Should make this account for proximities, // probably some kind of breadth-first search to find a valid one - foreach (var (_, node) in endNode.Neighbors) + foreach (var node in endNode.GetNeighbors()) { if (Traversable(pathfindingArgs.CollisionMask, pathfindingArgs.Access, node)) { @@ -42,11 +41,41 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding // 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 // Also this will help with corner-cutting - - currentNode.Neighbors.TryGetValue(Direction.North, out var northNeighbor); - currentNode.Neighbors.TryGetValue(Direction.South, out var southNeighbor); - currentNode.Neighbors.TryGetValue(Direction.East, out var eastNeighbor); - currentNode.Neighbors.TryGetValue(Direction.West, out var westNeighbor); + + PathfindingNode northNeighbor = null; + PathfindingNode southNeighbor = null; + PathfindingNode eastNeighbor = null; + PathfindingNode westNeighbor = null; + foreach (var neighbor in currentNode.GetNeighbors()) + { + if (neighbor.TileRef.X == currentNode.TileRef.X && + neighbor.TileRef.Y == currentNode.TileRef.Y + 1) + { + northNeighbor = neighbor; + continue; + } + + if (neighbor.TileRef.X == currentNode.TileRef.X + 1 && + neighbor.TileRef.Y == currentNode.TileRef.Y) + { + eastNeighbor = neighbor; + continue; + } + + if (neighbor.TileRef.X == currentNode.TileRef.X && + neighbor.TileRef.Y == currentNode.TileRef.Y - 1) + { + southNeighbor = neighbor; + continue; + } + + if (neighbor.TileRef.X == currentNode.TileRef.X - 1 && + neighbor.TileRef.Y == currentNode.TileRef.Y) + { + westNeighbor = neighbor; + continue; + } + } switch (direction) { @@ -255,5 +284,66 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding return cost; } + + public static Direction RelativeDirection(PathfindingChunk endChunk, PathfindingChunk startChunk) + { + var xDiff = (endChunk.Indices.X - startChunk.Indices.X) / PathfindingChunk.ChunkSize; + var yDiff = (endChunk.Indices.Y - startChunk.Indices.Y) / PathfindingChunk.ChunkSize; + + return RelativeDirection(xDiff, yDiff); + } + + public static Direction RelativeDirection(PathfindingNode endNode, PathfindingNode startNode) + { + var xDiff = endNode.TileRef.X - startNode.TileRef.X; + var yDiff = endNode.TileRef.Y - startNode.TileRef.Y; + + return RelativeDirection(xDiff, yDiff); + } + + public static Direction RelativeDirection(int x, int y) + { + switch (x) + { + case -1: + switch (y) + { + case -1: + return Direction.SouthWest; + case 0: + return Direction.West; + case 1: + return Direction.NorthWest; + default: + throw new InvalidOperationException(); + } + case 0: + switch (y) + { + case -1: + return Direction.South; + case 0: + throw new InvalidOperationException(); + case 1: + return Direction.North; + default: + throw new InvalidOperationException(); + } + case 1: + switch (y) + { + case -1: + return Direction.SouthEast; + case 0: + return Direction.East; + case 1: + return Direction.NorthEast; + default: + throw new InvalidOperationException(); + } + default: + throw new InvalidOperationException(); + } + } } } diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs index 262631a4b3..855bad56e7 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Linq; using Content.Server.GameObjects.Components.Access; using Content.Server.GameObjects.Components.Doors; using Content.Server.GameObjects.EntitySystems.AI.Pathfinding; @@ -9,6 +10,7 @@ using Robust.Shared.GameObjects.Components; using Robust.Shared.Interfaces.GameObjects; using Robust.Shared.Map; using Robust.Shared.Maths; +using Robust.Shared.Utility; namespace Content.Server.GameObjects.EntitySystems.Pathfinding { @@ -16,10 +18,7 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding { public PathfindingChunk ParentChunk => _parentChunk; private readonly PathfindingChunk _parentChunk; - - public Dictionary Neighbors => _neighbors; - private Dictionary _neighbors = new Dictionary(); - + public TileRef TileRef { get; private set; } /// @@ -45,72 +44,48 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding GenerateMask(); } - public void AddNeighbor(Direction direction, PathfindingNode node) + /// + /// Return our neighboring nodes (even across chunks) + /// + /// + public IEnumerable GetNeighbors() { - _neighbors.Add(direction, node); - } - - public void AddNeighbor(PathfindingNode node) - { - if (node.TileRef.GridIndex != TileRef.GridIndex) + List neighborChunks = null; + if (ParentChunk.OnEdge(this)) { - throw new InvalidOperationException(); + neighborChunks = ParentChunk.RelevantChunks(this).ToList(); } - - Direction direction; - if (node.TileRef.X < TileRef.X) + + for (var x = -1; x <= 1; x++) { - if (node.TileRef.Y > TileRef.Y) + for (var y = -1; y <= 1; y++) { - direction = Direction.NorthWest; - } else if (node.TileRef.Y < TileRef.Y) - { - direction = Direction.SouthWest; - } - else - { - direction = Direction.West; + if (x == 0 && y == 0) continue; + var indices = new MapIndices(TileRef.X + x, TileRef.Y + y); + if (ParentChunk.InBounds(indices)) + { + var (relativeX, relativeY) = (indices.X - ParentChunk.Indices.X, + indices.Y - ParentChunk.Indices.Y); + yield return ParentChunk.Nodes[relativeX, relativeY]; + } + else + { + DebugTools.AssertNotNull(neighborChunks); + // Get the relevant chunk and then get the node on it + foreach (var neighbor in neighborChunks) + { + // A lot of edge transitions are going to have a single neighboring chunk + // (given > 1 only affects corners) + // So we can just check the count to see if it's inbound + if (neighborChunks.Count > 0 && !neighbor.InBounds(indices)) continue; + var (relativeX, relativeY) = (indices.X - neighbor.Indices.X, + indices.Y - neighbor.Indices.Y); + yield return neighbor.Nodes[relativeX, relativeY]; + break; + } + } } } - else if (node.TileRef.X > TileRef.X) - { - if (node.TileRef.Y > TileRef.Y) - { - direction = Direction.NorthEast; - } else if (node.TileRef.Y < TileRef.Y) - { - direction = Direction.SouthEast; - } - else - { - direction = Direction.East; - } - } - else - { - if (node.TileRef.Y > TileRef.Y) - { - direction = Direction.North; - } - else - { - direction = Direction.South; - } - } - - if (_neighbors.ContainsKey(direction)) - { - // Should we verify that they align? - return; - } - - _neighbors.Add(direction, node); - } - - public PathfindingNode GetNeighbor(Direction direction) - { - _neighbors.TryGetValue(direction, out var node); - return node; } public void UpdateTile(TileRef newTile) diff --git a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingSystem.cs b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingSystem.cs index 62a8479306..5a718e01e2 100644 --- a/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingSystem.cs +++ b/Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingSystem.cs @@ -167,32 +167,11 @@ namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding { var newChunk = new PathfindingChunk(gridId, indices); newChunk.Initialize(); - if (_graph.TryGetValue(gridId, out var chunks)) - { - for (var x = -1; x < 2; x++) - { - for (var y = -1; y < 2; y++) - { - if (x == 0 && y == 0) continue; - - var neighborIndices = new MapIndices( - indices.X + x * PathfindingChunk.ChunkSize, - indices.Y + y * PathfindingChunk.ChunkSize); - - if (chunks.TryGetValue(neighborIndices, out var neighborChunk)) - { - neighborChunk.AddNeighbor(newChunk); - } - } - } - } - else + if (!_graph.ContainsKey(gridId)) { _graph.Add(gridId, new Dictionary()); } - _graph[gridId].Add(indices, newChunk); - return newChunk; }