From af3495f4e39c7d85545aa0d3a81e0849987fc672 Mon Sep 17 00:00:00 2001
From: Rijad Zuzo <rijad.zuzo@matriq.ch>
Date: Fri, 19 Jun 2026 10:02:42 +0200
Subject: [PATCH] feat: add npc path finding to the zones based on needs

---
 main.tscn                   | 12 ++++++--
 player/npc/NpcVisual.gd     | 55 ++++++++++++++++++++++++++++---------
 player/npc/NpcVisual.tscn   |  6 ++++
 world/world_view_manager.gd | 38 +++++++++++++++++++------
 4 files changed, 88 insertions(+), 23 deletions(-)

diff --git a/main.tscn b/main.tscn
index 1483ee8..f347ecd 100644
--- a/main.tscn
+++ b/main.tscn
@@ -1,4 +1,4 @@
-[gd_scene load_steps=12 format=3 uid="uid://rs3hv73svbpa"]
+[gd_scene load_steps=13 format=3 uid="uid://rs3hv73svbpa"]
 
 [ext_resource type="Script" uid="uid://4mg67crlim53" path="res://player/player.gd" id="1_h2yge"]
 [ext_resource type="Script" uid="uid://ii8ai801jur2" path="res://player/camera_rig.gd" id="2_1bvp3"]
@@ -6,6 +6,10 @@
 [ext_resource type="Script" uid="uid://c1f3b26n4yntf" path="res://world/world_view_manager.gd" id="4_1bvp3"]
 [ext_resource type="PackedScene" uid="uid://dhxxyprqflotq" path="res://player/npc/NpcVisual.tscn" id="5_lquwl"]
 
+[sub_resource type="NavigationMesh" id="NavigationMesh_lquwl"]
+vertices = PackedVector3Array(-14.5, 0.4, -14.5, -14.5, 0.4, 14.5, 14.5, 0.4, 14.5, 14.5, 0.4, -14.5)
+polygons = [PackedInt32Array(3, 2, 0), PackedInt32Array(0, 2, 1)]
+
 [sub_resource type="StandardMaterial3D" id="StandardMaterial3D_o5qli"]
 albedo_color = Color(0.16862746, 0.52156866, 0.30588236, 1)
 
@@ -34,10 +38,14 @@ light_energy = 2.0
 
 [node name="World" type="Node3D" parent="."]
 
-[node name="Ground" type="MeshInstance3D" parent="World"]
+[node name="NavigationRegion3D" type="NavigationRegion3D" parent="World"]
+navigation_mesh = SubResource("NavigationMesh_lquwl")
+
+[node name="Ground" type="MeshInstance3D" parent="World/NavigationRegion3D"]
 transform = Transform3D(30, 0, 0, 0, 0.1, 0, 0, 0, 30, 0, -0.05, 0)
 material_override = SubResource("StandardMaterial3D_o5qli")
 mesh = SubResource("BoxMesh_sgp6g")
+skeleton = NodePath("../..")
 
 [node name="Player" type="CharacterBody3D" parent="." node_paths=PackedStringArray("camera_rig")]
 script = ExtResource("1_h2yge")
diff --git a/player/npc/NpcVisual.gd b/player/npc/NpcVisual.gd
index 3a71e86..57383d4 100644
--- a/player/npc/NpcVisual.gd
+++ b/player/npc/NpcVisual.gd
@@ -1,41 +1,70 @@
 extends CharacterBody3D
 
+const DEBUG_LOGS := false
+
 @export var move_speed := 3.0
 @export var rotation_speed := 8.0
+@export var waypoint_reached_distance := 0.35
+
+@onready var nav_agent: NavigationAgent3D = $NavigationAgent3D
 
 var sim_id: int = -1
 var npc_name: String = ""
 var profession: String = ""
-var target_position: Vector3
-var has_target := false
+
+var current_path: PackedVector3Array = []
+var path_index := 0
+var current_target := Vector3.INF
+
+func debug_log(message: String) -> void:
+	if DEBUG_LOGS:
+		print("[NPCVisual] ", name, " | ", message)
 
 func setup_from_sim(npc: SimNPC) -> void:
 	sim_id = npc.id
 	npc_name = npc.npc_name
 	profession = npc.profession
 	name = "NPC_%s_%s" % [sim_id, npc_name]
-	target_position = global_position
 
 func set_target_position(pos: Vector3) -> void:
-	target_position = pos
-	has_target = true
-
-func _physics_process(delta: float) -> void:
-	if not has_target:
+	if current_target.distance_to(pos) < 0.1:
 		return
 
-	var to_target := target_position - global_position
-	to_target.y = 0
+	current_target = pos
+	nav_agent.target_position = pos
 
