Added NavigationComponent::CheckSweptTriangleCellCollision().

HexCell.cs

@@ -1,21 +1,75 @@
 using System;
-using System.Diagnostics;
 using System.Linq;
 using Godot;
-using Array = Godot.Collections.Array;
+using GodotComponentTest.utils;
 
-public class HexCell : Resource
+public class HexCell : IEquatable<HexCell>
 {
-    public static readonly Vector2 size = new Vector2(1, Mathf.Sqrt(3) / 2);
-    public static readonly Vector3 DIR_N = new Vector3(0, 1, -1);
-    public static readonly Vector3 DIR_NE = new Vector3(1, 0, -1);
-    public static readonly Vector3 DIR_SE = new Vector3(1, -1, 0);
-    public static readonly Vector3 DIR_S = new Vector3(0, -1, 1);
-    public static readonly Vector3 DIR_SW = new Vector3(-1, 0, 1);
-    public static readonly Vector3 DIR_NW = new Vector3(-1, 1, 0);
+    public override bool Equals(object obj)
+    {
+        if (ReferenceEquals(null, obj)) return false;
+        if (ReferenceEquals(this, obj)) return true;
+        if (obj.GetType() != this.GetType()) return false;
+        return Equals((HexCell)obj);
+    }
 
-    public static readonly Array DIR_ALL = new Array()
-        { DIR_N, DIR_NE, DIR_SE, DIR_S, DIR_SW, DIR_NW };
+    public override int GetHashCode()
+    {
+        return _cubeCoords.GetHashCode();
+    }
+
+    public static readonly Vector2 Size = new(1, Mathf.Sqrt(3) / 2);
+    private const float Width = 2;
+    private static readonly float Height = Mathf.Sqrt(3);
+
+    public static readonly Vector3 DIR_N = new(0, 1, -1);
+    public static readonly Vector3 DIR_NE = new(1, 0, -1);
+    public static readonly Vector3 DIR_SE = new(1, -1, 0);
+    public static readonly Vector3 DIR_S = new(0, -1, 1);
+    public static readonly Vector3 DIR_SW = new(-1, 0, 1);
+    public static readonly Vector3 DIR_NW = new(-1, 1, 0);
+
+    public static readonly Vector3[] NeighborDirections =
+    {
+        DIR_N,
+        DIR_NW,
+        DIR_SW,
+        DIR_S,
+        DIR_SE,
+        DIR_NE
+    };
+
+    private static readonly Vector2 CornerNW = new Vector2(-Width / 4, -Height / 2);
+    private static readonly Vector2 CornerNE = new Vector2(Width / 4, -Height / 2);
+    private static readonly Vector2 CornerE = new Vector2(Width / 2, 0);
+    private static readonly Vector2 CornerSE = new Vector2(Width / 4, Height / 2);
+    private static readonly Vector2 CornerSW = new Vector2(-Width / 4, Height / 2);
+    private static readonly Vector2 CornerW = new Vector2(-Width / 2, 0);
+    private static readonly Vector2 PlaneNormalN = new Vector2(0, 1);
+
+    private static readonly Vector2 PlaneNormalNE =
+        -new Vector2(Mathf.Cos(Mathf.Deg2Rad(30)), -Mathf.Sin(Mathf.Deg2Rad(30)));
+
+    private static readonly Vector2 PlaneNormalSE =
+        -new Vector2(Mathf.Cos(Mathf.Deg2Rad(-30)), -Mathf.Sin(Mathf.Deg2Rad(-30)));
+
+    private static readonly Vector2 PlaneNormalS = new Vector2(0, -1);
+
+    private static readonly Vector2 PlaneNormalSW =
+        new Vector2(Mathf.Cos(Mathf.Deg2Rad(30)), -Mathf.Sin(Mathf.Deg2Rad(30)));
+
+    private static readonly Vector2 PlaneNormalNW =
+        new Vector2(Mathf.Cos(Mathf.Deg2Rad(-30)), -Mathf.Sin(Mathf.Deg2Rad(-30)));
+
+    private static readonly Plane2D[] BoundaryPlanes =
+    {
+        new Plane2D(CornerNE, PlaneNormalN),
+        new Plane2D(CornerNW, PlaneNormalNW),
+        new Plane2D(CornerW, PlaneNormalSW),
+        new Plane2D(CornerSW, PlaneNormalS),
+        new Plane2D(CornerSE, PlaneNormalSE),
+        new Plane2D(CornerE, PlaneNormalNE),
+    };
 
     public HexCell()
     {
@@ -26,16 +80,26 @@ public class HexCell : Resource
         CubeCoords = RoundCoords(new Vector3(cubeX, cubeY, cubeZ));
     }
 
-    public static bool operator==(HexCell cellA, HexCell cellB)
+    public virtual bool Equals(HexCell other)
     {
-        return cellA.AxialCoords == cellB.AxialCoords;
+        if (other == null)
+        {
+            return false;
+        }
+
+        return CubeCoords == other.CubeCoords;
     }
-    
-    public static bool operator!=(HexCell cellA, HexCell cellB)
+
+    public static bool operator ==(HexCell cellA, HexCell cellB)
     {
-        return cellA.AxialCoords != cellB.AxialCoords;
+        return Equals(cellA, cellB);
     }
-    
+
+    public static bool operator !=(HexCell cellA, HexCell cellB)
+    {
+        return !(cellA == cellB);
+    }
+
     public HexCell(Vector3 cubeCoords)
     {
         CubeCoords = cubeCoords;
@@ -93,16 +157,16 @@ public class HexCell : Resource
         {
             int x = (int)CubeCoords.x;
             int y = (int)CubeCoords.y;
-            int off_y = y + (x - (x & 1)) / 2;
-            return new Vector2(x, off_y);
+            int offY = y + (x - (x & 1)) / 2;
+            return new Vector2(x, offY);
         }
 
         set
         {
             int x = (int)value.x;
             int y = (int)value.y;
-            int cube_y = y - (x - (x & 1)) / 2;
-            AxialCoords = new Vector2(x, cube_y);
+            int cubeY = y - (x - (x & 1)) / 2;
+            AxialCoords = new Vector2(x, cubeY);
         }
     }
 
@@ -151,10 +215,10 @@ public class HexCell : Resource
     {
         return new[]
         {
-            GetAdjacent(DIR_NE), 
-            GetAdjacent(DIR_SE), 
-            GetAdjacent(DIR_S), 
-            GetAdjacent(DIR_SW), 
+            GetAdjacent(DIR_NE),
+            GetAdjacent(DIR_SE),
+            GetAdjacent(DIR_S),
+            GetAdjacent(DIR_SW),
             GetAdjacent(DIR_NW),
             GetAdjacent(DIR_N)
         };
@@ -187,4 +251,33 @@ public class HexCell : Resource
 
         return path;
     }
+
+    public void QueryClosestCellBoundary(Vector2 pointLocal, Vector2 dir, out int neighbourIndex, out float distance)
+    {
+        distance = Single.PositiveInfinity;
+        neighbourIndex = 0;
+        foreach (int i in Enumerable.Range(0, 6))
+        {
+            if (BoundaryPlanes[i].Normal.Dot(dir) >= Plane2D.DistancePrecision)
+            {
+                continue;
+            }
+
+            float planeDistance = BoundaryPlanes[i].DistanceToLineSegment(pointLocal, dir);
+            if (planeDistance > Single.NegativeInfinity && planeDistance < distance)
+            {
+                distance = planeDistance;
+                neighbourIndex = i;
+            }
+        }
+    }
+
+    public HexCell NextCellAlongLine(Vector2 pointLocal, Vector2 dir)
+    {
+        int planeIndex;
+
+        QueryClosestCellBoundary(pointLocal, dir, out planeIndex, out _);
+
+        return GetAdjacent(NeighborDirections[planeIndex]);
+    }
 }

