Add path following toy demo

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)

project/demos/follow_path/FollowPathDemo.gd

@@ -0,0 +1,8 @@
+extends Node2D
+
+
+onready var drawer: = $Drawer 
+
+
+func _ready() -> void:
+	$PathFollower.setup()

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 )

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

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={

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
+	length = 0
-	var current: Vector3 = _segments[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
+		length += segment.length
-		segment.cumulative_length = path_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)
+			target_distance = length + fmod(target_distance, length)
-		elif target_distance > path_length:
+		elif target_distance > length:
-			target_distance = fmod(target_distance, path_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 (

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)