Added debug drawing helper class, navigation planning is only done once instead of every frame.

components/NavigationComponent.cs

@@ -4,6 +4,7 @@ using System.Diagnostics;
 using System.Linq;
 using System.Numerics;
 using Godot;
+using GodotComponentTest.utils;
 using Vector2 = Godot.Vector2;
 using Vector3 = Godot.Vector3;
 using GoDotLog;
@@ -307,4 +308,33 @@ public class NavigationComponent : Node
             _currentGoalPositionWorld = currentTransformWorld.origin;
         }
     }
+
+    public void DebugDraw(Spatial parentNode, DebugGeometry debugGeometry)
+    {
+        
+        Vector3 yOffset = Vector3.Up * 0.1f;
+
+        debugGeometry.GlobalTransform = Transform.Identity;
+        debugGeometry.Begin(Mesh.PrimitiveType.Lines);
+        debugGeometry.SetColor(Colors.Pink);
+        debugGeometry.AddVertex(parentNode.GlobalTranslation + yOffset);
+        debugGeometry.SetColor(Colors.Pink);
+        debugGeometry.AddVertex(CurrentGoalPositionWorld + yOffset);
+        debugGeometry.SetColor(Colors.Pink);
+
+        debugGeometry.PushTranslated(CurrentGoalPositionWorld);
+        debugGeometry.AddBox(Vector3.One * 1);
+        debugGeometry.PopTransform();
+
+        Vector3 previousPoint = parentNode.GlobalTranslation;
+        foreach (NavigationPoint point in _pathWorldNavigationPoints)
+        {
+            debugGeometry.AddVertex(previousPoint + yOffset);
+            debugGeometry.AddVertex(point.WorldPosition + yOffset);
+
+            previousPoint = point.WorldPosition;
+        }
+        
+        debugGeometry.End();
+    }
 }

components/TaskQueueComponent.cs

@@ -22,6 +22,7 @@ public class TaskQueueComponent : Component
     public class NavigationTask : Task
     {
         public NavigationComponent.NavigationPoint NavigationPoint;
+        public bool PlanningComplete = false;
 
         public NavigationTask(NavigationComponent.NavigationPoint navigationPoint)
         {

entities/Player.cs

@@ -5,6 +5,7 @@ using System.Diagnostics;
 using System.Linq;
 using GodotComponentTest.components;
 using GodotComponentTest.entities;
+using GodotComponentTest.utils;
 
 public class Player : Entity, IInteractionInterface
 {
@@ -32,6 +33,7 @@ public class Player : Entity, IInteractionInterface
     private BoneAttachment _toolAttachement;
     private AnimationPlayer _playerAnimationPlayer;
     private AnimationTree _animationTree;
+    private DebugGeometry _debugGeometry;
     private InteractionComponent _interactionComponent;
     public InteractionComponent InteractionComponent
     {
@@ -81,6 +83,8 @@ public class Player : Entity, IInteractionInterface
         {
             GD.PushWarning("No ToolAttachement found!");
         }
+
+        _debugGeometry = (DebugGeometry)FindNode("DebugGeometry");
     }
 
 
@@ -100,10 +104,11 @@ public class Player : Entity, IInteractionInterface
             if (TaskQueueComponent.Queue.Count > 0)
             {
                 var currentTask = TaskQueueComponent.Queue.Peek();
-                if (currentTask is TaskQueueComponent.NavigationTask)
+                TaskQueueComponent.NavigationTask navigationTask = currentTask as TaskQueueComponent.NavigationTask;
+                if (navigationTask != null && navigationTask.PlanningComplete == false)
                 {
-                    TaskQueueComponent.NavigationTask navigationTask = (TaskQueueComponent.NavigationTask)currentTask;
                     _navigationComponent.PlanGridPath(GlobalTransform, navigationTask.NavigationPoint);
+                    navigationTask.PlanningComplete = true;
                 }
             }
         }
@@ -128,6 +133,26 @@ public class Player : Entity, IInteractionInterface
                 bar.SetTarget(GlobalTransform.origin);
             }
         }
+        
+        UpdateDebugGeometry();
+    }
+
+    public void UpdateDebugGeometry()
+    {
+        if (_debugGeometry == null || _debugGeometry.Visible == false)
+        {
+            return;
+        }
+        
+        _debugGeometry.Clear();
+        _debugGeometry.GlobalTransform = Transform.Identity;
+        
+        if (_navigationComponent != null)
+        {
+            _navigationComponent.DebugDraw(this, _debugGeometry);
+            return;
+        }
+
     }
 
     public void OnItemAttractorBodyEntered(Node node)