HexGrid.cs

@@ -1,30 +1,24 @@
-using System;
 using System.Collections.Generic;
-using System.Collections.ObjectModel;
 using System.Linq;
-using System.Threading;
 using Godot;
-using Godot.Collections;
+using GodotComponentTest.utils;
 using Priority_Queue;
-using Array = Godot.Collections.Array;
-using AxialCoordDirectionPair = System.Tuple<Godot.Vector2, Godot.Vector3>; 
+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>();
+    private Vector2 _baseHexSize = new(1, Mathf.Sqrt(3) / 2);
+    private Vector2 _hexSize = new(1, Mathf.Sqrt(3) / 2);
+    private Vector2 _hexScale = new(1, 1);
+    private Transform2D _hexTransform;
+    private Transform2D _hexTransformInv;
+    private HexCell _minCoords = new();
+    private HexCell _maxCoords = new();
+    private Rect2 _boundsAxialCoords;
 
-    public System.Collections.Generic.Dictionary<(Vector2, Vector3), float> Barriers =
-        new System.Collections.Generic.Dictionary<(Vector2, Vector3), float>();
+    public Dictionary<Vector2, float> Obstacles = new();
+
+    public Dictionary<(Vector2, Vector3), float> Barriers = new();
 
 
     public float PathCostDefault = 1;
@@ -82,6 +76,11 @@ public class HexGrid : Resource
         return result;
     }
 
+    public void SetBoundsOffset(Vector2 minOffset, Vector2 maxOffset)
+    {
+        SetBounds(HexCell.FromOffsetCoords(minOffset), HexCell.FromOffsetCoords(maxOffset));
+    }
+
     public void SetBounds(Vector2 minAxial, Vector2 maxAxial)
     {
         SetBounds(new HexCell(minAxial), new HexCell(maxAxial));
@@ -91,32 +90,26 @@ public class HexGrid : Resource
     {
         _minCoords = minCell;
         _maxCoords = maxCell;
-        _bounds = new Rect2(_minCoords.OffsetCoords, (_maxCoords.OffsetCoords - _minCoords.OffsetCoords) + Vector2.One); 
+        _boundsAxialCoords = 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);
-        }
+        Obstacles[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);
@@ -124,13 +117,11 @@ public class HexGrid : Resource
 
     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));
@@ -144,8 +135,7 @@ public class HexGrid : Resource
 
     public float GetHexCost(Vector2 axialCoords)
     {
-        HexCell cell = new HexCell(axialCoords);
-        if (!_bounds.HasPoint(cell.OffsetCoords))
+        if (!_boundsAxialCoords.HasPoint(axialCoords))
         {
             return 0;
         }
@@ -172,8 +162,7 @@ public class HexGrid : Resource
         }
 
         float barrierCost;
-        AxialCoordDirectionPair barrierKey = new AxialCoordDirectionPair(axialCoords, directionCube);
-       if (Barriers.ContainsKey((axialCoords, directionCube)))
+        if (Barriers.ContainsKey((axialCoords, directionCube)))
         {
             barrierCost = Barriers[(axialCoords, directionCube)];
             if (barrierCost == 0)
@@ -184,7 +173,6 @@ public class HexGrid : Resource
             cost += barrierCost;
         }
 
-        AxialCoordDirectionPair reversedBarrierKey = new AxialCoordDirectionPair(targetCell.AxialCoords, -directionCube);
         if (Barriers.ContainsKey((targetCell.AxialCoords, -directionCube)))
         {
             barrierCost = Barriers[(targetCell.AxialCoords, -directionCube)];
@@ -195,11 +183,11 @@ public class HexGrid : Resource
 
             cost += barrierCost;
         }
-        
+
         return cost;
     }
 
-    
+
     public HexCell GetClosestWalkableCell(HexCell fromCell, HexCell toCell)
     {
         if (GetHexCost(toCell) == 0)
@@ -218,27 +206,29 @@ public class HexGrid : Resource
 
         return toCell;
     }
-    
+
     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>();
-        
+        Dictionary<Vector2, Vector2> cameFrom =
+            new Dictionary<Vector2, Vector2>();
+        Dictionary<Vector2, float> costSoFar =
+            new Dictionary<Vector2, float>();
+
         cameFrom.Add(startHex.AxialCoords, startHex.AxialCoords);
         costSoFar.Add(startHex.AxialCoords, 0);
 
         FindPathCheckedCells = 0;
-        
+
         while (frontier.Any())
         {
             FindPathCheckedCells++;
             HexCell currentHex = new HexCell(frontier.Dequeue());
             Vector2 currentAxial = currentHex.AxialCoords;
+
             if (currentHex == goalHex)
             {
                 break;
@@ -275,13 +265,14 @@ public class HexGrid : Resource
         }
 
         // GD.Print("Checked Cell Count: " + FindPathCheckedCells);
-        
-        if (!cameFrom.ContainsKey(goalHex.AxialCoords))
-        {
-            return new List<HexCell>();
-        }
 
         List<HexCell> result = new List<HexCell>();
+        if (!cameFrom.ContainsKey(goalHex.AxialCoords))
+        {
+            GD.Print("Failed to find path from " + startHex + " to " + goalHex);
+            return result;
+        }
+
         if (GetHexCost(goalAxialCoords) != 0)
         {
             result.Add(goalHex);
@@ -296,4 +287,35 @@ public class HexGrid : Resource
 
         return result;
     }
+
+
+    public List<HexCell> GetCellsForLine(Vector2 fromPlane, Vector2 toPlane)
+    {
+        List<HexCell> result = new List<HexCell>();
+
+        HexCell toCell = GetHexAt(toPlane);
+        Vector2 direction = (toPlane - fromPlane).Normalized();
+
+        Vector2 currentPointPlane = fromPlane;
+        HexCell currentCell = GetHexAt(currentPointPlane);
+
+        do
+        {
+            result.Add(currentCell);
+            GetHexCenter(currentCell);
+            Vector2 currentPointLocal = currentPointPlane - GetHexCenter(currentCell);
+
+            int neighbourIndex;
+            float boundaryPlaneDistance;
+            currentCell.QueryClosestCellBoundary(currentPointLocal, direction, out neighbourIndex,
+                out boundaryPlaneDistance);
+            
+            currentCell = currentCell.GetAdjacent(HexCell.NeighborDirections[neighbourIndex]);
+            currentPointPlane += direction * boundaryPlaneDistance * 1.001f;
+        } while (currentCell != toCell);
+
+        result.Add(currentCell);
+
+        return result;
+    }
 }

components/NavigationComponent.cs

@@ -2,12 +2,10 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Linq;
-using System.Numerics;
 using Godot;
 using GodotComponentTest.utils;
 using Vector2 = Godot.Vector2;
 using Vector3 = Godot.Vector3;
-using GoDotLog;
 
 /// <summary>
 /// </summary>
