92 lines
3.1 KiB
GDScript
92 lines
3.1 KiB
GDScript
extends CharacterBody3D
|
||
|
||
## NS-23: Nav waypoints + stored server goal for arrival.
|
||
## NS-19: bump collision is one convex frustum (plateau + slopes); no autostep.
|
||
|
||
const MOVE_SPEED: float = 5.0
|
||
const ARRIVE_EPS: float = 0.35
|
||
## Must be **below** NS-19 bump rise (~0.12–0.15) or we "arrive" while still on the plateau when the
|
||
## goal is on the floor beside the bump (small horiz offset + ~0.15 m vertical).
|
||
const VERT_ARRIVE_EPS: float = 0.055
|
||
const MAX_CLIMB_SPEED: float = 2.6
|
||
const MAX_DESCENT_SPEED: float = 2.2
|
||
|
||
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 pos := global_position
|
||
var delta_h: Vector3 = Vector3(point.x - pos.x, 0.0, point.z - pos.z)
|
||
# Full 3D toward a waypoint that is mostly *below* (same xz, on the slope) while standing on
|
||
# the plateau yields almost no horizontal speed, so you never reach the rim. Nav in xz + floor
|
||
# slide handles ramps and departure from bump tops.
|
||
if delta_h.length_squared() > 1e-8:
|
||
velocity = delta_h.normalized() * MOVE_SPEED
|
||
return
|
||
var dy: float = point.y - pos.y
|
||
if dy > 0.08:
|
||
var delta: Vector3 = point - pos
|
||
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
|
||
elif dy < -0.04:
|
||
# Goal is lower and almost same xz (e.g. floor under the plateau): still need rim escape.
|
||
var escape_h := Vector3(-global_transform.basis.z.x, 0.0, -global_transform.basis.z.z)
|
||
if escape_h.length_squared() < 1e-8:
|
||
escape_h = Vector3(1.0, 0.0, 0.0)
|
||
velocity = escape_h.normalized() * MOVE_SPEED
|
||
else:
|
||
velocity = Vector3.ZERO
|
||
|
||
|
||
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
|
||
|
||
# Do not follow NavigationAgent3D waypoints: from the bump plateau the first segment often
|
||
# drops straight down the mesh, and the agent map can disagree with presentation. Steer only
|
||
# toward the server goal in xz; move_and_slide + floor handles slopes and obstacles.
|
||
_steer_toward_world_point(_auth_walk_goal)
|
||
move_and_slide()
|