feat: add better priorities setting

player/npc/NpcVisual.gd

@@ -2,7 +2,7 @@ extends CharacterBody3D
 
 signal arrived_at_target(sim_id: int)
 
-const DEBUG_LOGS := false
+const DEBUG_LOGS := true
 
 @export var move_speed := 3.0
 @export var rotation_speed := 8.0
@@ -18,6 +18,7 @@ var profession: String = ""
 var current_path: PackedVector3Array = []
 var path_index := 0
 var current_target := Vector3.INF
+var arrival_distance := 0.6
 
 func debug_log(message: String) -> void:
 	if DEBUG_LOGS:
@@ -30,28 +31,15 @@ func setup_from_sim(npc: SimNPC) -> void:
 	name = "NPC_%s_%s" % [sim_id, npc_name]
 
 func set_target_position(pos: Vector3) -> void:
-	if current_target.distance_to(pos) < 0.1:
-		return
-
 	current_target = pos
 	has_reported_arrival = false
-	nav_agent.target_position = pos
 
-	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()])
-	if current_target.distance_to(pos) < 0.1:
+	if global_position.distance_to(pos) <= arrival_distance:
+		current_path = PackedVector3Array()
+		path_index = 0
+		debug_log("Already close to target, will report arrival")
 		return
 
-	current_target = pos
 	nav_agent.target_position = pos
 
 	await get_tree().physics_frame
@@ -65,18 +53,26 @@ func set_target_position(pos: Vector3) -> void:
 
 	path_index = 0
 
-	debug_log("New target: %s Path points: %s" % [pos, current_path.size()])
+	if current_path.is_empty():
+		debug_log("Path is empty. Will report arrival/failure fallback.")
+	else:
+		debug_log("New target: %s Path points: %s" % [pos, current_path.size()])
 
 func _physics_process(delta: float) -> void:
+	if current_target == Vector3.INF:
+		velocity = Vector3.ZERO
+		move_and_slide()
+		return
+
+	if global_position.distance_to(current_target) <= arrival_distance:
+		_report_arrival_once()
+		velocity = Vector3.ZERO
+		move_and_slide()
+		return
+
 	if current_path.is_empty() or path_index >= current_path.size():
 		velocity = Vector3.ZERO
 		move_and_slide()
-
-		if not has_reported_arrival and current_target != Vector3.INF:
-			has_reported_arrival = true
-			debug_log("Arrived at target")
-			arrived_at_target.emit(sim_id)
-
 		return
 
 	var next_pos := current_path[path_index]
@@ -97,3 +93,14 @@ func _physics_process(delta: float) -> void:
 
 	var target_angle := atan2(direction.x, direction.z)
 	rotation.y = lerp_angle(rotation.y, target_angle, rotation_speed * delta)
+	
+func _report_arrival_once() -> void:
+	if has_reported_arrival:
+		return
+
+	has_reported_arrival = true
+	current_path = PackedVector3Array()
+	path_index = 0
+
+	debug_log("Arrived at target")
+	arrived_at_target.emit(sim_id)