From 432ceb2506921bf769c750b048c668769b09efd1 Mon Sep 17 00:00:00 2001 From: Kyle Belanger Date: Sat, 24 Feb 2024 04:37:42 -0500 Subject: [PATCH] Base pathfinding implementation, conversion from packet<->pathing coordinate space in tests is still off --- BLHX.Server.Common/Utils/GridExtensions.cs | 34 +++++++ BLHX.Server.Common/Utils/Pathfinding.cs | 110 +++++++++++++++++++++ BLHX.Server.Game/Commands/TestCommand.cs | 55 ++++++++++- 3 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 BLHX.Server.Common/Utils/GridExtensions.cs create mode 100644 BLHX.Server.Common/Utils/Pathfinding.cs diff --git a/BLHX.Server.Common/Utils/GridExtensions.cs b/BLHX.Server.Common/Utils/GridExtensions.cs new file mode 100644 index 0000000..3df009b --- /dev/null +++ b/BLHX.Server.Common/Utils/GridExtensions.cs @@ -0,0 +1,34 @@ +using BLHX.Server.Common.Data; + +namespace BLHX.Server.Common.Utils; + +public static class GridExtensions +{ + public static GridItem? Find(this List nodes, uint x, uint y) + => nodes.FirstOrDefault(n => n.Row == x && n.Column == y); + + public static GridItem? Find(this List nodes, int x, int y) + => Find(nodes, (uint)x, (uint)y); + + public static (uint, uint, uint, uint) GetDimensions(this List grid) + { + uint startX = uint.MaxValue; + uint startY = uint.MaxValue; + uint width = 0; + uint height = 0; + + foreach (var node in grid) + { + if (node.Column < startX) + startX = node.Column; + if (node.Row < startY) + startY = node.Row; + if (node.Column > width) + width = node.Column; + if (node.Row > height) + height = node.Row; + } + + return (startX, startY, width, height); + } +} \ No newline at end of file diff --git a/BLHX.Server.Common/Utils/Pathfinding.cs b/BLHX.Server.Common/Utils/Pathfinding.cs new file mode 100644 index 0000000..589a758 --- /dev/null +++ b/BLHX.Server.Common/Utils/Pathfinding.cs @@ -0,0 +1,110 @@ +using BLHX.Server.Common.Data; + +namespace BLHX.Server.Common.Utils; + +public class PathNode +{ + public int X { get; set; } + public int Y { get; set; } + public PathNode Previous { get; set; } + public int GCost { get; set; } // Represents the cost from the start node to the current node + public int HCost { get; set; } // Represents the heuristic cost estimate, which is the estimated cost from the current node to the goal node + public int FCost => GCost + HCost; + public bool Blocking { get; set; } + + public override string ToString() + => $"Node: {{ X: {X}, Y: {Y}, GCost: {GCost}, HCost: {HCost}, FCost: {FCost}, Blocking: {Blocking} }}"; +} + +public static class Pathfinding +{ + public static List? AStar(List grid, uint startX, uint startY, uint goalX, uint goalY) + { + (uint _, uint _, uint width, uint height) = grid.GetDimensions(); + + var nodes = new PathNode[width, height]; + for (int x = 0; x < width; x++) + for (int y = 0; y < height; y++) + { + nodes[x, y] = new PathNode + { + X = x, + Y = y, + Blocking = grid.Find(x, y)?.Blocking ?? false + }; + } + + return AStar(nodes, nodes[startX, startY], nodes[goalX, goalY]); + } + + public static List? AStar(PathNode[,] grid, PathNode start, PathNode goal) + { + var openSet = new List { start }; + var closedSet = new HashSet(); + + start.GCost = 0; + start.HCost = HeuristicCostEstimate(start, goal); + + while (openSet.Count > 0) + { + var current = openSet.OrderBy(node => node.FCost).First(); + + if (current == goal) + return ReconstructPath(current); + + openSet.Remove(current); + closedSet.Add(current); + + foreach (var neighbor in GetNeighbors(grid, current)) + { + if (closedSet.Contains(neighbor) || neighbor.Blocking) + continue; + + var tentativeGCost = current.GCost + 1; + + if (tentativeGCost < neighbor.GCost || !openSet.Contains(neighbor)) + { + neighbor.Previous = current; + neighbor.GCost = tentativeGCost; + neighbor.HCost = HeuristicCostEstimate(neighbor, goal); + + if (!openSet.Contains(neighbor)) + openSet.Add(neighbor); + } + } + } + + return null; + } + + static List ReconstructPath(PathNode current) + { + var path = new List(); + while (current != null) + { + path.Insert(0, current); + current = current.Previous; + } + + return path; + } + + static PathNode[] GetNeighbors(PathNode[,] grid, PathNode node) + { + int x = node.X; + int y = node.Y; + int maxX = grid.GetLength(0) - 1; + int maxY = grid.GetLength(1) - 1; + var neighbors = new List(); + + if (x > 0) neighbors.Add(grid[x - 1, y]); + if (x < maxX) neighbors.Add(grid[x + 1, y]); + if (y > 0) neighbors.Add(grid[x, y - 1]); + if (y < maxY) neighbors.Add(grid[x, y + 1]); + + return neighbors.ToArray(); + } + + static int HeuristicCostEstimate(PathNode start, PathNode goal) + => Math.Abs(start.X - goal.X) + Math.Abs(start.Y - goal.Y); +} diff --git a/BLHX.Server.Game/Commands/TestCommand.cs b/BLHX.Server.Game/Commands/TestCommand.cs index f80520f..22f015e 100644 --- a/BLHX.Server.Game/Commands/TestCommand.cs +++ b/BLHX.Server.Game/Commands/TestCommand.cs @@ -19,7 +19,7 @@ public class TestCommand : Command [Argument("value")] public string? Value { get; set; } - + public override void Execute(Dictionary args) { base.Execute(args); @@ -32,6 +32,9 @@ public class TestCommand : Command case "lookup": LookupShip(Parse(Value, 1)); break; + case "pathfinding": + TestPathfinding(); + break; default: Logger.c.Warn("Unknown test type"); break; @@ -76,4 +79,54 @@ public class TestCommand : Command else Logger.c.Warn($"Ship {id} not found"); } + + void TestPathfinding() + { + bool verbose = Parse(Verbose, true); + var stopwatch = System.Diagnostics.Stopwatch.StartNew(); + var grid = Data.ChapterTemplate[103].GridItems; + (uint startX, uint startY, uint width, uint height) = grid.GetDimensions(); + var path = Pathfinding.AStar(grid, 0, 0, width - 1, height - 1); + + if (verbose) + { + Logger.c.Log("TEST GRID:"); + for (uint x = 0; x < width; x++) + { + for (uint y = 0; y < height; y++) + { + var gridItem = grid.Find(x, y).Value; + + if (gridItem.Column >= startX && gridItem.Column <= width && + gridItem.Row >= startY && gridItem.Row <= height) + Console.ForegroundColor = ConsoleColor.Green; + else if (x == 0 && y == 0) + Console.ForegroundColor = ConsoleColor.Cyan; + else if (x == width - 1 && y == height - 1) + Console.ForegroundColor = ConsoleColor.Yellow; + + Console.Write(grid.Find(x, y).Value.Blocking ? "1" : "0"); + Console.ResetColor(); + } + + Console.WriteLine(); + } + } + + if (path != null) + { + Logger.c.Log($"Path found! Length: {path.Count}"); + + if (verbose) + foreach (var node in path) + Logger.c.Log(node.ToString()); + } + else + Logger.c.Warn("No path found!"); + + stopwatch.Stop(); + + Logger.c.Log("----------------------------------------"); + Logger.c.Log($"PROCESSING TIME: {stopwatch.Elapsed}"); + } }