Add FollowPath behavior

project/src/GSTPath.gd

@@ -0,0 +1,120 @@
+extends Reference
+class_name GSTPath
+
+
+var segments: Array
+var is_open: bool
+var path_length: float
+
+var nearest_point_on_segment: Vector3
+var nearest_point_on_path: Vector3
+
+
+func _init(waypoints: Array, is_open: = false) -> void:
+	self.is_open = is_open
+	_create_path(waypoints)
+	nearest_point_on_segment = waypoints[0]
+	nearest_point_on_path = waypoints[0]
+
+
+func get_start_point() -> Vector3:
+	return segments.front().begin
+
+
+func get_end_point() -> Vector3:
+	return segments.back().end
+
+
+func calculate_point_segment_distance_squared(a: Vector3, b: Vector3, c: Vector3) -> float:
+	nearest_point_on_segment = a
+	var ab: = b - a
+	var ab_length_squared: = ab.length_squared()
+	if ab_length_squared != 0:
+		var t = (c - a).dot(ab) / ab_length_squared
+		nearest_point_on_segment += ab * clamp(t, 0, 1)
+	
+	return nearest_point_on_segment.distance_squared_to(c)
+
+
+func calculate_distance(agent_current_position: Vector3, path_parameter: Dictionary) -> float:
+	var smallest_distance_squared: float = INF
+	var nearest_segment: GSTSegment
+	for i in range(segments.size()):
+		var segment: GSTSegment = segments[i]
+		var distance_squared: = calculate_point_segment_distance_squared(
+				segment.begin,
+				segment.end,
+				agent_current_position)
+		
+		if distance_squared < smallest_distance_squared:
+			nearest_point_on_path = nearest_point_on_segment
+			smallest_distance_squared = distance_squared
+			nearest_segment = segment
+			path_parameter.segment_index = i
+	
+	var length_on_path: = (
+		nearest_segment.cumulative_length - 
+		nearest_point_on_path.distance_to(nearest_segment.end))
+	
+	path_parameter.distance = length_on_path
+	
+	return length_on_path
+
+
+func calculate_target_position(param: Dictionary, target_distance: float) -> Vector3:
+	if is_open:
+		target_distance = clamp(target_distance, 0, path_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)
+	
+	var desired_segment: GSTSegment
+	for i in range(segments.size()):
+		var segment: GSTSegment = segments[i]
+		if segment.cumulative_length >= target_distance:
+			desired_segment = segment
+			break
+	
+	var distance: = desired_segment.cumulative_length - target_distance
+	
+	return (
+		(desired_segment.begin - desired_segment.end) *
+		(distance / desired_segment.length) + desired_segment.end)
+
+
+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.")
+	
+	segments = []
+	path_length = 0
+	var current: Vector3 = segments[0]
+	var previous: Vector3
+	
+	for i in range(1, waypoints.size(), 1):
+		previous = current
+		if i < waypoints.size():
+			current = waypoints[i]
+		elif is_open:
+			break
+		else:
+			current = waypoints[0]
+		var segment: = GSTSegment.new(previous, current)
+		path_length += segment.length
+		segment.cumulative_length = path_length
+		segments.append(segment)
+
+
+class GSTSegment:
+	var begin: Vector3
+	var end: Vector3
+	var length: float
+	var cumulative_length: float
+	
+	
+	func _init(begin: Vector3, end: Vector3) -> void:
+		self.begin = begin
+		self.end = end
+		length = begin.distance_to(end)

project/src/behaviors/GSTFollowPath.gd

@@ -0,0 +1,47 @@
+extends GSTArrive
+class_name GSTFollowPath
+# Produces a linear acceleration that moves the agent along the specified path.
+
+
+var path: GSTPath
+var path_offset: = 0.0
+
+var path_param: = {}
+
+var arrive_enabled: = true
+var prediction_time: = 0.0
+
+
+func _init(
+		agent: GSTSteeringAgent, 
+		path: GSTPath, 
+		path_offset: = 0.0, 
+		prediction_time: = 0.0).(agent, null) -> void:
+	self.path = path
+	self.path_offset = path_offset
+	self.prediction_time = prediction_time
+
+
+func _calculate_steering(acceleration: GSTTargetAcceleration) -> GSTTargetAcceleration:
+	var location: = (
+			agent.position if prediction_time == 0
+			else agent.position + (agent.linear_velocity * prediction_time))
+	
+	var distance: = path.calculate_distance(location, path_param)
+	var target_distance: = distance + path_offset
+	
+	var target_position: = path.calculate_target_position(path_param, target_distance)
+	
+	if arrive_enabled and path.is_open:
+		if path_offset >= 0:
+			if target_distance > path.length - deceleration_radius:
+				return _arrive(acceleration, target_position)
+		else:
+			if target_distance < deceleration_radius:
+				return _arrive(acceleration, target_position)
+	
+	acceleration.linear = (target_position - agent.position).normalized()
+	acceleration.linear *= agent.max_linear_acceleration
+	acceleration.angular = 0
+	
+	return acceleration