From 780e6038b81bcb961463977f02b0338bd83ed1e0 Mon Sep 17 00:00:00 2001 From: Francois Belair Date: Thu, 9 Jan 2020 12:26:35 -0500 Subject: [PATCH] Add FollowPath behavior --- project/src/GSTPath.gd | 120 +++++++++++++++++++++++++ project/src/behaviors/GSTFollowPath.gd | 47 ++++++++++ 2 files changed, 167 insertions(+) create mode 100644 project/src/GSTPath.gd create mode 100644 project/src/behaviors/GSTFollowPath.gd diff --git a/project/src/GSTPath.gd b/project/src/GSTPath.gd new file mode 100644 index 0000000..7fbb733 --- /dev/null +++ b/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) diff --git a/project/src/behaviors/GSTFollowPath.gd b/project/src/behaviors/GSTFollowPath.gd new file mode 100644 index 0000000..49699a3 --- /dev/null +++ b/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