@@ -24,7 +22,7 @@ public class NavigationComponent : Spatial
 
         public Vector3 WorldPosition = Vector3.Zero;
         public Quat WorldOrientation = Quat.Identity;
-        public NavigationFlags Flags = NavigationFlags.Position | NavigationFlags.Orientation;
+        public readonly NavigationFlags Flags;
 
         public NavigationPoint(Vector3 worldPosition)
         {
@@ -49,22 +47,22 @@ public class NavigationComponent : Spatial
         {
             bool goalReached = false;
             float positionErrorSquared = (worldTransform.origin - WorldPosition).LengthSquared();
-            Quat own_orientation = worldTransform.basis.Quat();
+            worldTransform.basis.Quat();
             float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
 
-            if (Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
-                && Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)
+            if (Flags.HasFlag(NavigationFlags.Position)
+                && Flags.HasFlag(NavigationFlags.Orientation)
                 && positionErrorSquared < Globals.EpsPositionSquared
                 && orientationError < Globals.EpsRadians)
             {
                 goalReached = true;
             }
-            else if (Flags == NavigationPoint.NavigationFlags.Position &&
+            else if (Flags == NavigationFlags.Position &&
                      positionErrorSquared < Globals.EpsPositionSquared)
             {
                 goalReached = true;
             }
-            else if (Flags == NavigationPoint.NavigationFlags.Orientation &&
+            else if (Flags == NavigationFlags.Orientation &&
                      orientationError < Globals.EpsRadians)
             {
                 goalReached = true;
@@ -254,6 +252,44 @@ public class NavigationComponent : Spatial
         return false;
     }
 
+    
+    bool CheckSweptTriangleCellCollision(Vector3 startWorld, Vector3 endWorld, float radius)
+    {
+        Vector2 startPlane = new Vector2(startWorld.x, startWorld.z);
+        Vector2 endPlane = new Vector2(endWorld.x, endWorld.z);
+        Vector2 directionPlane = (endPlane - startPlane).Normalized();
+        Vector2 sidePlane = directionPlane.Rotated(Mathf.Pi * 0.5f);
+
+        List<HexCell> cells = TileWorld.HexGrid.GetCellsForLine(startPlane + directionPlane * radius, endPlane + directionPlane * radius);
+        foreach (HexCell cell in cells)
+        {
+            if (TileWorld.HexGrid.GetHexCost(cell) == 0)
+            {
+                return true;
+            }
+        }
+        
+        cells = TileWorld.HexGrid.GetCellsForLine(startPlane + sidePlane * radius, endPlane + sidePlane * radius);
+        foreach (HexCell cell in cells)
+        {
+            if (TileWorld.HexGrid.GetHexCost(cell) == 0)
+            {
+                return true;
+            }
+        }
+        
+        cells = TileWorld.HexGrid.GetCellsForLine(startPlane - sidePlane * radius, endPlane - sidePlane * radius);
+        foreach (HexCell cell in cells)
+        {
+            if (TileWorld.HexGrid.GetHexCost(cell) == 0)
+            {
+                return true;
+            }
+        }
+        
+        return false;
+    }
+    
     public List<NavigationPoint> SmoothPath(KinematicBody body, List<NavigationPoint> navigationPoints)
     {
         if (navigationPoints.Count <= 2)
@@ -273,7 +309,7 @@ public class NavigationComponent : Spatial
         {
             Vector3 endPoint = navigationPoints[endIndex].WorldPosition;
 
-            if (HasPathCollision(body, startPoint, endPoint))
+            if (CheckSweptTriangleCellCollision(startPoint, endPoint, 0.27f))
             {
                 if (endIndex - startIndex == 1)
                 {
@@ -434,7 +470,7 @@ public class NavigationComponent : Spatial
         previousPoint = parentNode.GlobalTranslation;
         foreach (NavigationPoint point in _smoothedPathWorldNavigationPoints)
         {
-            debugGeometry.SetColor(new Color(0, 0, 1, 1));
+            debugGeometry.SetColor(new Color(0, 0, 1));
             debugGeometry.AddVertex(previousPoint + yOffset);
             debugGeometry.AddVertex(point.WorldPosition + yOffset);
 
@@ -444,7 +480,7 @@ public class NavigationComponent : Spatial
         previousPoint = parentNode.GlobalTranslation;
         foreach (NavigationPoint point in _planningPathWorldNavigationPoints)
         {
-            debugGeometry.SetColor(new Color(1, 0, 1, 1));
+            debugGeometry.SetColor(new Color(1, 0, 1));
             debugGeometry.AddVertex(previousPoint + yOffset);
             debugGeometry.AddVertex(point.WorldPosition + yOffset);
 
@@ -454,7 +490,7 @@ public class NavigationComponent : Spatial
         previousPoint = parentNode.GlobalTranslation;
         foreach (NavigationPoint point in _planningPathSmoothedWorldNavigationPoints)
         {
-            debugGeometry.SetColor(new Color(1, 1, 0, 1));
+            debugGeometry.SetColor(new Color(1, 1, 0));
             debugGeometry.AddVertex(previousPoint + yOffset);
             debugGeometry.AddVertex(point.WorldPosition + yOffset);
 

scenes/Game.cs

@@ -11,7 +11,8 @@ public class Game : Spatial
     private Label _tileOffsetLabel;
     private Label _numTilesLabel;
     private Label _mouseWorldLabel;
-    private Label _mouseTileLabel;
+    private Label _mouseTileOffsetLabel;
+    private Label _mouseTileCubeLabel;
     private Label _numCoordsAddedLabel;
     private Label _numCoordsRemovedLabel;
     private TextureRect _worldTextureRect;
@@ -53,7 +54,8 @@ public class Game : Spatial
         _tileOffsetLabel = debugStatsContainer.GetNode<Label>("tile_offset_label");
         _numTilesLabel = debugStatsContainer.GetNode<Label>("num_tiles_label");
         _mouseWorldLabel = debugStatsContainer.GetNode<Label>("mouse_world_label");
-        _mouseTileLabel = debugStatsContainer.GetNode<Label>("mouse_tile_label");
+        _mouseTileOffsetLabel = debugStatsContainer.GetNode<Label>("mouse_tile_offset_label");
+        _mouseTileCubeLabel = debugStatsContainer.GetNode<Label>("mouse_tile_cube_label");
         _numCoordsAddedLabel = debugStatsContainer.GetNode<Label>("num_coords_added_label");
         _numCoordsRemovedLabel = debugStatsContainer.GetNode<Label>("num_coords_removed_label");
 
@@ -235,7 +237,8 @@ public class Game : Spatial
         Transform highlightTransform = Transform.Identity;
 
         _mouseWorldLabel.Text = position.ToString();
-        _mouseTileLabel.Text = cellAtCursor.OffsetCoords.ToString();
+        _mouseTileOffsetLabel.Text = cellAtCursor.OffsetCoords.ToString();
+        _mouseTileCubeLabel.Text = cellAtCursor.CubeCoords.ToString();
         _mouseTileHighlight.Transform = highlightTransform;
 
         if (inputEvent is InputEventMouseButton && ((InputEventMouseButton)inputEvent).Pressed)
@@ -267,8 +270,9 @@ public class Game : Spatial
         Transform highlightTransform = tile.GlobalTransform;
         highlightTransform.origin.y += 0.1f;
         _mouseTileHighlight.Transform = highlightTransform;
-        _mouseWorldLabel.Text = tile.GlobalTranslation.ToString();
-        _mouseTileLabel.Text = tile.OffsetCoords.ToString();
+        _mouseWorldLabel.Text = highlightTransform.origin.ToString();
+        _mouseTileOffsetLabel.Text = tile.OffsetCoords.ToString();
+        _mouseTileCubeLabel.Text = tile.Cell.CubeCoords.ToString();
         
         _player.Navigation.FindPath(_player, _player.GlobalTranslation, tile.GlobalTranslation);
     }
@@ -344,7 +348,6 @@ public class Game : Spatial
         _worldTextureRect.Texture = newWorldTexture;
         _tileMaterial.SetShaderParam("MapAlbedoTexture", newWorldTexture);
         _tileMaterial.SetShaderParam("TextureSize", (int)_tileWorld.ColormapImage.GetSize().x);
-        GD.Print("Texture size: " + (int)_tileWorld.ColormapImage.GetSize().x);
 
         ImageTexture newHeightTexture = new ImageTexture();
         newHeightTexture.CreateFromImage(_tileWorld.HeightmapImage,

scenes/Game.tscn

@@ -34,10 +34,9 @@ script = ExtResource( 9 )
 
 [node name="TileHighlight" parent="." instance=ExtResource( 5 )]
 transform = Transform( 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.1, 0 )
-visible = false
 
 [node name="MouseTileHighlight" parent="." instance=ExtResource( 5 )]
-transform = Transform( 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.1, 0 )
+transform = Transform( 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1.3, 0 )
 
 [node name="TileWorld" parent="." instance=ExtResource( 8 )]
 GenerationMapType = 0
@@ -81,136 +80,151 @@ margin_left = -141.0
 margin_top = -172.0
 grow_horizontal = 0
 grow_vertical = 0
+mouse_filter = 2
 size_flags_horizontal = 3
 size_flags_vertical = 3
 
 [node name="DebugStatsContainer" type="GridContainer" parent="DebugContainer"]
 margin_left = 7.0
 margin_top = 7.0
-margin_right = 134.0
-margin_bottom = 165.0
+margin_right = 156.0
+margin_bottom = 183.0
 grow_horizontal = 0
 grow_vertical = 0
 mouse_filter = 2
 columns = 2
 
 [node name="Label9" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 14.0
 rect_pivot_offset = Vector2( -335, -33 )
 text = "FPS"
 
 [node name="fps_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
-margin_right = 127.0
+margin_left = 117.0
+margin_right = 149.0
 margin_bottom = 14.0
 text = "0,0"
 
 [node name="Label" type="Label" parent="DebugContainer/DebugStatsContainer"]
 margin_top = 18.0
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 32.0
 rect_pivot_offset = Vector2( -335, -33 )
 text = "Tile"
 
 [node name="tile_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
+margin_left = 117.0
 margin_top = 18.0
-margin_right = 127.0
+margin_right = 149.0
 margin_bottom = 32.0
 text = "0,0"
 
 [node name="Label2" type="Label" parent="DebugContainer/DebugStatsContainer"]
 margin_top = 36.0
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 50.0
 text = "Tile Offset"
 
 [node name="tile_offset_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
+margin_left = 117.0
 margin_top = 36.0
-margin_right = 127.0
+margin_right = 149.0
 margin_bottom = 50.0
 text = "0,0"
 
 [node name="Label4" type="Label" parent="DebugContainer/DebugStatsContainer"]
 margin_top = 54.0
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 68.0
 rect_pivot_offset = Vector2( -335, -33 )
 text = "Mouse World"
 
 [node name="mouse_world_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
+margin_left = 117.0
 margin_top = 54.0
-margin_right = 127.0
+margin_right = 149.0
 margin_bottom = 68.0
 text = "0,0"
 
 [node name="Label6" type="Label" parent="DebugContainer/DebugStatsContainer"]
 margin_top = 72.0
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 86.0
 rect_pivot_offset = Vector2( -335, -33 )
-text = "Mouse Tile"
+text = "Mouse Tile Offset"
 
-[node name="mouse_tile_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
+[node name="mouse_tile_offset_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
+margin_left = 117.0
 margin_top = 72.0
-margin_right = 127.0
+margin_right = 149.0
 margin_bottom = 86.0
 text = "0,0"
 
-[node name="Label3" type="Label" parent="DebugContainer/DebugStatsContainer"]
+[node name="Label10" type="Label" parent="DebugContainer/DebugStatsContainer"]
 margin_top = 90.0
-margin_right = 103.0
+margin_right = 113.0
 margin_bottom = 104.0
+rect_pivot_offset = Vector2( -335, -33 )
+text = "Mouse Tile Cube"
+
+[node name="mouse_tile_cube_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
+margin_left = 117.0
+margin_top = 90.0
+margin_right = 149.0
+margin_bottom = 104.0
+text = "0,0,0"
+
+[node name="Label3" type="Label" parent="DebugContainer/DebugStatsContainer"]
+margin_top = 108.0
+margin_right = 113.0
+margin_bottom = 122.0
 text = "#Tiles"
 
 [node name="num_tiles_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
-margin_top = 90.0
-margin_right = 127.0
-margin_bottom = 104.0
+margin_left = 117.0
+margin_top = 108.0
+margin_right = 149.0
+margin_bottom = 122.0
 text = "0"
 
 [node name="Label5" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_top = 108.0
-margin_right = 103.0
-margin_bottom = 122.0
+margin_top = 126.0
+margin_right = 113.0
+margin_bottom = 140.0
 text = "#Active"
 
 [node name="num_active_tiles_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
-margin_top = 108.0
-margin_right = 127.0
-margin_bottom = 122.0
+margin_left = 117.0
+margin_top = 126.0
+margin_right = 149.0
+margin_bottom = 140.0
 text = "0"
 
 [node name="Label7" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_top = 126.0
-margin_right = 103.0
-margin_bottom = 140.0
+margin_top = 144.0
+margin_right = 113.0
+margin_bottom = 158.0
 text = "#Tiles Added"
 
 [node name="num_coords_added_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
-margin_top = 126.0
-margin_right = 127.0
-margin_bottom = 140.0
+margin_left = 117.0
+margin_top = 144.0
+margin_right = 149.0
+margin_bottom = 158.0
 text = "0"
 
 [node name="Label8" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_top = 144.0
-margin_right = 103.0
-margin_bottom = 158.0
+margin_top = 162.0
+margin_right = 113.0
+margin_bottom = 176.0
 text = "#Tiles Removed"
 
 [node name="num_coords_removed_label" type="Label" parent="DebugContainer/DebugStatsContainer"]
-margin_left = 107.0
-margin_top = 144.0
-margin_right = 127.0
-margin_bottom = 158.0
+margin_left = 117.0
+margin_top = 162.0
+margin_right = 149.0
+margin_bottom = 176.0
 text = "0"
 
 [node name="Generator Container" type="Control" parent="."]

scenes/TileWorld.cs

@@ -43,6 +43,7 @@ public class TileWorld : Spatial
     public float HeightScale = 2.0f;
     public Image HeightmapImage;
     public Image ColormapImage;
+    public Image NavigationmapImage;
     public int Seed = 0;
     public Spatial Entities;
     public HexGrid HexGrid;
@@ -124,15 +125,12 @@ public class TileWorld : Spatial
         HeightmapImage.FillRect(new Rect2(0, 0, size, size), Colors.ForestGreen);
         _heightmapOffscreenTextureRect.SetSize(sizeVector);
         _heightmapOffscreenViewport.Size = sizeVector;
+
+        NavigationmapImage = new Image();
+        NavigationmapImage.Create(size, size, false, Image.Format.Rgb8);
+        NavigationmapImage.FillRect(new Rect2(0, 0, size, size), new Color(1, 1, 1));
     }
 
-    public void PrintTextureSizes()
-    {
-        GD.Print("Color Viewport:        " + _worldOffscreenViewport.Size);
-        GD.Print("Color TextureRect:     " + _worldOffscreenTextureRect.Texture.GetSize());
-        GD.Print("Heightmap Viewport:    " + _heightmapOffscreenViewport.Size);
-        GD.Print("Heightmap TextureRect: " + _heightmapOffscreenTextureRect.Texture.GetSize());
-    }
 
     public void Generate(int size)
     {
@@ -314,6 +312,14 @@ public class TileWorld : Spatial
         return (color.r == 0 && color.g == 0 && color.b > 0.01);
     }
 
+    public void MarkCellUnwalkable(HexCell cell)
+    {
+        HexGrid.AddObstacle(cell);
+        NavigationmapImage.Lock();
+        NavigationmapImage.SetPixelv(OffsetToTextureCoord(cell.OffsetCoords), Colors.Red);
+        NavigationmapImage.Unlock();
+    }
+    
     private void PopulateEnvironment()
     {
         Random environmentRandom = new Random(Seed);
@@ -336,7 +342,7 @@ public class TileWorld : Spatial
                     if (rockAsset != null)
                     {
                         _environmentNode.AddChild(rockAsset);
-                        HexGrid.AddObstacle(cell);
+                        MarkCellUnwalkable(cell);
                     }
                 }
                 else if (IsColorEqualApprox(colorValue, GrassColor))
@@ -351,7 +357,7 @@ public class TileWorld : Spatial
                     if (treeAsset != null)
                     {
                         Entities.AddChild(treeAsset);
-                        HexGrid.AddObstacle(cell);
+                        MarkCellUnwalkable(cell);
                     }
                     else if (environmentRandom.NextDouble() < 0.01)
                     {
@@ -361,12 +367,12 @@ public class TileWorld : Spatial
                         assetTransform.origin.y += 1.2f;
                         chestAsset.Transform = assetTransform;
                         Entities.AddChild(chestAsset);
-                        HexGrid.AddObstacle(cell);
+                        MarkCellUnwalkable(cell);
                     }
                 }
                 else if (IsColorWater(colorValue))
                 {
-                    HexGrid.AddObstacle(cell);
+                    MarkCellUnwalkable(cell);
                 }
             }
         }
@@ -473,8 +479,6 @@ public class TileWorld : Spatial
         ColormapImage.Lock();
         ColormapImage.SetPixel((int)textureCoord.x, (int)textureCoord.y, color);
         ColormapImage.Unlock();
-
-        OnMapGenerationComplete();
     }
 
     public Vector2 WorldToOffsetCoords(Vector3 worldCoord)

scenes/tests/EditorUI.cs

@@ -16,27 +16,34 @@ public class EditorUI : Control
     private Button _grassButton;
     private Button _sandButton;
     private Button _waterButton;
+    private Button _obstacleButton;
+    private Button _navigateButton;
+    private ShaderMaterial _tileMaterial;
 
     private CheckBox _gameGeometryCheckBox;
     private CheckBox _physicsGeometryCheckBox;
+    private CheckBox _navigationGeometryCheckBox;
 
     private TileWorld _tileWorld;
     private StreamContainer _streamContainer;
 
-    private enum TileType
+    public enum InputMode
     {
         None,
         Grass,
         Sand,
-        Water
+        Water,
+        Obstacle,
+        Navigate
     }
-    private TileType _currentTileType = TileType.None;
+    public InputMode CurrentInputMode = InputMode.None;
     
     // Called when the node enters the scene tree for the first time.
     public override void _Ready()
     {
         _tileWorld = (TileWorld) GetNode(World);
         _streamContainer = (StreamContainer)GetNode(StreamContainer);
+        _tileMaterial = GD.Load<ShaderMaterial>("materials/HexTileTextureLookup.tres");
         
         // signals
         _resetButton = (Button) FindNode("ResetButton");
@@ -51,11 +58,20 @@ public class EditorUI : Control
         _waterButton = (Button) FindNode("WaterButton");
         _waterButton.Connect("pressed", this, nameof(OnWaterButton));
 
+        _obstacleButton = (Button) FindNode("ObstacleButton");
+        _obstacleButton.Connect("pressed", this, nameof(OnObstacleButton));
+
+        _navigateButton = (Button) FindNode("NavigateButton");
+        _navigateButton.Connect("pressed", this, nameof(OnNavigateButton));
+        
         _gameGeometryCheckBox = (CheckBox)FindNode("GameGeometryCheckBox");
         _gameGeometryCheckBox.Connect("toggled", this, nameof(OnGameGeometryCheckBoxToggled));
         
         _physicsGeometryCheckBox = (CheckBox)FindNode("PhysicsGeometryCheckBox");
         _physicsGeometryCheckBox.Connect("toggled", this, nameof(OnPhysicsGeometryCheckBoxToggled));
+        
+        _navigationGeometryCheckBox = (CheckBox)FindNode("NavigationGeometryCheckBox");
+        _navigationGeometryCheckBox.Connect("toggled", this, nameof(OnNavigationGeometryCheckBoxToggled));
     }
 
 
@@ -63,24 +79,37 @@ public class EditorUI : Control
     {
         GD.Print("Resetting Map");
         _tileWorld.Seed = _tileWorld.Seed + 1;
-        _tileWorld.Generate(12);
+        _tileWorld.Generate(24);
     }
 
     public void OnGrassButton()
     {
-        _currentTileType = TileType.Grass;
+        CurrentInputMode = InputMode.Grass;
     }
 
     public void OnSandButton()
     {
-        _currentTileType = TileType.Sand;
+        CurrentInputMode = InputMode.Sand;
 
     }
 
     public void OnWaterButton()
     {
-        _currentTileType = TileType.Water;
+        CurrentInputMode = InputMode.Water;
     }
+    
+    
+    public void OnObstacleButton()
+    {
+        CurrentInputMode = InputMode.Obstacle;
+    }
+
+    
+    public void OnNavigateButton()
+    {
+        CurrentInputMode = InputMode.Navigate;
+    }
+    
 
     public void OnGameGeometryCheckBoxToggled(bool pressed)
     {
@@ -107,16 +136,48 @@ public class EditorUI : Control
         }
     }
     
-    public void OnTileClicked(Vector2 offsetCoord)
+        
+    public void OnNavigationGeometryCheckBoxToggled(bool pressed)
     {
-        switch (_currentTileType)
+        UpdateTileMaterial();
+    }
+
+    public void UpdateTileMaterial()
+    {
+        if (_navigationGeometryCheckBox.Pressed)
         {
-            case TileType.Grass:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Green);
-                break; 
-            case TileType.Water:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Blue);
-                break; 
-            case TileType.Sand:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Yellow);
-                break; 
+            ImageTexture newWorldTexture = new ImageTexture();
+            newWorldTexture.CreateFromImage(_tileWorld.NavigationmapImage,
+                (uint)(Texture.FlagsEnum.Mipmaps | Texture.FlagsEnum.Repeat));
+            _tileMaterial.SetShaderParam("MapAlbedoTexture", newWorldTexture);
+            _tileMaterial.SetShaderParam("TextureSize", (int)_tileWorld.NavigationmapImage.GetSize().x);
+        }
+        else
+        {
+            ImageTexture newWorldTexture = new ImageTexture();
+            newWorldTexture.CreateFromImage(_tileWorld.ColormapImage,
+                (uint)(Texture.FlagsEnum.Mipmaps | Texture.FlagsEnum.Repeat));
+            _tileMaterial.SetShaderParam("MapAlbedoTexture", newWorldTexture);
+            _tileMaterial.SetShaderParam("TextureSize", (int)_tileWorld.ColormapImage.GetSize().x);
         }
     }
+    
+    
+    public void OnTileClicked(Vector2 offsetCoord)
+    {
+        switch (CurrentInputMode)
+        {
+            case InputMode.Grass:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Green);
+                break; 
+            case InputMode.Water:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Blue);
+                break; 
+            case InputMode.Sand:_tileWorld.SetTileColorAtOffset(currentTileOffset, Colors.Yellow);
+                break; 
+            case InputMode.Obstacle:
+                _tileWorld.MarkCellUnwalkable(HexCell.FromOffsetCoords(offsetCoord));
+                break; 
+        }
+        
+        UpdateTileMaterial();
+    }
 }

scenes/tests/NavigationTests.cs

@@ -58,8 +58,8 @@ public class NavigationTests : Spatial
         foreach (Spatial entity in entities)
         {
             Vector2 entityPlaneCoords = new Vector2(entity.GlobalTranslation.x, entity.GlobalTranslation.z);
-            HexCell entityCell = _hexGrid.GetHexAt(entityPlaneCoords);
-            _tileWorld.HexGrid.AddObstacle(entityCell);
+            HexCell entityCell = _tileWorld.HexGrid.GetHexAt(entityPlaneCoords);
+            _tileWorld.MarkCellUnwalkable(entityCell);
             Vector2 cellPlaneCoords = _hexGrid.GetHexCenterFromOffset(entityCell.OffsetCoords);
             Vector3 entityGlobalTranslation = entity.GlobalTranslation;
             entityGlobalTranslation.x = cellPlaneCoords.x;
@@ -85,6 +85,8 @@ public class NavigationTests : Spatial
             (uint)(Texture.FlagsEnum.Mipmaps | Texture.FlagsEnum.Repeat));
         _tileMaterial.SetShaderParam("MapAlbedoTexture", newWorldTexture);
         _tileMaterial.SetShaderParam("TextureSize", (int)_tileWorld.ColormapImage.GetSize().x);
+
+        CorrectEntityGridPositions();
     }
 
 
@@ -125,6 +127,11 @@ public class NavigationTests : Spatial
         if (_editorUi != null)
         {
             _editorUi.OnTileClicked(tile.OffsetCoords);
+
+            if (_editorUi.CurrentInputMode == EditorUI.InputMode.Navigate)
+            {
+                _playerNavigationComponent.FindPath(_player, _player.GlobalTranslation, tile.GlobalTranslation);
+            }
         }
     }
 

tests/HexCellTests.cs

@@ -1,8 +1,8 @@
-using System;
 using Godot;
 using GoDotTest;
 using System.Diagnostics;
 using System.Linq;
+using Xunit;
 
 public class HexCellTests : TestClass
 {
@@ -17,6 +17,25 @@ public class HexCellTests : TestClass
         Debug.Assert(cell.CubeCoords == new Vector3(0, 0, 0));
     }
 
+    [Test]
+    public void TestHexCellEqualityInequality()
+    {
+        HexCell cellA = new HexCell();
+        HexCell cellB = new HexCell();
+
+        cellA.AxialCoords = new Vector2(2, 3);
+        cellB.AxialCoords = new Vector2(2, 3);
+        Assert.Equal(cellA, cellB);
+        bool result = cellA == cellB;
+        Assert.True(cellA == cellB);
+        Assert.False(cellA != cellB);
+
+        cellB.AxialCoords = new Vector2(3, 2);
+        Assert.NotEqual(cellA, cellB);
+        Assert.False(cellA == cellB);
+        Assert.True(cellA != cellB);
+    }
+
     [Test]
     public void TestAxialCoords()
     {
@@ -113,13 +132,13 @@ public class HexCellTests : TestClass
     {
         HexCell cell = new HexCell();
         cell.OffsetCoords = new Vector2(1, 2);
-        
+
         Debug.Assert(cell.DistanceTo(new HexCell(new Vector2(0, 0))) == 3);
         Debug.Assert(cell.DistanceTo(new HexCell(new Vector2(3, 4))) == 4);
         Debug.Assert(cell.DistanceTo(new HexCell(new Vector2(-1, -1))) == 5);
     }
-    
-    
+
+
     [Test]
     public void TestLineTo()
     {
@@ -136,7 +155,7 @@ public class HexCellTests : TestClass
             new HexCell(4, 2),
             new HexCell(5, 2)
         };
-        
+
         Debug.Assert(path.Length == pathExpected.Length);
 
         foreach (int index in Enumerable.Range(0, path.Length))
@@ -161,10 +180,10 @@ public class HexCellTests : TestClass
             new HexCell(2, 3),
             new HexCell(3, 3),
             new HexCell(4, 3),
-            new HexCell(4, 4),            
+            new HexCell(4, 4),
             new HexCell(5, 4)
         };
-        
+
         Debug.Assert(path.Length == pathExpected.Length);
 
         foreach (int index in Enumerable.Range(0, path.Length))
@@ -172,7 +191,7 @@ public class HexCellTests : TestClass
             Debug.Assert(path[index].AxialCoords == pathExpected[index].AxialCoords);
         }
     }
-    
+
 
     [Test]
     public void TestLineEdge()
@@ -187,16 +206,109 @@ public class HexCellTests : TestClass
             new HexCell(1, 2),
             new HexCell(2, 2),
             new HexCell(2, 3),
-            new HexCell(2, 4),            
+            new HexCell(2, 4),
             new HexCell(3, 4)
         };
-        
+
         Debug.Assert(path.Length == pathExpected.Length);
 
         foreach (int index in Enumerable.Range(0, path.Length))
         {
-            Debug.Print("index: " + index + " path: " + path[index].AxialCoords + " expected: " + pathExpected[index].AxialCoords);
+            Debug.Print("index: " + index + " path: " + path[index].AxialCoords + " expected: " +
+                        pathExpected[index].AxialCoords);
             Debug.Assert(path[index].AxialCoords == pathExpected[index].AxialCoords);
         }
     }
+
+    [Test]
+    public void TestCellDirections()
+    {
+        HexCell cell = new HexCell();
+
+        HexCell cellN = HexCell.FromOffsetCoords(new Vector2(0, 1));
+        HexCell cellNW = HexCell.FromOffsetCoords(new Vector2(-1, 0));
+        HexCell cellSW = HexCell.FromOffsetCoords(new Vector2(-1, -1));
+        HexCell cellS = HexCell.FromOffsetCoords(new Vector2(0, -1));
+        HexCell cellSE = HexCell.FromOffsetCoords(new Vector2(1, -1));
+        HexCell cellNE = HexCell.FromOffsetCoords(new Vector2(1, 0));
+
+        HexCell neighbour = cell.GetAdjacent(HexCell.DIR_N);
+        Assert.Equal(cellN, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_NW);
+        Assert.Equal(cellNW, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_SW);
+        Assert.Equal(cellSW, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_S);
+        Assert.Equal(cellS, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_SE);
+        Assert.Equal(cellSE, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_NW);
+        Assert.Equal(cellNW, neighbour);
+    }
+
+    [Test]
+    public void TestCellDirectionsNonzeroReference()
+    {
+        HexCell cell = HexCell.FromOffsetCoords(new Vector2(-4, -3));
+
+        HexCell cellN = HexCell.FromOffsetCoords(new Vector2(-4, -2));
+        HexCell cellNW = HexCell.FromOffsetCoords(new Vector2(-5, -3));
+        HexCell cellSW = HexCell.FromOffsetCoords(new Vector2(-5, -4));
+        HexCell cellS = HexCell.FromOffsetCoords(new Vector2(-4, -4));
+        HexCell cellSE = HexCell.FromOffsetCoords(new Vector2(-3, -4));
+        HexCell cellNE = HexCell.FromOffsetCoords(new Vector2(-3, -3));
+
+        HexCell neighbour = cell.GetAdjacent(HexCell.DIR_N);
+        Assert.Equal(cellN, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_NW);
+        Assert.Equal(cellNW, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_SW);
+        Assert.Equal(cellSW, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_S);
+        Assert.Equal(cellS, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_SE);
+        Assert.Equal(cellSE, neighbour);
+
+        neighbour = cell.GetAdjacent(HexCell.DIR_NW);
+        Assert.Equal(cellNW, neighbour);
+    }
+
+    [Test]
+    public void TestNextCellAlongLine()
+    {
+        HexCell cell = new HexCell();
+        HexCell cellN = HexCell.FromOffsetCoords(new Vector2(0, 1));
+        HexCell cellNE = HexCell.FromOffsetCoords(new Vector2(1, 0));
+        HexCell cellSE = HexCell.FromOffsetCoords(new Vector2(1, -1));
+        HexCell cellS = HexCell.FromOffsetCoords(new Vector2(0, -1));
+        HexCell cellSW = HexCell.FromOffsetCoords(new Vector2(-1, -1));
+        HexCell cellNW = HexCell.FromOffsetCoords(new Vector2(-1, 0));
+
+        HexCell nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(0, 1));
+        Assert.Equal(cellS, nextCell);
+
+        nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(1, 1).Normalized());
+        Assert.Equal(cellSE, nextCell);
+
+        nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(1, -1).Normalized());
+        Assert.Equal(cellNE, nextCell);
+
+        nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(0, -1));
+        Assert.Equal(cellN, nextCell);
+
+        nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(-1, -1).Normalized());
+        Assert.Equal(cellNW, nextCell);
+
+        nextCell = cell.NextCellAlongLine(new Vector2(0, 0), new Vector2(-1, 1).Normalized());
+        Assert.Equal(cellSW, nextCell);
+    }
 }

tests/HexGridPathFindingTests.cs

@@ -1,15 +1,9 @@
 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;
 
@@ -18,14 +12,14 @@ public class HexGridPathFindingTests : TestClass
     private HexGrid _hexGrid;
     private HexCell _hexCell;
 
-    private HexCell _positionA = new HexCell(new Vector2(2, 0)); 
+    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),
@@ -53,7 +47,7 @@ public class HexGridPathFindingTests : TestClass
             _hexGrid.AddObstacle(new HexCell(obstacle));
         }
     }
-    
+
     [Test]
     public void TestBounds()
     {
@@ -61,7 +55,7 @@ public class HexGridPathFindingTests : TestClass
         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)));
@@ -73,7 +67,7 @@ public class HexGridPathFindingTests : TestClass
     {
         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)));
@@ -86,7 +80,7 @@ public class HexGridPathFindingTests : TestClass
     {
         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)));
@@ -98,11 +92,11 @@ public class HexGridPathFindingTests : TestClass
     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)]);
@@ -110,7 +104,7 @@ public class HexGridPathFindingTests : TestClass
         // 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)));
     }
@@ -119,10 +113,10 @@ public class HexGridPathFindingTests : TestClass
     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))));
-        
+        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)));
+        Assert.Equal(1.337f, _hexGrid.GetHexCost(new Vector2(1, 1)));
     }
 
     [Test]
@@ -140,7 +134,7 @@ public class HexGridPathFindingTests : TestClass
         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));
     }
 
@@ -155,7 +149,7 @@ public class HexGridPathFindingTests : TestClass
             Assert.Equal(expectedCell.AxialCoords, pathCell.AxialCoords);
         }
     }
-    
+
     [Test]
     public void TestStraightLine()
     {
@@ -204,7 +198,7 @@ public class HexGridPathFindingTests : TestClass
 
         ComparePath(expectedPath, _hexGrid.FindPath(expectedPath.First(), expectedPath.Last()));
     }
