71 lines
3.0 KiB
GDScript
71 lines
3.0 KiB
GDScript
extends RigidBody3D
|
|
class_name Unit
|
|
|
|
@export var movement_force: float = 100
|
|
@export var max_speed: float = 100
|
|
@export var minimum_progress_rate: float = 1.0
|
|
@export var stuck_time: float = 1.0
|
|
|
|
var target_velocity: Vector3 = Vector3()
|
|
var avoidance_velocity: Vector3 = Vector3()
|
|
var avoidance_timeout: float = 0.0
|
|
var last_distance_to_target: float = 0.0
|
|
var stuck_timer: float = 0.0
|
|
|
|
func _ready() -> void:
|
|
$NavigationAgent3D.connect("velocity_computed", avoidance_velocity_computed)
|
|
pass
|
|
|
|
func avoidance_velocity_computed(velocity: Vector3) -> void:
|
|
if velocity != target_velocity:
|
|
avoidance_velocity = velocity
|
|
avoidance_timeout = 0.5
|
|
|
|
func _process(delta: float) -> void:
|
|
if $NavigationAgent3D.is_target_reached() \
|
|
or $NavigationAgent3D.target_position.is_zero_approx() \
|
|
or !$NavigationAgent3D.is_target_reachable():
|
|
$NavigationAgent3D.target_position = NavigationServer3D.map_get_random_point(NavigationServer3D.get_maps()[0], 1, true)
|
|
last_distance_to_target = $NavigationAgent3D.distance_to_target()
|
|
else:
|
|
var distance_to_target: float = $NavigationAgent3D.distance_to_target()
|
|
var progress_rate: float = (last_distance_to_target - distance_to_target) / delta
|
|
$Label3D.text = "Prog: %f" % progress_rate
|
|
last_distance_to_target = distance_to_target
|
|
if progress_rate < minimum_progress_rate:
|
|
$Label3D.modulate = Color.RED
|
|
stuck_timer += delta
|
|
if stuck_timer >= stuck_time:
|
|
unstuck()
|
|
else:
|
|
$Label3D.modulate = Color.WHITE
|
|
stuck_timer = 0
|
|
#DebugDraw3D.draw_sphere($NavigationAgent3D.target_position, 0.5, Color.RED)
|
|
var next_point: Vector3 = $NavigationAgent3D.get_next_path_position()
|
|
#DebugDraw3D.draw_sphere(next_point, 0.1, Color.YELLOW)
|
|
var direction: Vector3 = (next_point - global_position).normalized()
|
|
#DebugDraw3D.draw_line(global_position, global_position + linear_velocity, Color.BLUE)
|
|
target_velocity = direction * max_speed
|
|
$NavigationAgent3D.velocity = target_velocity
|
|
#DebugDraw3D.draw_line(global_position, global_position + target_velocity, Color.MAGENTA)
|
|
#DebugDraw3D.draw_text(global_position + Vector3(0,1,0), "%f" % $NavigationAgent3D.distance_to_target())
|
|
|
|
func unstuck() -> void:
|
|
# teleport to next path point
|
|
global_position = $NavigationAgent3D.get_next_path_position()
|
|
stuck_timer = 0
|
|
|
|
func _physics_process(delta: float) -> void:
|
|
if $ShapeCast3D.is_colliding():
|
|
var actual_target_velocity: Vector3 = target_velocity
|
|
if avoidance_timeout > 0:
|
|
avoidance_timeout -= delta
|
|
actual_target_velocity = actual_target_velocity.slerp(avoidance_velocity, 0.25)
|
|
#DebugDraw3D.draw_line(global_position, global_position + actual_target_velocity, Color.ORANGE)
|
|
var force_direction: Vector3 = (actual_target_velocity-linear_velocity)
|
|
var normal: Vector3 = $ShapeCast3D.get_collision_normal(0)
|
|
#DebugDraw3D.draw_line(global_position, global_position + normal, Color.DODGER_BLUE)
|
|
var force: Vector3 = (force_direction * movement_force).slide(normal)
|
|
#DebugDraw3D.draw_line(global_position, global_position + force, Color.GREEN)
|
|
apply_central_force(force)
|