-	if to_target.length() < 0.25:
+	await get_tree().physics_frame
+
+	current_path = NavigationServer3D.map_get_path(
+		get_world_3d().navigation_map,
+		global_position,
+		pos,
+		true
+	)
+
+	path_index = 0
+
+	debug_log("New target: %s Path points: %s" % [pos, current_path.size()])
+
+func _physics_process(delta: float) -> void:
+	if current_path.is_empty() or path_index >= current_path.size():
 		velocity = Vector3.ZERO
 		move_and_slide()
 		return
 
-	var direction := to_target.normalized()
+	var next_pos := current_path[path_index]
+	var flat_next_pos := Vector3(next_pos.x, global_position.y, next_pos.z)
+	var to_next := flat_next_pos - global_position
+
+	if to_next.length() <= waypoint_reached_distance:
+		path_index += 1
+		return
+
+	var direction := to_next.normalized()
+
 	velocity.x = direction.x * move_speed
 	velocity.z = direction.z * move_speed
-	velocity.y = 0
+	velocity.y = 0.0
 
 	move_and_slide()
 
diff --git a/player/npc/NpcVisual.tscn b/player/npc/NpcVisual.tscn
index 6c32034..85484f3 100644
--- a/player/npc/NpcVisual.tscn
+++ b/player/npc/NpcVisual.tscn
@@ -26,3 +26,9 @@ mesh = SubResource("CapsuleMesh_d844k")
 [node name="FaceMarker" type="MeshInstance3D" parent="."]
 transform = Transform3D(0.15, 0, 0, 0, 0.15, 0, 0, 0, 0.35, 0, 1.2, 0.59052056)
 mesh = SubResource("BoxMesh_d844k")
+
+[node name="NavigationAgent3D" type="NavigationAgent3D" parent="."]
+path_desired_distance = 0.3
+target_desired_distance = 0.5
+height = 1.7
+radius = 0.4
diff --git a/world/world_view_manager.gd b/world/world_view_manager.gd
index d6646eb..f4b8f8a 100644
--- a/world/world_view_manager.gd
+++ b/world/world_view_manager.gd
@@ -1,5 +1,7 @@
 extends Node
 
+const DEBUG_LOGS := true
+
 @export var npc_visual_scene: PackedScene
 @export var simulation_manager: Node
 @export var active_npcs_parent: Node3D
@@ -11,11 +13,24 @@ extends Node
 @export var rest_zone: Marker3D
 @export var food_zone: Marker3D
 
-var active_npc_visuals := {} # sim_id -> NPCVisual
+var active_npc_visuals := {}
+
+func debug_log(message: String) -> void:
+	if DEBUG_LOGS:
+		print("[WorldViewManager] ", message)
 
 func _ready() -> void:
 	call_deferred("spawn_initial_npcs")
 
+var target_update_timer := 0.0
+var target_update_interval := 0.5
+
+func _process(delta: float) -> void:
+	target_update_timer += delta
+	if target_update_timer >= target_update_interval:
+		target_update_timer = 0.0
+		update_npc_targets()
+
 func spawn_initial_npcs() -> void:
 	if simulation_manager == null:
 		push_error("WorldViewManager: simulation_manager is missing")
@@ -25,6 +40,12 @@ func spawn_initial_npcs() -> void:
 		push_error("WorldViewManager: npc_visual_scene is missing")
 		return
 
+	if active_npcs_parent == null:
+		push_error("WorldViewManager: active_npcs_parent is missing")
+		return
+
+	debug_log("Spawning NPC visuals. Count: %s" % simulation_manager.npcs.size())
+
 	for npc in simulation_manager.npcs:
 		spawn_npc_visual(npc)
 
@@ -32,7 +53,7 @@ func spawn_npc_visual(npc: SimNPC) -> void:
 	if active_npc_visuals.has(npc.id):
 		return
 
-	var visual := npc_visual_scene.instantiate()
+	var visual = npc_visual_scene.instantiate()
 	active_npcs_parent.add_child(visual)
 
 	visual.global_position = npc.position
@@ -40,10 +61,8 @@ func spawn_npc_visual(npc: SimNPC) -> void:
 
 	active_npc_visuals[npc.id] = visual
 
-func _process(delta: float) -> void:
-	update_npc_targets()
-	
-	
+	debug_log("Spawned visual for %s at %s" % [npc.npc_name, npc.position])
+
 func update_npc_targets() -> void:
 	if simulation_manager == null:
 		return
@@ -55,8 +74,11 @@ func update_npc_targets() -> void:
 		var visual = active_npc_visuals[npc.id]
 		var target := get_target_for_task(npc.current_task)
 
-		if target != null:
-			visual.set_target_position(target.global_position)
+		if target == null:
+			debug_log("No target for task: %s" % npc.current_task)
+			continue
+
+		visual.set_target_position(target.global_position)
 
 func get_target_for_task(task: String) -> Marker3D:
 	match task:
