diff --git a/project/demos/follow_path/Drawer.gd b/project/demos/follow_path/Drawer.gd new file mode 100644 index 0000000..b14ee8d --- /dev/null +++ b/project/demos/follow_path/Drawer.gd @@ -0,0 +1,57 @@ +extends Node2D + + +signal path_established(points) + + +var active_points: = [] +var drawing: = false +var distance_threshold: = 100.0 + + +func _unhandled_input(event: InputEvent) -> void: + if event is InputEventMouseMotion: + if drawing: + active_points.append(event.position) + update() + elif event is InputEventMouseButton: + if event.pressed and event.button_index == BUTTON_LEFT: + active_points.clear() + active_points.append(event.position) + drawing = true + update() + elif not event.pressed: + drawing = false + _simplify() + + +func _draw() -> void: + if drawing: + for point in active_points: + draw_circle(point, 1, Color.red) + else: + if active_points.size() > 0: + draw_circle(active_points.front(), 2, Color.red) + draw_circle(active_points.back(), 2, Color.yellow) + for i in range(1, active_points.size()): + var start: Vector2 = active_points[i-1] + var end: Vector2 = active_points[i] + draw_line(start, end, Color.skyblue) + + +func _simplify() -> void: + var first: Vector2 = active_points.front() + var last: Vector2 = active_points.back() + var key: = first + var simplified_path: = [first] + for i in range(1, active_points.size()): + var point: Vector2 = active_points[i] + var distance: = point.distance_to(key) + if distance > distance_threshold: + key = point + simplified_path.append(key) + active_points = simplified_path + if active_points.back() != last: + active_points.append(last) + update() + emit_signal("path_established", active_points) diff --git a/project/demos/follow_path/FollowPathDemo.gd b/project/demos/follow_path/FollowPathDemo.gd new file mode 100644 index 0000000..62b2703 --- /dev/null +++ b/project/demos/follow_path/FollowPathDemo.gd @@ -0,0 +1,8 @@ +extends Node2D + + +onready var drawer: = $Drawer + + +func _ready() -> void: + $PathFollower.setup() diff --git a/project/demos/follow_path/FollowPathDemo.tscn b/project/demos/follow_path/FollowPathDemo.tscn index ee075a6..b5cf5a8 100644 --- a/project/demos/follow_path/FollowPathDemo.tscn +++ b/project/demos/follow_path/FollowPathDemo.tscn @@ -1,3 +1,26 @@ -[gd_scene format=2] +[gd_scene load_steps=6 format=2] + +[ext_resource path="res://demos/follow_path/Drawer.gd" type="Script" id=1] +[ext_resource path="res://assets/sprites/small_circle.png" type="Texture" id=2] +[ext_resource path="res://demos/follow_path/PathFollower.gd" type="Script" id=3] +[ext_resource path="res://demos/follow_path/FollowPathDemo.gd" type="Script" id=4] + +[sub_resource type="CircleShape2D" id=1] +radius = 16.0 [node name="FollowPathDemo" type="Node2D"] +script = ExtResource( 4 ) + +[node name="Drawer" type="Node2D" parent="."] +script = ExtResource( 1 ) + +[node name="PathFollower" type="KinematicBody2D" parent="."] +position = Vector2( 512, 300 ) +script = ExtResource( 3 ) + +[node name="CollisionShape2D" type="CollisionShape2D" parent="PathFollower"] +shape = SubResource( 1 ) + +[node name="Sprite" type="Sprite" parent="PathFollower"] +modulate = Color( 0.960784, 0.231373, 0.0392157, 1 ) +texture = ExtResource( 2 ) diff --git a/project/demos/follow_path/PathFollower.gd b/project/demos/follow_path/PathFollower.gd new file mode 100644 index 0000000..b66405a --- /dev/null +++ b/project/demos/follow_path/PathFollower.gd @@ -0,0 +1,43 @@ +extends KinematicBody2D + + +onready var agent: = GSTSteeringAgent.new() +onready var path: = GSTPath.new([ + Vector3(global_position.x, global_position.y, 0), + Vector3(global_position.x, global_position.y, 0) + ], true) +onready var follow: = GSTFollowPath.new(agent, path, 20, 0) + +var _velocity: = Vector2.ZERO +var _accel: = GSTTargetAcceleration.new() +var _valid: = false + + +func setup() -> void: + owner.drawer.connect("path_established", self, "_on_Drawer_path_established") + agent.max_linear_acceleration = 20 + agent.max_linear_speed = 200 + + +func _physics_process(delta: float) -> void: + if _valid: + _update_agent() + _accel = follow.calculate_steering(_accel) + _velocity += Vector2(_accel.linear.x, _accel.linear.y) + _velocity = _velocity.clamped(agent.max_linear_speed) + _velocity = move_and_slide(_velocity) + + +func _update_agent() -> void: + agent.position.x = global_position.x + agent.position.y = global_position.y + agent.linear_velocity.x = _velocity.x + agent.linear_velocity.y = _velocity.y + + +func _on_Drawer_path_established(points: Array) -> void: + var points3: = [] + for p in points: + points3.append(Vector3(p.x, p.y, 0)) + path.create_path(points3) + _valid = true diff --git a/project/project.godot b/project/project.godot index 8a29bcd..fc258be 100644 --- a/project/project.godot +++ b/project/project.godot @@ -161,10 +161,6 @@ _global_script_class_icons={ config/name="SteeringToolkit" config/icon="res://icon.png" -[display] - -window/size/always_on_top=true - [input] sf_left={ diff --git a/project/src/GSTPath.gd b/project/src/GSTPath.gd index 4277025..1f6e502 100644 --- a/project/src/GSTPath.gd +++ b/project/src/GSTPath.gd @@ -7,7 +7,7 @@ class_name GSTPath var open: bool -var path_length: float +var length: float var _segments: Array @@ -16,7 +16,7 @@ var _nearest_point_on_path: Vector3 func _init(waypoints: Array, is_open: = false) -> void: - self.is_open = is_open + open = is_open create_path(waypoints) _nearest_point_on_segment = waypoints[0] _nearest_point_on_path = waypoints[0] @@ -25,10 +25,11 @@ func _init(waypoints: Array, is_open: = false) -> void: func create_path(waypoints: Array) -> void: if not waypoints or waypoints.size() < 2: printerr("Waypoints cannot be null and must contain at least two (2) waypoints.") + return _segments = [] - path_length = 0 - var current: Vector3 = _segments[0] + length = 0 + var current: Vector3 = waypoints.front() var previous: Vector3 for i in range(1, waypoints.size(), 1): @@ -38,14 +39,16 @@ func create_path(waypoints: Array) -> void: elif open: break else: - current = waypoints[0] + current = waypoints.front() var segment: = GSTSegment.new(previous, current) - path_length += segment.length - segment.cumulative_length = path_length + length += segment.length + segment.cumulative_length = length _segments.append(segment) func calculate_distance(agent_current_position: Vector3, path_parameter: Dictionary) -> float: + if _segments.size() == 0: + return 0.0 var smallest_distance_squared: float = INF var nearest_segment: GSTSegment for i in range(_segments.size()): @@ -53,7 +56,8 @@ func calculate_distance(agent_current_position: Vector3, path_parameter: Diction var distance_squared: = _calculate_point_segment_distance_squared( segment.begin, segment.end, - agent_current_position) + agent_current_position + ) if distance_squared < smallest_distance_squared: _nearest_point_on_path = _nearest_point_on_segment @@ -72,12 +76,12 @@ func calculate_distance(agent_current_position: Vector3, path_parameter: Diction func calculate_target_position(param: Dictionary, target_distance: float) -> Vector3: if open: - target_distance = clamp(target_distance, 0, path_length) + target_distance = clamp(target_distance, 0, length) else: if target_distance < 0: - target_distance = path_length + fmod(target_distance, path_length) - elif target_distance > path_length: - target_distance = fmod(target_distance, path_length) + target_distance = length + fmod(target_distance, length) + elif target_distance > length: + target_distance = fmod(target_distance, length) var desired_segment: GSTSegment for i in range(_segments.size()): @@ -86,6 +90,9 @@ func calculate_target_position(param: Dictionary, target_distance: float) -> Vec desired_segment = segment break + if not desired_segment: + desired_segment = _segments.back() + var distance: = desired_segment.cumulative_length - target_distance return ( diff --git a/project/src/behaviors/GSTFollowPath.gd b/project/src/behaviors/GSTFollowPath.gd index 49699a3..d870fee 100644 --- a/project/src/behaviors/GSTFollowPath.gd +++ b/project/src/behaviors/GSTFollowPath.gd @@ -6,7 +6,7 @@ class_name GSTFollowPath var path: GSTPath var path_offset: = 0.0 -var path_param: = {} +var path_param: = {segment_index = 0, distance = 0} var arrive_enabled: = true var prediction_time: = 0.0 @@ -32,7 +32,7 @@ func _calculate_steering(acceleration: GSTTargetAcceleration) -> GSTTargetAccele var target_position: = path.calculate_target_position(path_param, target_distance) - if arrive_enabled and path.is_open: + if arrive_enabled and path.open: if path_offset >= 0: if target_distance > path.length - deceleration_radius: return _arrive(acceleration, target_position)