From f56a5bcaa77cef7dbd417a605f6abf834a026e77 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Sat, 15 Jul 2023 21:50:38 +0200 Subject: [PATCH] Started working on path finding in HexGrid.cs. --- GodotComponentTest.csproj | 2 + HexCell.cs | 33 +++- HexGrid.cs | 209 ++++++++++++++++++++++ tests/HexGridPathFindingTests.cs | 297 +++++++++++++++++++++++++++++++ 4 files changed, 540 insertions(+), 1 deletion(-) create mode 100644 tests/HexGridPathFindingTests.cs diff --git a/GodotComponentTest.csproj b/GodotComponentTest.csproj index 08b6e82..a7bd7b6 100644 --- a/GodotComponentTest.csproj +++ b/GodotComponentTest.csproj @@ -12,6 +12,8 @@ + + diff --git a/HexCell.cs b/HexCell.cs index 50cb04d..34a2a66 100644 --- a/HexCell.cs +++ b/HexCell.cs @@ -26,6 +26,24 @@ public class HexCell : Resource CubeCoords = RoundCoords(new Vector3(cubeX, cubeY, cubeZ)); } + public static bool operator==(HexCell cellA, HexCell cellB) + { + if (cellA == null && cellB == null) + { + return true; + } + return cellA.AxialCoords == cellB.AxialCoords; + } + + public static bool operator!=(HexCell cellA, HexCell cellB) + { + if (cellA == null || cellB == null) + { + return false; + } + return cellA.AxialCoords != cellB.AxialCoords; + } + public HexCell(Vector3 cubeCoords) { CubeCoords = cubeCoords; @@ -137,6 +155,19 @@ public class HexCell : Resource return new HexCell(this.CubeCoords + dir); } + public HexCell[] GetAllAdjacent() + { + return new[] + { + GetAdjacent(DIR_NE), + GetAdjacent(DIR_SE), + GetAdjacent(DIR_S), + GetAdjacent(DIR_SW), + GetAdjacent(DIR_NW), + GetAdjacent(DIR_N) + }; + } + public int DistanceTo(HexCell target) { return (int)( @@ -159,7 +190,7 @@ public class HexCell : Resource path[dist] = new HexCell(); path[dist].CubeCoords = CubeCoords.LinearInterpolate(nudgedTarget.CubeCoords, (float)dist / steps); } - + path[steps] = target; return path; diff --git a/HexGrid.cs b/HexGrid.cs index bac7f24..439001e 100644 --- a/HexGrid.cs +++ b/HexGrid.cs @@ -1,13 +1,33 @@ using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; +using System.Threading; using Godot; +using Godot.Collections; +using Priority_Queue; +using Array = Godot.Collections.Array; +using AxialCoordDirectionPair = System.Tuple; public class HexGrid : Resource { + private Vector2 _baseHexSize = new Vector2(1, Mathf.Sqrt(3) / 2); private Vector2 _hexSize = new Vector2(1, Mathf.Sqrt(3) / 2); private Vector2 _hexScale = new Vector2(1, 1); private Godot.Transform2D _hexTransform; private Godot.Transform2D _hexTransformInv; + private HexCell _minCoords = new HexCell(); + private HexCell _maxCoords = new HexCell(); + private Rect2 _bounds = new Rect2(); + + public System.Collections.Generic.Dictionary Obstacles = new System.Collections.Generic.Dictionary(); + + public System.Collections.Generic.Dictionary<(Vector2, Vector3), float> Barriers = + new System.Collections.Generic.Dictionary<(Vector2, Vector3), float>(); + + + public float PathCostDefault = 1; public Vector2 HexSize { @@ -60,4 +80,193 @@ public class HexGrid : Resource HexCell result = new HexCell(_hexTransformInv * planeCoord); return result; } + + public void SetBounds(Vector2 minAxial, Vector2 maxAxial) + { + SetBounds(new HexCell(minAxial), new HexCell(maxAxial)); + } + + public void SetBounds(HexCell minCell, HexCell maxCell) + { + _minCoords = minCell; + _maxCoords = maxCell; + _bounds = new Rect2(_minCoords.AxialCoords, (_maxCoords.AxialCoords - _minCoords.AxialCoords) + Vector2.One); + } + + public void AddObstacle(Vector2 axialCoords, float cost = 0) + { + AddObstacle(new HexCell(axialCoords), cost); + } + + public void AddObstacle(HexCell cell, float cost = 0) + { + if (Obstacles.ContainsKey(cell.AxialCoords)) + { + Obstacles[cell.AxialCoords] = cost; + } + else + { + Obstacles.Add(cell.AxialCoords, cost); + } + } + + public void RemoveObstacle(HexCell cell) + { + Obstacles.Remove(cell.AxialCoords); + } + + + public void AddBarrier(Vector2 axialCoords, Vector3 directionCube, float cost = 0) + { + AddBarrier(new HexCell(axialCoords), directionCube, cost); + } + + public void AddBarrier(HexCell cell, Vector3 directionCube, float cost = 0) + { + AxialCoordDirectionPair barrierKey = new AxialCoordDirectionPair(cell.AxialCoords, directionCube); + Barriers.Add((cell.AxialCoords, directionCube), cost); + } + + public void RemoveBarrier(HexCell cell, Vector3 directionCube) + { + AxialCoordDirectionPair barrierKey = new AxialCoordDirectionPair(cell.AxialCoords, directionCube); + if (Barriers.ContainsKey((cell.AxialCoords, directionCube))) + { + Barriers.Remove((cell.AxialCoords, directionCube)); + } + } + + public float GetHexCost(HexCell cell) + { + return GetHexCost(cell.AxialCoords); + } + + public float GetHexCost(Vector2 axialCoords) + { + if (!_bounds.HasPoint(axialCoords)) + { + return 0; + } + + float value; + return Obstacles.TryGetValue(axialCoords, out value) ? value : PathCostDefault; + } + + public float GetMoveCost(Vector2 axialCoords, Vector3 directionCube) + { + HexCell startCell = new HexCell(axialCoords); + HexCell targetCell = new HexCell(startCell.CubeCoords + directionCube); + + float cost = GetHexCost(axialCoords); + if (cost == 0) + { + return 0; + } + + cost = GetHexCost(targetCell.AxialCoords); + if (cost == 0) + { + return 0; + } + + float barrierCost; + AxialCoordDirectionPair barrierKey = new AxialCoordDirectionPair(axialCoords, directionCube); + if (Barriers.ContainsKey((axialCoords, directionCube))) + { + barrierCost = Barriers[(axialCoords, directionCube)]; + if (barrierCost == 0) + { + return 0; + } + + cost += barrierCost; + } + + AxialCoordDirectionPair reversedBarrierKey = new AxialCoordDirectionPair(targetCell.AxialCoords, -directionCube); + if (Barriers.ContainsKey((targetCell.AxialCoords, -directionCube))) + { + barrierCost = Barriers[(targetCell.AxialCoords, -directionCube)]; + if (barrierCost == 0) + { + return 0; + } + + cost += barrierCost; + } + + return cost; + } + + public List FindPath(HexCell startHex, HexCell goalHex) + { + Vector2 startAxialCoords = startHex.AxialCoords; + Vector2 goalAxialCoords = goalHex.AxialCoords; + + SimplePriorityQueue frontier = new SimplePriorityQueue(); + frontier.Enqueue(startHex.AxialCoords, 0); + System.Collections.Generic.Dictionary cameFrom = new System.Collections.Generic.Dictionary(); + System.Collections.Generic.Dictionary costSoFar = new System.Collections.Generic.Dictionary(); + + cameFrom.Add(startHex.AxialCoords, startHex.AxialCoords); + costSoFar.Add(startHex.AxialCoords, 0); + + while (frontier.Any()) + { + HexCell currentHex = new HexCell(frontier.Dequeue()); + Vector2 currentAxial = currentHex.AxialCoords; + if (currentHex == goalHex) + { + break; + } + + foreach (HexCell nextHex in currentHex.GetAllAdjacent()) + { + Vector2 nextAxial = nextHex.AxialCoords; + float nextCost = GetMoveCost(currentAxial, new HexCell(nextAxial - currentHex.AxialCoords).CubeCoords); + + if ((nextHex == goalHex) && (GetHexCost(nextAxial) == 0)) + { + // Goal ist an obstacle + cameFrom[nextHex.AxialCoords] = currentHex.AxialCoords; + frontier.Clear(); + break; + } + + if (nextCost == 0) + { + continue; + } + + nextCost += costSoFar[currentHex.AxialCoords]; + if (!costSoFar.ContainsKey(nextHex.AxialCoords) || nextCost < costSoFar[nextHex.AxialCoords]) + { + costSoFar[nextHex.AxialCoords] = nextCost; + float priority = nextCost + nextHex.DistanceTo(goalHex); + + frontier.Enqueue(nextHex.AxialCoords, priority); + cameFrom[nextHex.AxialCoords] = currentHex.AxialCoords; + } + } + } + + if (!cameFrom.ContainsKey(goalHex.AxialCoords)) + { + return new List(); + } + + List result = new List(); + if (GetHexCost(goalAxialCoords) != 0) + { + result.Add(goalHex); + } + + HexCell pathHex = goalHex; + while (pathHex != startHex) + { + pathHex = new HexCell(cameFrom[pathHex.AxialCoords]); + result.Insert(0, pathHex); + } + + return result; + } } \ No newline at end of file diff --git a/tests/HexGridPathFindingTests.cs b/tests/HexGridPathFindingTests.cs new file mode 100644 index 0000000..22b40c6 --- /dev/null +++ b/tests/HexGridPathFindingTests.cs @@ -0,0 +1,297 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using Godot; +using Godot.Collections; +using GoDotTest; +using Xunit; +using Array = System.Array; + +//using GodotXUnitApi; +//using Xunit; + +namespace GodotComponentTest.tests; + +public class HexGridPathFindingTests : TestClass +{ + private HexGrid _hexGrid; + private HexCell _hexCell; + + private HexCell _positionA = new HexCell(new Vector2(2, 0)); + private HexCell _positionB = new HexCell(new Vector2(4, 2)); + private HexCell _positionC = new HexCell(new Vector2(7, 0)); + private HexCell _positionD = new HexCell(new Vector2(5, 0)); + private HexCell _positionE = new HexCell(new Vector2(2, 2)); + private HexCell _positionF = new HexCell(new Vector2(1, 3)); + private HexCell _positionG = new HexCell(new Vector2(1, 0)); + + private Vector2[] _obstacles = + { + new Vector2(2, 1), + new Vector2(3, 1), + new Vector2(4, 1), + new Vector2(1, 2), + new Vector2(3, 2), + new Vector2(1, 3), + new Vector2(2, 3), + }; + + public HexGridPathFindingTests(Node testScene) : base(testScene) + { + } + + [Setup] + public void Setup() + { + _hexGrid = new HexGrid(); + _hexCell = new HexCell(); + + _hexGrid.SetBounds(new Vector2(0, 0), new Vector2(7, 4)); + foreach (Vector2 obstacle in _obstacles) + { + _hexGrid.AddObstacle(new HexCell(obstacle)); + } + } + + [Test] + public void TestBounds() + { + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetHexCost(new Vector2(0, 0))); + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetHexCost(new Vector2(0, 4))); + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetHexCost(new Vector2(7, 0))); + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetHexCost(new Vector2(7, 4))); + + Assert.Equal(0, _hexGrid.GetHexCost(new Vector2(8, 2))); + Assert.Equal(0, _hexGrid.GetHexCost(new Vector2(6, 5))); + Assert.Equal(0, _hexGrid.GetHexCost(new Vector2(-1, 2))); + Assert.Equal(0, _hexGrid.GetHexCost(new Vector2(6, -1))); + } + + [Test] + public void TestNegativeBounds() + { + HexGrid grid = new HexGrid(); + grid.SetBounds(new Vector2(-5, -5), new Vector2(-2, -2)); + + Assert.Equal(grid.PathCostDefault, grid.GetHexCost(new Vector2(-2, -2))); + Assert.Equal(grid.PathCostDefault, grid.GetHexCost(new Vector2(-5, -5))); + Assert.Equal(0, grid.GetHexCost(new Vector2(0, 0))); + Assert.Equal(0, grid.GetHexCost(new Vector2(-6, -3))); + Assert.Equal(0, grid.GetHexCost(new Vector2(-3, -1))); + } + + [Test] + public void TestNegativeBoundsAlt() + { + HexGrid grid = new HexGrid(); + grid.SetBounds(new Vector2(-3, -3), new Vector2(2, 2)); + + Assert.Equal(grid.PathCostDefault, grid.GetHexCost(new Vector2(-3, -3))); + Assert.Equal(grid.PathCostDefault, grid.GetHexCost(new Vector2(2, 2))); + Assert.Equal(grid.PathCostDefault, grid.GetHexCost(new Vector2(0, 0))); + Assert.Equal(0, grid.GetHexCost(new Vector2(-4, 0))); + Assert.Equal(0, grid.GetHexCost(new Vector2(0, 3))); + } + + [Test] + public void TestGridObstacles() + { + Assert.Equal(_obstacles.Length, _hexGrid.Obstacles.Count); + + // Adding an obstacle + _hexGrid.AddObstacle(new HexCell(new Vector2(0, 0))); + Assert.Equal(0, _hexGrid.Obstacles[new Vector2(0, 0)]); + + // Replacing obstacle + _hexGrid.AddObstacle(new HexCell(new Vector2(0, 0)), 2); + Assert.Equal(2, _hexGrid.Obstacles[new Vector2(0, 0)]); + + // Removing obstacle + _hexGrid.RemoveObstacle(new HexCell(new Vector2(0, 0))); + Assert.DoesNotContain(new Vector2(0, 0), _hexGrid.Obstacles); + + // Removing invalid does not cause error + _hexGrid.RemoveObstacle(new HexCell(new Vector2(0, 0))); + } + + [Test] + public void TestHexCost() + { + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetHexCost(new Vector2(1, 1))); + Assert.Equal(0, _hexGrid.GetHexCost(new HexCell(new Vector3 (2, 1, -3)))); + + _hexGrid.AddObstacle(new HexCell(1, 1), 1.337f); + Assert.Equal(1.337f, _hexGrid.GetHexCost(new Vector2(1,1))); + } + + [Test] + public void TestMoveCost() + { + Assert.Equal(_hexGrid.PathCostDefault, _hexGrid.GetMoveCost(new Vector2(0, 0), HexCell.DIR_N)); + } + + [Test] + public void TestMovieCostCumulative() + { + _hexGrid.AddObstacle(new Vector2(0, 0), 1); + _hexGrid.AddObstacle(new Vector2(0, 1), 2); + _hexGrid.AddBarrier(new Vector2(0, 0), HexCell.DIR_N, 4); + Assert.Single(_hexGrid.Barriers); + _hexGrid.AddBarrier(new Vector2(0, 1), HexCell.DIR_S, 8); + Assert.Equal(2, _hexGrid.Barriers.Count); + + Assert.Equal(14, _hexGrid.GetMoveCost(new Vector2(0, 0), HexCell.DIR_N)); + } + + void ComparePath(List expected, List path) + { + Assert.Equal(expected.Count, path.Count()); + + foreach (int i in Enumerable.Range(0, Math.Min(expected.Count, path.Count()))) + { + HexCell pathCell = path[i]; + HexCell expectedCell = expected[i]; + Assert.Equal(expectedCell.AxialCoords, pathCell.AxialCoords); + } + } + + [Test] + public void TestStraightLine() + { + List expectedPath = new List() + { + _positionA, + new HexCell(new Vector2(3, 0)), + new HexCell(new Vector2(4, 0)), + new HexCell(new Vector2(5, 0)), + new HexCell(new Vector2(6, 0)), + _positionC + }; + + ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last())); + } + + [Test] + public void TestWonkyLine() + { + List expectedPath = new List() + { + _positionB, + new HexCell(new Vector2(5, 1)), + new HexCell(new Vector2(5, 2)), + new HexCell(new Vector2(6, 0)), + new HexCell(new Vector2(6, 1)), + _positionC + }; + + ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last())); + } + + [Test] + public void TestObstacle() + { + List expectedPath = new List() + { + _positionA, + new HexCell(new Vector2(3, 0)), + new HexCell(new Vector2(4, 0)), + new HexCell(new Vector2(5, 0)), + new HexCell(new Vector2(5, 1)), + _positionB + }; + + ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last())); + } + + [Test] + public void TestWalls() + { + Vector3[] walls = + { + HexCell.DIR_N, + HexCell.DIR_NE, + HexCell.DIR_SE, + HexCell.DIR_S, + // DIR_SE is open + HexCell.DIR_NW + }; + + foreach (Vector3 wall in walls) + { + _hexGrid.AddBarrier(_positionG, wall); + } + + List expectedPath = new List() + { + _positionA, + new HexCell(new Vector2(1, 1)), + new HexCell(new Vector2(0, 1)), + new HexCell(new Vector2(0, 0)), + _positionG + }; + + ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last())); + } + + [Test] + public void TestSlopes() + { + _hexGrid.AddBarrier(_positionG, HexCell.DIR_NE, 3); + _hexGrid.AddBarrier(_positionG, HexCell.DIR_N, _hexGrid.PathCostDefault - 0.1f); + + List expectedPath = new List() + { + _positionA, + new HexCell(new Vector2(1, 1)), + _positionG + }; + + ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last())); + } + + [Test] + public void TestRoughTerrain() + { + List shortPath = new List() + { + _positionA, + new HexCell(new Vector2(3, 0)), + new HexCell(new Vector2(4, 0)), + _positionD, + new HexCell(new Vector2(5, 1)), + _positionB, + }; + + List longPath = new List() + { + _positionA, + new HexCell(new Vector2(1, 1)), + new HexCell(new Vector2(0, 2)), + new HexCell(new Vector2(0, 3)), + new HexCell(new Vector2(0, 4)), + new HexCell(new Vector2(1, 4)), + new HexCell(new Vector2(2, 4)), + new HexCell(new Vector2(3, 3)), + _positionB, + }; + + _hexGrid.PathCostDefault = 1f; + ComparePath(shortPath, _hexGrid.FindPath(shortPath.First(), shortPath.Last())); + + _hexGrid.PathCostDefault = 2f; + ComparePath(shortPath, _hexGrid.FindPath(shortPath.First(), shortPath.Last())); + + _hexGrid.PathCostDefault = 3.9f; + ComparePath(shortPath, _hexGrid.FindPath(shortPath.First(), shortPath.Last())); + + _hexGrid.PathCostDefault = 4.1f; + ComparePath(longPath, _hexGrid.FindPath(longPath.First(), longPath.Last())); + + _hexGrid.PathCostDefault = 41f; + ComparePath(longPath, _hexGrid.FindPath(longPath.First(), longPath.Last())); + + _hexGrid.PathCostDefault = 0f; + ComparePath(longPath, _hexGrid.FindPath(longPath.First(), longPath.Last())); + } +} \ No newline at end of file