Added HexGrid::GetCellsForLine().

HexCell.cs

@@ -1,9 +1,7 @@
 using System;
-using System.Diagnostics;
 using System.Linq;
-using System.Runtime.CompilerServices;
 using Godot;
-using Array = Godot.Collections.Array;
+using GodotComponentTest.utils;
 
 public class HexCell : IEquatable<HexCell>
 {
@@ -20,16 +18,58 @@ public class HexCell : IEquatable<HexCell>
         return _cubeCoords.GetHashCode();
     }
 
-    public static readonly Vector2 size = new Vector2(1, Mathf.Sqrt(3) / 2);
+    public static readonly Vector2 Size = new(1, Mathf.Sqrt(3) / 2);
-    public static readonly Vector3 DIR_N = new Vector3(0, 1, -1);
+    private const float Width = 2;
-    public static readonly Vector3 DIR_NE = new Vector3(1, 0, -1);
+    private static readonly float Height = Mathf.Sqrt(3);
-    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 static readonly Array DIR_ALL = new Array()
+    public static readonly Vector3 DIR_N = new(0, 1, -1);
-        { DIR_N, DIR_NE, DIR_SE, DIR_S, DIR_SW, DIR_NW };
+    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()
     {
@@ -117,16 +157,16 @@ public class HexCell : IEquatable<HexCell>
         {
             int x = (int)CubeCoords.x;
             int y = (int)CubeCoords.y;
-            int off_y = y + (x - (x & 1)) / 2;
+            int offY = y + (x - (x & 1)) / 2;
-            return new Vector2(x, off_y);
+            return new Vector2(x, offY);
         }
 
         set
         {
             int x = (int)value.x;
             int y = (int)value.y;
-            int cube_y = y - (x - (x & 1)) / 2;
+            int cubeY = y - (x - (x & 1)) / 2;
-            AxialCoords = new Vector2(x, cube_y);
+            AxialCoords = new Vector2(x, cubeY);
         }
     }
 
@@ -211,4 +251,33 @@ public class HexCell : IEquatable<HexCell>
 
         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>;
 
 public class HexGrid : Resource
 {
+    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;
 
-    private Vector2 _baseHexSize = new Vector2(1, Mathf.Sqrt(3) / 2);
+    public Dictionary<Vector2, float> Obstacles = new();
-    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 _boundsAxialCoords = new Rect2();
 
-    public System.Collections.Generic.Dictionary<Vector2, float> Obstacles = new System.Collections.Generic.Dictionary<Vector2, float>();
+    public Dictionary<(Vector2, Vector3), float> Barriers = new();
-
-    public System.Collections.Generic.Dictionary<(Vector2, Vector3), float> Barriers =
-        new System.Collections.Generic.Dictionary<(Vector2, Vector3), float>();
 
 
     public float PathCostDefault = 1;
@@ -84,7 +78,7 @@ public class HexGrid : Resource
 
     public void SetBoundsOffset(Vector2 minOffset, Vector2 maxOffset)
     {
-        SetBounds (HexCell.FromOffsetCoords(minOffset), HexCell.FromOffsetCoords(maxOffset));
+        SetBounds(HexCell.FromOffsetCoords(minOffset), HexCell.FromOffsetCoords(maxOffset));
     }
 
     public void SetBounds(Vector2 minAxial, Vector2 maxAxial)
@@ -96,7 +90,8 @@ public class HexGrid : Resource
     {
         _minCoords = minCell;
         _maxCoords = maxCell;
-        _boundsAxialCoords = new Rect2(_minCoords.AxialCoords, (_maxCoords.AxialCoords - _minCoords.AxialCoords) + Vector2.One); 
+        _boundsAxialCoords = new Rect2(_minCoords.AxialCoords,
+            (_maxCoords.AxialCoords - _minCoords.AxialCoords) + Vector2.One);
     }
 
     public void AddObstacle(Vector2 axialCoords, float cost = 0)
@@ -105,16 +100,9 @@ public class HexGrid : Resource
     }
 
     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)
     {
@@ -129,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));
@@ -176,7 +162,6 @@ public class HexGrid : Resource
         }
 
         float barrierCost;
-        AxialCoordDirectionPair barrierKey = new AxialCoordDirectionPair(axialCoords, directionCube);
         if (Barriers.ContainsKey((axialCoords, directionCube)))
         {
             barrierCost = Barriers[(axialCoords, directionCube)];
@@ -188,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)];
@@ -225,13 +209,14 @@ public class HexGrid : Resource
 
     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>();
+        Dictionary<Vector2, Vector2> cameFrom =
-        System.Collections.Generic.Dictionary<Vector2, float> costSoFar = new System.Collections.Generic.Dictionary<Vector2, float>();
+            new Dictionary<Vector2, Vector2>();
+        Dictionary<Vector2, float> costSoFar =
+            new Dictionary<Vector2, float>();
 
         cameFrom.Add(startHex.AxialCoords, startHex.AxialCoords);
         costSoFar.Add(startHex.AxialCoords, 0);
@@ -281,13 +266,13 @@ public class HexGrid : Resource
 
         // GD.Print("Checked Cell Count: " + FindPathCheckedCells);
 
+        List<HexCell> result = new List<HexCell>();
         if (!cameFrom.ContainsKey(goalHex.AxialCoords))
         {
             GD.Print("Failed to find path from " + startHex + " to " + goalHex);
-            return new List<HexCell>();
+            return result;
         }
 
-        List<HexCell> result = new List<HexCell>();
         if (GetHexCost(goalAxialCoords) != 0)
         {
             result.Add(goalHex);
@@ -302,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;
+    }
 }

tests/HexCellTests.cs

@@ -1,4 +1,3 @@
-using System;
 using Godot;
 using GoDotTest;
 using System.Diagnostics;
@@ -215,8 +214,101 @@ public class HexCellTests : TestClass
 
         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/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()));
+    }
+}

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;
+    }
+}