RuinAdventurer/Scripts/WorldGeneration/Pathfinding.cs

using System.Collections.Generic;
using System.Linq;
using Godot;

public class Pathfinding
{
    private static AStar3D aStar = new AStar3D();
    private static Dictionary<Vector3I, long> coordToId = new();
    private static Dictionary<long, Vector3I> idToCoord = new();
    private static long nextId = 1;
    private static long[] layerGateIds = new long[GameData.ruinSize];

    private static long GetOrCreateId(Vector3I coord)
    {
        if (coordToId.TryGetValue(coord, out long id))
            return id;

        id = nextId++;
        coordToId[coord] = id;
        idToCoord[id] = coord;

        return id;
    }

    public static void BuildAStarGraph()
    {
        aStar.Clear();
        coordToId.Clear();
        idToCoord.Clear();
        layerGateIds = new long[GameData.ruinSize];
        nextId = 1;

        for (int y = 0; y < GameData.ruinSize; y++)
        {
            for (int x = 0; x < GameData.layerSize; x++)
            {
                for (int z = 0; z < GameData.layerSize; z++)
                {
                    Vector3I coord = new Vector3I(x, y, z);
                    Tile tile = GameData.map[y].tiles[x, z];

                    if (tile == null || tile.collapsedMesh == null)
                        continue;

                    long id = GetOrCreateId(coord);

                    aStar.AddPoint(id, tile.Position);
                    if (tile.collapsedMesh == "gate")
                    {
                        layerGateIds[y] = id;
                    }
                }
            }
        }

        foreach (var kvp in coordToId)
        {
            Vector3I from = kvp.Key;
            long fromId = kvp.Value;

            foreach (Vector3I offset in WFC.offsets3D)
            {
                var to = new Vector3I(
                    from.X + offset.X,
                    from.Y + offset.Y,
                    from.Z + offset.Z
                );

                if (!coordToId.ContainsKey(to))
                    continue;

                if (!WFC.CanWalk3D(from, to))
                    continue;

                long toId = coordToId[to];

                if (!aStar.ArePointsConnected(fromId, toId))
                {
                    aStar.ConnectPoints(fromId, toId);
                }
            }
        }

        for (int y = 0; y < GameData.ruinSize; y++)
        {
            UpdateGatePoint(y, false);
        }
    }

    public static void UpdateGatePoint(int layer, bool isOpen)
    {
        aStar.SetPointDisabled(layerGateIds[layer], !isOpen);
    }

    public static List<Vector3> GetPath(Vector3I start, Vector3I end)
    {
        if (!coordToId.ContainsKey(start) || !coordToId.ContainsKey(end))
            return new List<Vector3>();

        long startId = coordToId[start];
        long endId = coordToId[end];

        return aStar.GetPointPath(startId, endId).ToList();
    }

    public static Vector3I GetClosestStartPoint(Vector3 robotPosition)
    {
        return idToCoord[aStar.GetClosestPoint(robotPosition)];
    }
}