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

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

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


    public float PathCostDefault = 1;
    public int FindPathCheckedCellCount;

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