135 lines
4.2 KiB
GDScript
135 lines
4.2 KiB
GDScript
extends CharacterBody3D
|
|
|
|
## NS-23: Nav agent suggests waypoints; arrival uses stored server goal only (not agent “finished”).
|
|
## NS-19 bumps: thin box colliders need **auto-step** (`test_move` up+forward); Y steering alone
|
|
## is not enough for vertical faces.
|
|
|
|
const MOVE_SPEED: float = 5.0
|
|
const ARRIVE_EPS: float = 0.35
|
|
const VERT_ARRIVE_EPS: float = 0.16
|
|
const MAX_CLIMB_SPEED: float = 2.6
|
|
const MAX_DESCENT_SPEED: float = 2.2
|
|
const DIRECT_APPROACH_RADIUS: float = 0.85
|
|
## NS-19 bumps ≤ ~0.15 m; probe slightly above for capsule + nav tolerance.
|
|
const STEP_MAX_HEIGHT: float = 0.32
|
|
const STEP_PROBE_FWD: float = 0.38
|
|
const STUCK_HORIZ_SPEED: float = 0.28
|
|
|
|
var _has_walk_goal: bool = false
|
|
var _auth_walk_goal: Vector3 = Vector3.ZERO
|
|
|
|
@onready var _nav_agent: NavigationAgent3D = $NavigationAgent3D
|
|
|
|
|
|
func _ready() -> void:
|
|
_nav_agent.set_target_position(global_position)
|
|
|
|
|
|
func set_authoritative_nav_goal(world_pos: Vector3) -> void:
|
|
_auth_walk_goal = world_pos
|
|
_has_walk_goal = true
|
|
_nav_agent.set_target_position(world_pos)
|
|
|
|
|
|
func clear_nav_goal() -> void:
|
|
velocity = Vector3.ZERO
|
|
_has_walk_goal = false
|
|
_nav_agent.set_target_position(global_position)
|
|
|
|
|
|
func snap_to_server(world_pos: Vector3) -> void:
|
|
global_position = world_pos
|
|
velocity = Vector3.ZERO
|
|
_has_walk_goal = false
|
|
_nav_agent.set_target_position(world_pos)
|
|
|
|
|
|
func _steer_toward_world_point(point: Vector3) -> void:
|
|
var delta: Vector3 = point - global_position
|
|
if delta.length_squared() < 1e-8:
|
|
velocity = Vector3.ZERO
|
|
return
|
|
var desired: Vector3 = delta.normalized() * MOVE_SPEED
|
|
desired.y = clampf(desired.y, -MAX_DESCENT_SPEED, MAX_CLIMB_SPEED)
|
|
velocity = desired
|
|
|
|
|
|
## Godot `test_move`: returns **true** if the motion would hit something (blocked).
|
|
func _try_autostep(delta: float) -> bool:
|
|
var to_goal_h: Vector3 = Vector3(
|
|
_auth_walk_goal.x - global_position.x, 0.0, _auth_walk_goal.z - global_position.z
|
|
)
|
|
if to_goal_h.length_squared() < 0.0225:
|
|
return false
|
|
var dir_h: Vector3 = to_goal_h.normalized()
|
|
var fwd_len: float = maxf(MOVE_SPEED * delta, 0.1)
|
|
var motion_fwd: Vector3 = dir_h * minf(fwd_len, STEP_PROBE_FWD)
|
|
var motion_up: Vector3 = Vector3(0.0, STEP_MAX_HEIGHT, 0.0)
|
|
var xf: Transform3D = global_transform
|
|
if not test_move(xf, motion_fwd):
|
|
return false
|
|
if test_move(xf, motion_up):
|
|
return false
|
|
var xf_up := Transform3D(xf.basis, xf.origin + motion_up)
|
|
if test_move(xf_up, motion_fwd):
|
|
return false
|
|
global_position += motion_up + motion_fwd
|
|
velocity.x = dir_h.x * MOVE_SPEED
|
|
velocity.z = dir_h.z * MOVE_SPEED
|
|
velocity.y = 0.0
|
|
return true
|
|
|
|
|
|
func _physics_process(delta: float) -> void:
|
|
if not _has_walk_goal:
|
|
velocity = Vector3.ZERO
|
|
move_and_slide()
|
|
return
|
|
|
|
var full_to_goal: Vector3 = _auth_walk_goal - global_position
|
|
var horiz_dist: float = Vector2(full_to_goal.x, full_to_goal.z).length()
|
|
var vert_err: float = absf(full_to_goal.y)
|
|
if horiz_dist <= ARRIVE_EPS and vert_err <= VERT_ARRIVE_EPS:
|
|
velocity = Vector3.ZERO
|
|
_has_walk_goal = false
|
|
_nav_agent.set_target_position(global_position)
|
|
move_and_slide()
|
|
return
|
|
|
|
var nav_map: RID = _nav_agent.get_navigation_map()
|
|
if NavigationServer3D.map_get_iteration_id(nav_map) == 0:
|
|
_steer_toward_world_point(_auth_walk_goal)
|
|
move_and_slide()
|
|
_maybe_autostep_after_slide(delta, full_to_goal, horiz_dist)
|
|
return
|
|
|
|
if horiz_dist <= DIRECT_APPROACH_RADIUS:
|
|
_steer_toward_world_point(_auth_walk_goal)
|
|
else:
|
|
var next_path_position: Vector3 = _nav_agent.get_next_path_position()
|
|
var to_next_h: Vector3 = Vector3(
|
|
next_path_position.x - global_position.x, 0.0, next_path_position.z - global_position.z
|
|
)
|
|
if to_next_h.length_squared() > 0.0025:
|
|
_steer_toward_world_point(next_path_position)
|
|
else:
|
|
_steer_toward_world_point(_auth_walk_goal)
|
|
|
|
move_and_slide()
|
|
_maybe_autostep_after_slide(delta, full_to_goal, horiz_dist)
|
|
|
|
|
|
func _maybe_autostep_after_slide(
|
|
delta: float, full_to_goal: Vector3, horiz_dist: float
|
|
) -> void:
|
|
if not _has_walk_goal:
|
|
return
|
|
if not is_on_floor() and not is_on_wall():
|
|
return
|
|
var hspd: float = Vector2(velocity.x, velocity.z).length()
|
|
var want_step: bool = hspd < STUCK_HORIZ_SPEED
|
|
if not want_step and is_on_wall():
|
|
want_step = full_to_goal.y > 0.03 and horiz_dist > 0.12
|
|
if want_step and _try_autostep(delta):
|
|
move_and_slide()
|