scenes/Game.tscn

@@ -1,4 +1,4 @@
-[gd_scene load_steps=22 format=2]
+[gd_scene load_steps=23 format=2]
 
 [ext_resource path="res://scenes/StreamContainer.cs" type="Script" id=1]
 [ext_resource path="res://components/NavigationComponent.cs" type="Script" id=2]
@@ -12,6 +12,7 @@
 [ext_resource path="res://scenes/DebugCamera.gd" type="Script" id=10]
 [ext_resource path="res://assets/Characters/Pirate.tscn" type="PackedScene" id=11]
 [ext_resource path="res://ui/WorldGeneratorUI.gd" type="Script" id=12]
+[ext_resource path="res://utils/DebugGeometry.cs" type="Script" id=13]
 [ext_resource path="res://entities/Axe.tscn" type="PackedScene" id=14]
 [ext_resource path="res://systems/InteractionSystem.cs" type="Script" id=15]
 
@@ -243,8 +244,8 @@ mouse_filter = 2
 [node name="WorldGeneratorContainer" type="VBoxContainer" parent="Generator Container"]
 margin_left = 10.0
 margin_top = 10.0
-margin_right = 110.0
+margin_right = 145.0
-margin_bottom = 258.0
+margin_bottom = 94.0
 script = ExtResource( 12 )
 
 [node name="HBoxContainer" type="HBoxContainer" parent="Generator Container/WorldGeneratorContainer"]
@@ -268,7 +269,7 @@ text = "4"
 
 [node name="WorldGenerateButton" type="Button" parent="Generator Container/WorldGeneratorContainer"]
 margin_top = 20.0
-margin_right = 100.0
+margin_right = 135.0
 margin_bottom = 40.0
 text = "Generate"
 
@@ -364,6 +365,11 @@ shape = SubResource( 23 )
 
 [node name="Geometry" parent="Player" instance=ExtResource( 11 )]
 
+[node name="DebugGeometry" type="Spatial" parent="Player"]
+script = ExtResource( 13 )
+
+[node name="ImmediateGeometry" type="ImmediateGeometry" parent="Player/DebugGeometry"]
+
 [node name="Entities" type="Spatial" parent="."]
 
 [node name="Chest" parent="Entities" instance=ExtResource( 7 )]

utils/DebugGeometry.cs

@@ -0,0 +1,105 @@
+using System.Collections.Generic;
+using System.Linq;
+using Godot;
+
+namespace GodotComponentTest.utils;
+
+public class DebugGeometry : Spatial
+{
+    private ImmediateGeometry _immediateGeometry;
+
+    private List<Transform> _transformStack;
+    private Transform _currentTransform = Transform.Identity; 
+
+    public override void _Ready()
+    {
+        base._Ready();
+        
+        _immediateGeometry = (ImmediateGeometry)FindNode("ImmediateGeometry");
+        
+        Clear();
+    }
+
+    public void Clear()
+    {
+        _immediateGeometry.Clear();
+        _transformStack = new List<Transform>();
+        _transformStack.Add(Transform.Identity);
+    }
+
+    public void PushTransform(Transform transform)
+    {
+        _transformStack.Add(transform);
+        _currentTransform = transform;
+    }
+
+    public void PushTranslated(Vector3 offset)
+    {
+        PushTransform(_currentTransform.Translated(offset));
+    }
+
+    public void PopTransform()
+    {
+        _transformStack.RemoveAt(_transformStack.Count - 1);
+        _currentTransform = PeekTransform();
+    }
+
+    public Transform PeekTransform() 
+    {
+        return _transformStack[_transformStack.Count - 1];
+    }
+
+    public void Begin(Mesh.PrimitiveType primitive, Texture texture = null)
+    {
+        _immediateGeometry.Begin(primitive, texture);
+    }
+
+    public void End()
+    {
+        _immediateGeometry.End();
+    }
+
+    public void AddVertex(Vector3 vertex)
+    {
+        _immediateGeometry.AddVertex(_currentTransform.Xform(vertex));
+    }
+
+    public void SetColor(Color color)
+    {
+        _immediateGeometry.SetColor(color);
+    }
+    
+    public void AddBox(Vector3 extents)
+    {
+        Transform currentTransform = PeekTransform();
+        // bottom square
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));
+        
+        // top square
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        
+        // side
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));        
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
+        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));        
+    }
+}