Started working on path finding in HexGrid.cs.

GodotComponentTest.csproj

@@ -12,6 +12,8 @@
 
 	<ItemGroup>
 	  <PackageReference Include="Chickensoft.GoDotTest" Version="1.0.0" />
+	  <PackageReference Include="OptimizedPriorityQueue" Version="5.1.0" />
+	  <PackageReference Include="xunit" Version="2.5.0" />
 	</ItemGroup>
 
 	<ItemGroup>

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)(

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<Godot.Vector2, Godot.Vector3>; 
 
 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<Vector2, float> Obstacles = new System.Collections.Generic.Dictionary<Vector2, float>();
+
+    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<HexCell> FindPath(HexCell startHex, HexCell goalHex)
+    {
+        Vector2 startAxialCoords = startHex.AxialCoords;
+        Vector2 goalAxialCoords = goalHex.AxialCoords;
+
+        SimplePriorityQueue<Vector2, float> frontier = new SimplePriorityQueue<Vector2, float>();
+        frontier.Enqueue(startHex.AxialCoords, 0);
+        System.Collections.Generic.Dictionary<Vector2, Vector2> cameFrom = new System.Collections.Generic.Dictionary<Vector2, Vector2>();
+        System.Collections.Generic.Dictionary<Vector2, float> costSoFar = new System.Collections.Generic.Dictionary<Vector2, float>();
+        
+        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<HexCell>();
+        }
+
+        List<HexCell> result = new List<HexCell>();
+        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;
+    }
 }

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<HexCell> expected, List<HexCell> 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<HexCell> expectedPath = new List<HexCell>()
+        {
+            _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<HexCell> expectedPath = new List<HexCell>()
+        {
+            _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<HexCell> expectedPath = new List<HexCell>()
+        {
+            _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<HexCell> expectedPath = new List<HexCell>()
+        {
+            _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<HexCell> expectedPath = new List<HexCell>()
+        {
+            _positionA,
+            new HexCell(new Vector2(1, 1)),
+            _positionG
+        };
+
+        ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last()));
+    }
+
+    [Test]
+    public void TestRoughTerrain()
+    {
+        List<HexCell> shortPath = new List<HexCell>()
+        {
+            _positionA,
+            new HexCell(new Vector2(3, 0)),
+            new HexCell(new Vector2(4, 0)),
+            _positionD,
+            new HexCell(new Vector2(5, 1)),
+            _positionB,
+        };
+        
+        List<HexCell> longPath = new List<HexCell>()
+        {
+            _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()));
+    }
+}