Implemented A* pathfinding. Adjusted movement node accordingly.

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;
-            return true;
+            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;
     }