Implemented A* pathfinding. Adjusted movement node accordingly.

2026-04-29 17:05:29 +02:00
parent 21a1eb2085
commit 253c7d9f89
6 changed files with 200 additions and 26 deletions
@@ -0,0 +1,94 @@
+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 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();
+        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);
+                }
+            }
+        }
+
+        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);
+                }
+            }
+        }
+    }
+
+    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)];
+    }
+}