Started working on path finding in HexGrid.cs.

WorldChunkRefactoring
Martin Felis 2023-07-15 21:50:38 +02:00
parent c068663c3c
commit f56a5bcaa7
4 changed files with 540 additions and 1 deletions

View File

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

View File

@ -26,6 +26,24 @@ public class HexCell : Resource
CubeCoords = RoundCoords(new Vector3(cubeX, cubeY, cubeZ)); 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) public HexCell(Vector3 cubeCoords)
{ {
CubeCoords = cubeCoords; CubeCoords = cubeCoords;
@ -137,6 +155,19 @@ public class HexCell : Resource
return new HexCell(this.CubeCoords + dir); 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) public int DistanceTo(HexCell target)
{ {
return (int)( return (int)(
@ -159,7 +190,7 @@ public class HexCell : Resource
path[dist] = new HexCell(); path[dist] = new HexCell();
path[dist].CubeCoords = CubeCoords.LinearInterpolate(nudgedTarget.CubeCoords, (float)dist / steps); path[dist].CubeCoords = CubeCoords.LinearInterpolate(nudgedTarget.CubeCoords, (float)dist / steps);
} }
path[steps] = target; path[steps] = target;
return path; return path;

View File

@ -1,13 +1,33 @@
using System; using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Threading;
using Godot; 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 public class HexGrid : Resource
{ {
private Vector2 _baseHexSize = new Vector2(1, Mathf.Sqrt(3) / 2); private Vector2 _baseHexSize = new Vector2(1, Mathf.Sqrt(3) / 2);
private Vector2 _hexSize = 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 Vector2 _hexScale = new Vector2(1, 1);
private Godot.Transform2D _hexTransform; private Godot.Transform2D _hexTransform;
private Godot.Transform2D _hexTransformInv; 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 public Vector2 HexSize
{ {
@ -60,4 +80,193 @@ public class HexGrid : Resource
HexCell result = new HexCell(_hexTransformInv * planeCoord); HexCell result = new HexCell(_hexTransformInv * planeCoord);
return result; 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;
}
} }

View File

@ -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()));
}
}