GodotComponentTest/HexGrid.cs

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