GodotComponentTest/HexGrid.cs

using System.Collections.Generic;
using System.Linq;
using Godot;
using Priority_Queue;
using AxialCoordDirectionPair = System.Tuple<Godot.Vector2, Godot.Vector3>;

public class HexGrid : Resource
{
    private readonly Vector2 _baseHexSize = new(1, Mathf.Sqrt(3) / 2);
    private Rect2 _boundsAxialCoords;
    private Vector2 _hexScale = new(1, 1);
    private Vector2 _hexSize = new(1, Mathf.Sqrt(3) / 2);
    private Transform2D _hexTransform;
    private Transform2D _hexTransformInv;
    private HexCell _maxCoords = new();
    private HexCell _minCoords = new();

    public Dictionary<(Vector2, Vector3), float> Barriers = new();
    public int FindPathCheckedCellCount;

    public Dictionary<Vector2, float> Obstacles = new();


    public float PathCostDefault = 1;

    public HexGrid()
    {
        HexScale = new Vector2(1, 1);
    }

    public Vector2 HexSize => _hexSize;

    public Vector2 HexScale
    {
        get => _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 Vector2 GetHexCenter(HexCell cell)
    {
        return _hexTransform * cell.AxialCoords;
    }

    public Vector2 GetHexCenterFromOffset(Vector2 offsetCoord)
    {
        var cell = new HexCell();
        cell.OffsetCoords = offsetCoord;
        return GetHexCenter(cell);
    }

    public Vector3 GetHexCenterVec3FromOffset(Vector2 offsetCoord)
    {
        var cell = new HexCell();
        cell.OffsetCoords = offsetCoord;
        var hexCenter = GetHexCenter(cell);
        return new Vector3(hexCenter.x, 0, hexCenter.y);
    }

    public HexCell GetHexAtOffset(Vector2 offsetCoord)
    {
        var cell = new HexCell();
        cell.OffsetCoords = offsetCoord;
        return cell;
    }

    public HexCell GetHexAt(Vector2 planeCoord)
    {
        var result = new HexCell(_hexTransformInv * planeCoord);
        return result;
    }

    public void SetBounds(Vector2 minAxial, Vector2 maxAxial)
    {
        SetBounds(new HexCell(minAxial), new HexCell(maxAxial));
    }

    public void SetBounds(HexCell minCell, HexCell maxCell)
    {
        _minCoords = minCell;
        _maxCoords = maxCell;
        _boundsAxialCoords = new Rect2(_minCoords.AxialCoords,
            _maxCoords.AxialCoords - _minCoords.AxialCoords + Vector2.One);
    }

    public void SetBounds(HexCell center, int size)
    {
        var centerOffset = center.OffsetCoords;
        SetBounds(GetHexAtOffset(centerOffset - Vector2.One * size / 2),
            GetHexAtOffset(centerOffset + Vector2.One * size / 2));
    }

    public void AddObstacle(Vector2 axialCoords, float cost = 0)
    {
        AddObstacle(new HexCell(axialCoords), cost);
    }

    public void AddObstacle(HexCell cell, float cost = 0)
    {
        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);
    }

    public void AddBarrier(HexCell cell, Vector3 directionCube, float cost = 0)
    {
        Barriers.Add((cell.AxialCoords, directionCube), cost);
    }

    public void RemoveBarrier(HexCell cell, Vector3 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)
    {
        var startCell = new HexCell(axialCoords);
        var targetCell = new HexCell(startCell.CubeCoords + directionCube);

        var cost = GetHexCost(axialCoords);
        if (cost == 0) return 0;

        cost = GetHexCost(targetCell.AxialCoords);
        if (cost == 0) return 0;

        float barrierCost;
        if (Barriers.ContainsKey((axialCoords, directionCube)))
        {
            barrierCost = Barriers[(axialCoords, directionCube)];
            if (barrierCost == 0) return 0;

            cost += barrierCost;
        }

        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)
        {
            var line = fromCell.LineTo(toCell);

            foreach (var 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)
    {
        var goalAxialCoords = goalHex.AxialCoords;

        var frontier = new SimplePriorityQueue<Vector2, float>();
        frontier.Enqueue(startHex.AxialCoords, 0);
        var cameFrom =
            new Dictionary<Vector2, Vector2>();
        var costSoFar =
            new Dictionary<Vector2, float>();

        cameFrom.Add(startHex.AxialCoords, startHex.AxialCoords);
        costSoFar.Add(startHex.AxialCoords, 0);

        FindPathCheckedCellCount = 0;

        while (frontier.Any())
        {
            FindPathCheckedCellCount++;
            var currentHex = new HexCell(frontier.Dequeue());
            var currentAxial = currentHex.AxialCoords;

            if (currentHex == goalHex) break;

            foreach (var nextHex in currentHex.GetAllAdjacent())
            {
                var nextAxial = nextHex.AxialCoords;
                var 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;
                    var priority = nextCost + nextHex.DistanceTo(goalHex);

                    frontier.Enqueue(nextHex.AxialCoords, priority);
                    cameFrom[nextHex.AxialCoords] = currentHex.AxialCoords;
                }
            }
        }

        // GD.Print("Checked Cell Count: " + FindPathCheckedCellCount);

        var 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);

        var pathHex = goalHex;
        while (pathHex != startHex)
        {
            pathHex = new HexCell(cameFrom[pathHex.AxialCoords]);
            result.Insert(0, pathHex);
        }

        return result;
    }


    public List<HexCell> GetCellsForLine(Vector2 fromPlane, Vector2 toPlane)
    {
        var result = new List<HexCell>();

        var distance = (toPlane - fromPlane).Length();
        var direction = (toPlane - fromPlane) / distance;

        var currentPointPlane = fromPlane;
        var currentCell = GetHexAt(currentPointPlane);
        float currentDistance = 0;

        do
        {
            result.Add(currentCell);
            GetHexCenter(currentCell);
            var currentPointLocal = currentPointPlane - GetHexCenter(currentCell);

            int neighbourIndex;
            float boundaryPlaneDistance;
            currentCell.QueryClosestCellBoundary(currentPointLocal, direction, out neighbourIndex,
                out boundaryPlaneDistance);

            currentCell = currentCell.GetAdjacent(HexCell.NeighborDirections[neighbourIndex]);
            currentDistance += boundaryPlaneDistance * 1.001f;
            currentPointPlane = fromPlane + direction * boundaryPlaneDistance;
        } while (currentDistance < distance);

        result.Add(currentCell);

        return result;
    }
}