Scripts/DSL/Nodes/MoveNode.cs

@@ -1,26 +1,45 @@
+using System;
+using System.Collections.Generic;
 using Godot;
 
 public class MoveNode : ProgramNode
 {
     public Vector3 startPosition;
     public Vector3I targetPosition;
+    public List<Vector3> pathPoints;
     public MoveNode()
     {
         DisplayText = "Move";
     }
     public override bool Execute(Robot robot, double delta)
     {
-        Vector3 worldTarget = GameData.map[targetPosition.Y].tiles[targetPosition.X, targetPosition.Z].Position;
-        GD.Print(startPosition + "/" + worldTarget);
+        pathPoints ??= [.. Pathfinding.GetPath(Pathfinding.GetClosestStartPoint(robot.Position), targetPosition)];
+
         startPosition = robot.Position;
-        Vector3 direction = worldTarget - startPosition;
-        robot.Translate(direction.Normalized() * (float)delta * GameData.robotSpeed);
-        float distance = direction.Length();
+        Vector3 target = pathPoints[0] - startPosition;
+        float distance = target.Length();
+
         if (distance < 0.1f)
         {
-            robot.Position = worldTarget;
+            robot.Position = pathPoints[0];
+            pathPoints.Remove(pathPoints[0]);
+            if (pathPoints.Count <= 0)
+            {
+                pathPoints = null;
                 return true;
             }
+
+            return false;
+        }
+
+        Vector3 direction = target / distance;
+        Vector3 lookDirection = new Vector3(direction.X, 0, direction.Z);
+        if (lookDirection.Length() > 0.1f)
+        {
+            robot.LookAt(robot.GlobalPosition + lookDirection, Vector3.Up);
+        }
+        robot.GlobalPosition += direction * (float)delta * GameData.robotSpeed;
+
         return false;
     }
 

Scripts/WorldGeneration/Layer.cs

@@ -198,9 +198,9 @@ public partial class Layer : Node3D
 				? new HashSet<string> { currentTile.collapsedMesh }
 				: new HashSet<string>(currentTile.tileMeshes.Keys);
 
-			for (int i = 0; i < dirs.Length; i++)
+			for (int i = 0; i < offsets2D.Length; i++)
 			{
-				Vector2I newPos = currentPos + offsets[i];
+				Vector2I newPos = currentPos + offsets2D[i];
 				if (!InBounds(newPos, layerSize)) continue;
 
 				Tile neighborTile = tiles[newPos.X, newPos.Y];

Scripts/WorldGeneration/Pathfinding.cs

@@ -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)];
+    }
+}

Scripts/WorldGeneration/Pathfinding.cs.uid

@@ -0,0 +1 @@
+uid://dwya0owm8kv03

Scripts/WorldGeneration/WFC.cs

@@ -18,7 +18,17 @@ public class WFC
 		Down
 	}
 
-	public static readonly Vector2I[] offsets =
+	public static readonly Direction[] dirs =
+	{
+		Direction.Backward,
+		Direction.Forward,
+		Direction.Left,
+		Direction.Right,
+		Direction.Up,
+		Direction.Down
+	};
+
+	public static readonly Vector2I[] offsets2D =
 	{
 		new Vector2I(0, -1),
 		new Vector2I(0, 1),
@@ -26,12 +36,14 @@ public class WFC
 		new Vector2I(1, 0)
 	};
 
-	public static readonly Direction[] dirs =
+	public static readonly Vector3I[] offsets3D =
 	{
-		Direction.Backward,
-		Direction.Forward,
-		Direction.Left,
-		Direction.Right
+		new Vector3I(0, 0, -1),
+		new Vector3I(0, 0, 1),
+		new Vector3I(-1, 0, 0),
+		new Vector3I(1, 0, 0),
+		new Vector3I(0, -1, 0),
+		new Vector3I(0, 1, 0)
 	};
 
 	public static Dictionary<string, HashSet<Direction>> tileConnections = new Dictionary<string, HashSet<Direction>>
@@ -148,6 +160,51 @@ public class WFC
 		return CanConnect(fromTile.collapsedMesh, toTile.collapsedMesh, dir, true);
 	}
 
+	public static bool CanWalk3D(Vector3I from, Vector3I to)
+	{
+		Tile fromTile = GameData.map[from.Y].tiles[from.X, from.Z];
+		Tile toTile = GameData.map[to.Y].tiles[to.X, to.Z];
+
+		if (fromTile == null || toTile == null)
+			return false;
+
+		if (from.Y != to.Y)
+		{
+			if (Math.Abs(from.Y - to.Y) != 1)
+				return false;
+
+			if (from.Y > to.Y)
+			{
+				return toTile.collapsedMesh == "gate";
+			}
+			else
+			{
+				return fromTile.collapsedMesh == "gate";
+			}
+		}
+
+		int dx = to.X - from.X;
+		int dz = to.Z - from.Z;
+
+		if (Math.Abs(dx) + Math.Abs(dz) != 1)
+			return false;
+
+		Direction dir;
+
+		if (dx == 1) dir = Direction.Right;
+		else if (dx == -1) dir = Direction.Left;
+		else if (dz == 1) dir = Direction.Forward;
+		else if (dz == -1) dir = Direction.Backward;
+		else return false;
+
+		return CanWalk(
+			GameData.map[from.Y].tiles,
+			new Vector2I(from.X, from.Z),
+			new Vector2I(to.X, to.Z),
+			dir
+		);
+	}
+
 	public static bool IsMapConnected(Tile[,] layer, float accessibilityThreshhold)
 	{
 		bool result = false;
@@ -165,9 +222,9 @@ public class WFC
 			toCheck[index] = toCheck[^1];
 			toCheck.RemoveAt(toCheck.Count - 1);
 			if (!visited.Add(position)) continue;
-			for (int i = 0; i < 4; i++)
+			for (int i = 0; i < offsets2D.Length; i++)
 			{
-				var next = position + offsets[i];
+				var next = position + offsets2D[i];
 
 				if (!InBounds(next, layer.GetLength(0)))
 					continue;

Scripts/WorldGeneration/World.cs

@@ -14,6 +14,7 @@ public partial class World : Node3D
 	private Dictionary<string, MultiMeshInstance3D> multiMeshes = new();
 	private Dictionary<string, Mesh> meshLibrary = new();
 	Layer layerNode;
+	Pathfinding pathfinding;
 
 	private MultiMeshHandler multiMeshHandler;
 
@@ -43,6 +44,8 @@ public partial class World : Node3D
 		map = new Layer[ruinSize];
 		GenerateWorld();
 
+		Pathfinding.BuildAStarGraph();
+
 		HandleRenderData(BuildRenderData(0));
 	}