-    
+
     [Test]
     public void TestWalls()
     {
@@ -222,7 +216,7 @@ public class HexGridPathFindingTests : TestClass
         {
             _hexGrid.AddBarrier(_positionG, wall);
         }
-        
+
         List<HexCell> expectedPath = new List<HexCell>()
         {
             _positionA,
@@ -240,7 +234,7 @@ public class HexGridPathFindingTests : TestClass
     {
         _hexGrid.AddBarrier(_positionG, HexCell.DIR_NE, 3);
         _hexGrid.AddBarrier(_positionG, HexCell.DIR_N, _hexGrid.PathCostDefault - 0.1f);
-        
+
         List<HexCell> expectedPath = new List<HexCell>()
         {
             _positionA,
@@ -263,7 +257,7 @@ public class HexGridPathFindingTests : TestClass
             new HexCell(new Vector2(5, 1)),
             _positionB,
         };
-        
+
         List<HexCell> longPath = new List<HexCell>()
         {
             _positionA,
@@ -279,7 +273,7 @@ public class HexGridPathFindingTests : TestClass
 
         _hexGrid.PathCostDefault = 1f;
         ComparePath(shortPath, _hexGrid.FindPath(shortPath.First(), shortPath.Last()));
-        
+
         _hexGrid.PathCostDefault = 2f;
         ComparePath(shortPath, _hexGrid.FindPath(shortPath.First(), shortPath.Last()));
 

tests/HexGridTests.cs

@@ -1,6 +1,8 @@
+using System.Collections.Generic;
 using Godot;
 using GoDotTest;
 using System.Diagnostics;
+using Xunit;
 
 public class HexGridTests : TestClass
 {
@@ -20,4 +22,70 @@ public class HexGridTests : TestClass
         Debug.Assert(grid.GetHexAt(new Vector2(w / 2 - 0.01f, -h / 2)).AxialCoords == new Vector2(1, 0));
         Debug.Assert(grid.GetHexAt(new Vector2(w / 2 - 0.01f, h / 2)).AxialCoords == new Vector2(1, -1));
     }
+
+    [Test]
+    public void TestGetCellsForLineSimple()
+    {
+        HexGrid grid = new HexGrid();
+
+        List<HexCell> lineCells =
+            grid.GetCellsForLine(new Vector2(0, 0), grid.GetHexCenterFromOffset(new Vector2(0, 2)));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 1)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 2)), lineCells[2]);
+
+        lineCells =
+            grid.GetCellsForLine(grid.GetHexCenterFromOffset(new Vector2(0, 2)), new Vector2(0, 0));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 2)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 1)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[2]);
+    }
+
+    [Test]
+    public void TestGetCellsDiagonal()
+    {
+        HexGrid grid = new HexGrid();
+
+        List<HexCell> lineCells =
+            grid.GetCellsForLine(new Vector2(0, 0), grid.GetHexCenterFromOffset(new Vector2(2, 1)));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(1, 0)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(2, 1)), lineCells[2]);
+
+        lineCells =
+            grid.GetCellsForLine(grid.GetHexCenterFromOffset(new Vector2(2, 1)), new Vector2(0, 0));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(2, 1)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(1, 0)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[2]);
+    }
+
+    [Test]
+    public void TestGetCellsForLineAlongEdge()
+    {
+        HexGrid grid = new HexGrid();
+
+        List<HexCell> lineCells =
+            grid.GetCellsForLine(new Vector2(0, -0.0001f), grid.GetHexCenterFromOffset(new Vector2(2, 0)));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(1, 0)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(2, 0)), lineCells[2]);
+
+        lineCells =
+            grid.GetCellsForLine(new Vector2(0, 0.0001f), grid.GetHexCenterFromOffset(new Vector2(2, 0)));
+
+        Assert.Equal(3, lineCells.Count);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(0, 0)), lineCells[0]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(1, -1)), lineCells[1]);
+        Assert.Equal(HexCell.FromOffsetCoords(new Vector2(2, 0)), lineCells[2]);
+    }
 }

