GodotComponentTest/HexGrid.cs

305 lines
9.1 KiB
C#

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Threading;
using Godot;
using Godot.Collections;
using Priority_Queue;
using Array = Godot.Collections.Array;
using AxialCoordDirectionPair = System.Tuple<Godot.Vector2, Godot.Vector3>;
public class HexGrid : Resource
{
private Vector2 _baseHexSize = new Vector2(1, Mathf.Sqrt(3) / 2);
private Vector2 _hexSize = new Vector2(1, Mathf.Sqrt(3) / 2);
private Vector2 _hexScale = new Vector2(1, 1);
private Godot.Transform2D _hexTransform;
private Godot.Transform2D _hexTransformInv;
private HexCell _minCoords = new HexCell();
private HexCell _maxCoords = new HexCell();
private Rect2 _boundsAxialCoords = 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 int FindPathCheckedCells = 0;
public Vector2 HexSize
{
get { return _hexSize; }
}
public Vector2 HexScale
{
get { return _hexScale; }
set
{
_hexScale = value;
_hexSize = _baseHexSize * _hexScale;
_hexTransform = new Transform2D(
new Vector2(_hexSize.x * 3 / 4, -_hexSize.y / 2),
new Vector2(0, -_hexSize.y),
new Vector2(0, 0)
);
_hexTransformInv = _hexTransform.AffineInverse();
}
}
public HexGrid()
{
HexScale = new Vector2(1, 1);
}
public Vector2 GetHexCenter(HexCell cell)
{
return _hexTransform * cell.AxialCoords;
}
public Vector2 GetHexCenterFromOffset(Vector2 offsetCoord)
{
HexCell cell = new HexCell();
cell.OffsetCoords = offsetCoord;
return GetHexCenter(cell);
}
public HexCell GetHexAtOffset(Vector2 offsetCoord)
{
HexCell cell = new HexCell();
cell.OffsetCoords = offsetCoord;
return cell;
}
public HexCell GetHexAt(Vector2 planeCoord)
{
HexCell result = new HexCell(_hexTransformInv * planeCoord);
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));
}
public void SetBounds(HexCell minCell, HexCell maxCell)
{
_minCoords = minCell;
_maxCoords = maxCell;
_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);
}
}
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 (!_boundsAxialCoords.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 HexCell GetClosestWalkableCell(HexCell fromCell, HexCell toCell)
{
if (GetHexCost(toCell) == 0)
{
HexCell[] line = fromCell.LineTo(toCell);
foreach (int i in Enumerable.Range(1, line.Length))
{
if (GetHexCost(line[i]) == 0)
{
toCell = line[i - 1];
break;
}
}
}
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>();
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;
}
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;
}
}
}
// GD.Print("Checked Cell Count: " + FindPathCheckedCells);
if (!cameFrom.ContainsKey(goalHex.AxialCoords))
{
GD.Print("Failed to find path from " + startHex + " to " + goalHex);
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;
}
}