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 = new(-Vector2.Inf, Vector2.Inf);
    private Rect2 _boundsOffsetCoords = new(-Vector2.Inf, Vector2.Inf);
    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) {
        HexCell cell = new HexCell();
        cell.OffsetCoords = offsetCoord;
        return GetHexCenter(cell);
    }

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

    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 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) {
        Vector2 centerOffset = center.OffsetCoords;
        SetBounds(GetHexAtOffset(centerOffset - Vector2.One * size / 2),
            GetHexAtOffset(centerOffset + Vector2.One * size / 2));
    }

    public void SetBoundsOffset(HexCell cellSouthEast, Vector2 size) {
        _boundsOffsetCoords = new Rect2(cellSouthEast.OffsetCoords, size);
        _boundsAxialCoords = new Rect2(-Vector2.Inf, Vector2.Inf);
    }

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

        if (!_boundsOffsetCoords.HasPoint(new HexCell(axialCoords).OffsetCoords)) {
            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;
        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) {
            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 goalAxialCoords = goalHex.AxialCoords;

        SimplePriorityQueue<Vector2, float> frontier = new SimplePriorityQueue<Vector2, float>();
        frontier.Enqueue(startHex.AxialCoords, 0);
        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);

        FindPathCheckedCellCount = 0;

        while (frontier.Any()) {
            FindPathCheckedCellCount++;
            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: " + FindPathCheckedCellCount);

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

        HexCell 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) {
        List<HexCell> result = new List<HexCell>();

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

        Vector2 currentPointPlane = fromPlane;
        HexCell currentCell = GetHexAt(currentPointPlane);
        float currentDistance = 0;

        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]);
            currentDistance += boundaryPlaneDistance * 1.001f;
            currentPointPlane = fromPlane + direction * boundaryPlaneDistance;
        } while (currentDistance < distance);

        result.Add(currentCell);

        return result;
    }
}