add nav agent avoidance

scenes/units/test_unit.tscn

@@ -26,3 +26,9 @@ texture = ExtResource("2_5725f")
 [node name="NavigationAgent3D" type="NavigationAgent3D" parent="." index="4"]
 path_desired_distance = 0.5
 target_desired_distance = 0.5
+avoidance_enabled = true
+height = 0.5
+radius = 0.25
+neighbor_distance = 10.0
+debug_enabled = true
+debug_use_custom = true

scripts/units/test_unit.gd

@@ -14,6 +14,8 @@ var ground_plane: Plane = Plane(Vector3.UP, 0)
 
 func _ready() -> void:
 	set_selected(false)
+	nav_agent.max_speed = SPEED
+	nav_agent.velocity_computed.connect(_on_nav_agent_velocity_computed)
 
 
 func _process(_delta: float) -> void:
@@ -43,7 +45,7 @@ func _set_target_click(mouse_pos: Vector2) -> void:
 	var click_position := _click_raycast(mouse_pos)
 	if click_position == null:
 		return
-	
+		
 	DebugDraw.marker(click_position)
 	nav_agent.set_target_position(click_position)
 
@@ -56,20 +58,26 @@ func _click_raycast(mouse_pos: Vector2) -> Vector3:
 
 func _navigate() -> void:
 	if nav_agent.is_navigation_finished():
+		velocity = Vector3.ZERO
 		return
-	
-	var target_pos := nav_agent.get_next_path_position()
-	DebugDraw.vector(position, target_pos)
+		
+	var next_pos := nav_agent.get_next_path_position()
+	DebugDraw.vector(position, next_pos)
 	DebugDraw.marker(nav_agent.get_final_position())
 
-	var direction := position.direction_to(target_pos)
-	velocity = direction * SPEED
-	move_and_slide()
+	var direction := position.direction_to(next_pos)
+	var new_velocity := direction * SPEED
+	nav_agent.set_velocity(new_velocity)
 
 
 func _animate() -> void:
 	if velocity.length() > 0.1:
+		look_at(position + velocity, Vector3.UP, true)
 		animation_player.play('walk')
 	else:
 		animation_player.play('idle')
-	look_at(position + velocity, Vector3.UP, true)
+
+
+func _on_nav_agent_velocity_computed(safe_velocity: Vector3) -> void:
+	velocity = safe_velocity
+	move_and_slide()