tests/Plane2DTests.cs

@@ -0,0 +1,69 @@
+using System;
+using Godot;
+using GodotComponentTest.utils;
+using GoDotTest;
+using Xunit;
+
+namespace GodotComponentTest.tests;
+
+public class Plane2DTests : TestClass
+{
+    public Plane2DTests(Node testScene) : base(testScene)
+    {
+    }
+
+    [Test]
+    public void Plane2DDistSimple()
+    {
+        Plane2D plane2D = new Plane2D(new Vector2(0, 1), new Vector2(0, -1));
+
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(0, 0)) - 1) < Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(0, 1))) < Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(0, 2)) + 1) < Single.Epsilon);
+    }
+
+    [Test]
+    public void Plane2DDistAngled()
+    {
+        Plane2D plane2D = new Plane2D(new Vector2(0, 1), new Vector2(1, -1).Normalized());
+
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(0, 1))) < Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(0, 0)) - MathF.Sqrt(2) / 2) < Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToPoint(new Vector2(-1, 0))) < Single.Epsilon);
+    }
+
+    [Test]
+    public void Plane2DDistLineSegment()
+    {
+        Plane2D plane2D = new Plane2D(new Vector2(0, 1), new Vector2(1, -1).Normalized());
+
+        Assert.True(
+            Mathf.Abs(plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(-1, 0)) - 1) < Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(0, 1)) - 1) <
+                    Single.Epsilon);
+        Assert.True(Mathf.Abs(plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(1, -1).Normalized()) +
+                              MathF.Sqrt(2) / 2) < Plane2D.DistancePrecision);
+        Assert.True(Mathf.Abs(plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(-1, 1).Normalized()) -
+                              MathF.Sqrt(2) / 2) < Plane2D.DistancePrecision);
+    }
+
+    [Test]
+    public void Plane2DTestIsParallel()
+    {
+        Plane2D plane2D = new Plane2D(new Vector2(0, 1), new Vector2(1, -1).Normalized());
+
+        Assert.True(plane2D.IsParallelToDir(new Vector2(1, 1.00001f).Normalized()));
+        Assert.True(plane2D.IsParallelToDir(new Vector2(1, 0.99999f).Normalized()));
+    }
+
+    [Test]
+    public void Plane2DDistLineSegmentParallel()
+    {
+        Plane2D plane2D = new Plane2D(new Vector2(0, 1), new Vector2(1, -1).Normalized());
+
+        Assert.Equal(Single.PositiveInfinity,
+            plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(1, 1.00001f).Normalized()));
+        Assert.Equal(Single.NegativeInfinity,
+            plane2D.DistanceToLineSegment(new Vector2(0, 0), new Vector2(1, 0.99999f).Normalized()));
+    }
+}

