140 lines
4.5 KiB
GDScript
140 lines
4.5 KiB
GDScript
extends CharacterBody3D
|
|
|
|
## NS-23: Follow server move target on walkable geometry.
|
|
##
|
|
## Horizontal velocity toward nav waypoint or goal only; `velocity.y` stays 0. Height along ramps
|
|
## and steps comes from `move_and_slide` + floor_* on CharacterBody3D. Move **legality** (step,
|
|
## distance, bounds) is server MoveCommand only — not reimplemented here.
|
|
##
|
|
## Nav for detours; short local step-down (feet vs goal Y, not body origin) may bypass waypoints.
|
|
|
|
const MOVE_SPEED: float = 5.0
|
|
const ARRIVE_EPS: float = 0.35
|
|
const VERT_ARRIVE_EPS: float = 0.055
|
|
const DIRECT_APPROACH_RADIUS: float = 0.85
|
|
const FLOOR_PROBE_DOWN: float = 4.0
|
|
const STEP_OFF_FLOOR_MARGIN: float = 0.04
|
|
## Only NS-19-sized step downs (not cliffs); allows a wider horiz cap without bee-lining big drops.
|
|
const STEP_OFF_MIN_DROP: float = 0.055
|
|
const STEP_OFF_MAX_DROP: float = 0.24
|
|
const STEP_OFF_MAX_HORIZ: float = 3.0
|
|
const FLOOR_PROBE_BODY_Y_OFFSET: float = -0.55
|
|
## If next waypoint is within this xz of us, steer to goal (rim / underfoot waypoints).
|
|
const NEXT_WAYPOINT_MIN_HORIZ_SQ: float = 0.01
|
|
|
|
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 _ray_floor_y_at(from: Vector3) -> float:
|
|
var space: PhysicsDirectSpaceState3D = get_world_3d().direct_space_state
|
|
var to: Vector3 = from + Vector3(0.0, -FLOOR_PROBE_DOWN, 0.0)
|
|
var q: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.create(from, to)
|
|
q.collision_mask = 1
|
|
q.exclude = [get_rid()]
|
|
var hit: Dictionary = space.intersect_ray(q)
|
|
if hit.is_empty():
|
|
return from.y
|
|
var pv: Variant = hit.get("position")
|
|
if pv is Vector3:
|
|
return (pv as Vector3).y
|
|
return from.y
|
|
|
|
|
|
func _floor_y_under_body() -> float:
|
|
var y_center: float = _ray_floor_y_at(global_position)
|
|
var lower: Vector3 = global_position + Vector3(0.0, FLOOR_PROBE_BODY_Y_OFFSET, 0.0)
|
|
var y_lower: float = _ray_floor_y_at(lower)
|
|
return maxf(y_center, y_lower)
|
|
|
|
|
|
func _nav_short_step_down_use_goal_direct() -> bool:
|
|
var floor_y: float = _floor_y_under_body()
|
|
if _auth_walk_goal.y >= floor_y - STEP_OFF_FLOOR_MARGIN:
|
|
return false
|
|
var drop: float = floor_y - _auth_walk_goal.y
|
|
if drop < STEP_OFF_MIN_DROP or drop > STEP_OFF_MAX_DROP:
|
|
return false
|
|
var hd: float = Vector2(
|
|
_auth_walk_goal.x - global_position.x, _auth_walk_goal.z - global_position.z
|
|
).length()
|
|
if hd > STEP_OFF_MAX_HORIZ:
|
|
return false
|
|
return true
|
|
|
|
|
|
func _set_horizontal_velocity_toward(point: Vector3) -> void:
|
|
var pos := global_position
|
|
var dh: Vector3 = Vector3(point.x - pos.x, 0.0, point.z - pos.z)
|
|
if dh.length_squared() < 1e-10:
|
|
dh = Vector3(1.0, 0.0, 0.0)
|
|
velocity = dh.normalized() * MOVE_SPEED
|
|
velocity.y = 0.0
|
|
|
|
|
|
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:
|
|
_set_horizontal_velocity_toward(_auth_walk_goal)
|
|
move_and_slide()
|
|
return
|
|
|
|
if _nav_short_step_down_use_goal_direct():
|
|
_set_horizontal_velocity_toward(_auth_walk_goal)
|
|
move_and_slide()
|
|
return
|
|
|
|
if horiz_dist <= DIRECT_APPROACH_RADIUS:
|
|
_set_horizontal_velocity_toward(_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() > NEXT_WAYPOINT_MIN_HORIZ_SQ:
|
|
_set_horizontal_velocity_toward(next_path_position)
|
|
else:
|
|
_set_horizontal_velocity_toward(_auth_walk_goal)
|
|
|
|
move_and_slide()
|