ui/EditorUI.tscn

@@ -4,7 +4,7 @@
 
 [sub_resource type="ButtonGroup" id=4]
 resource_local_to_scene = false
-resource_name = "TileTypeButtonGroup"
+resource_name = "InputTypeButtonGroup"
 
 [node name="EditorUI" type="Control"]
 anchor_right = 1.0
@@ -56,6 +56,22 @@ toggle_mode = true
 group = SubResource( 4 )
 text = "Sand"
 
+[node name="ObstacleButton" type="Button" parent="HBoxContainer"]
+margin_left = 244.0
+margin_right = 313.0
+margin_bottom = 25.0
+toggle_mode = true
+group = SubResource( 4 )
+text = "Obstacle"
+
+[node name="NavigateButton" type="Button" parent="HBoxContainer"]
+margin_left = 317.0
+margin_right = 384.0
+margin_bottom = 25.0
+toggle_mode = true
+group = SubResource( 4 )
+text = "Navigate"
+
 [node name="ViewFlagsContainer" type="HBoxContainer" parent="."]
 anchor_top = 1.0
 anchor_bottom = 1.0
@@ -79,3 +95,9 @@ margin_left = 104.0
 margin_right = 180.0
 margin_bottom = 40.0
 text = "Physics"
+
+[node name="NavigationGeometryCheckBox" type="CheckBox" parent="ViewFlagsContainer"]
+margin_left = 184.0
+margin_right = 279.0
+margin_bottom = 40.0
+text = "Navigation"

utils/Plane2D.cs

@@ -0,0 +1,47 @@
+using System;
+using Godot;
+
+namespace GodotComponentTest.utils;
+
+public class Plane2D
+{
+    public static readonly float DistancePrecision = 1.0e-5f;
+
+    private Vector2 _planePoint;
+    public Vector2 Normal;
+
+    public Plane2D(Vector2 planePoint, Vector2 normal)
+    {
+        _planePoint = planePoint;
+        Normal = normal;
+    }
+
+    public float DistanceToPoint(Vector2 point)
+    {
+        return (point - _planePoint).Dot(Normal);
+    }
+
+    public bool IsParallelToDir(Vector2 dir)
+    {
+        float normalDotDir = Normal.Dot(dir);
+
+        return (Mathf.Abs(normalDotDir) <= Plane2D.DistancePrecision);
+    }
+
+    public float DistanceToLineSegment(Vector2 point, Vector2 dir)
+    {
+        float normalDotDir = Normal.Dot(dir);
+
+        if (Mathf.Abs(normalDotDir) > Plane2D.DistancePrecision)
+        {
+            return (_planePoint.Dot(Normal) - point.Dot(Normal)) / normalDotDir;
+        }
+
+        if (normalDotDir < 0)
+        {
+            return Single.PositiveInfinity;
+        }
+
+        return Single.NegativeInfinity;
+    }
+}