Implement Face, Evade Pursue, Arrive, Flee, Seek

The MVP document goes over the main behaviors that need to be
implemented prior to having a product.

default_env.tres

@@ -1,5 +1,7 @@
 [gd_resource type="Environment" load_steps=2 format=2]
+
 [sub_resource type="ProceduralSky" id=1]
+
 [resource]
 background_mode = 2
 background_sky = SubResource( 1 )

project.godot

@@ -8,9 +8,74 @@
 
 config_version=4
 
-_global_script_classes=[  ]
+_global_script_classes=[ {
+"base": "Reference",
+"class": "AgentLocation",
+"language": "GDScript",
+"path": "res://src/Steering/AgentLocation.gd"
+}, {
+"base": "SteeringBehavior",
+"class": "Arrive",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Arrive.gd"
+}, {
+"base": "Pursue",
+"class": "Evade",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Evade.gd"
+}, {
+"base": "MatchOrientation",
+"class": "Face",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Face.gd"
+}, {
+"base": "Seek",
+"class": "Flee",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Flee.gd"
+}, {
+"base": "SteeringBehavior",
+"class": "MatchOrientation",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/MatchOrientation.gd"
+}, {
+"base": "SteeringBehavior",
+"class": "Pursue",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Pursue.gd"
+}, {
+"base": "SteeringBehavior",
+"class": "Seek",
+"language": "GDScript",
+"path": "res://src/Steering/Behaviors/Seek.gd"
+}, {
+"base": "AgentLocation",
+"class": "SteeringAgent",
+"language": "GDScript",
+"path": "res://src/Steering/SteeringAgent.gd"
+}, {
+"base": "Reference",
+"class": "SteeringBehavior",
+"language": "GDScript",
+"path": "res://src/Steering/SteeringBehavior.gd"
+}, {
+"base": "Reference",
+"class": "TargetAcceleration",
+"language": "GDScript",
+"path": "res://src/Steering/TargetAcceleration.gd"
+} ]
 _global_script_class_icons={
-
+"AgentLocation": "",
+"Arrive": "",
+"Evade": "",
+"Face": "",
+"Flee": "",
+"MatchOrientation": "",
+"Pursue": "",
+"Seek": "",
+"SteeringAgent": "",
+"SteeringBehavior": "",
+"TargetAcceleration": ""
 }
 
 [application]

root.tscn

@@ -0,0 +1,3 @@
+[gd_scene format=2]
+
+[node name="Node2D" type="Node2D"]

src/Steering/AgentLocation.gd

@@ -0,0 +1,6 @@
+extends Reference
+class_name AgentLocation
+
+
+var position: = Vector3.ZERO
+var orientation: = 0.0

src/Steering/Behaviors/Arrive.gd

@@ -0,0 +1,46 @@
+extends SteeringBehavior
+class_name Arrive
+
+
+var target: AgentLocation
+var arrival_tolerance: float
+var deceleration_radius: float
+var time_to_target: = 0.1
+
+
+func _init(agent: SteeringAgent, target: AgentLocation).(agent) -> void:
+	self.target = target
+
+
+func _arrive(acceleration: TargetAcceleration, target_position: Vector3) -> TargetAcceleration:
+	var to_target: = target_position - agent.position
+	var distance: = to_target.length()
+	
+	if distance <= arrival_tolerance:
+		return acceleration.set_zero()
+	
+	var target_speed: = agent.max_linear_speed
+	
+	if distance <= deceleration_radius:
+		target_speed *= distance / deceleration_radius
+	
+	var target_velocity: = to_target * target_speed/distance
+	
+	target_velocity = (target_velocity - agent.linear_velocity) * 1.0 / time_to_target
+	
+	acceleration.linear = limit_length(target_velocity, agent.max_linear_acceleration)
+	acceleration.angular = 0
+	
+	return acceleration
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	return _arrive(acceleration, target.position)
+
+
+static func limit_length(vector: Vector3, limit: float) -> Vector3:
+	var len2: = vector.length_squared()
+	var limit2: = limit * limit
+	if len2 > limit2:
+		vector *= sqrt(limit2 / len2)
+	return vector

src/Steering/Behaviors/Evade.gd

@@ -0,0 +1,10 @@
+extends Pursue
+class_name Evade
+
+
+func _init(agent: SteeringAgent, target: SteeringAgent, max_predict_time: = 1.0).(agent, target, max_predict_time):
+	pass
+
+
+func get_max_linear_acceleration() -> float:
+	return -agent.max_linear_acceleration

src/Steering/Behaviors/Face.gd

@@ -0,0 +1,22 @@
+extends MatchOrientation
+class_name Face
+
+
+func _init(agent: SteeringAgent, target: AgentLocation).(agent, target) -> void:
+	pass
+
+
+func _face(acceleration: TargetAcceleration, target_position: Vector3) -> TargetAcceleration:
+	var to_target: = target_position - agent.position
+	
+	var distance_squared: = to_target.length_squared()
+	if distance_squared < agent.zero_linear_speed_threshold:
+		return acceleration.set_zero()
+	
+	var orientation = Vector3.UP.angle_to(to_target)
+	
+	return _match_orientation(acceleration, orientation)
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	return _face(acceleration, target.position)

src/Steering/Behaviors/Flee.gd

@@ -0,0 +1,13 @@
+extends Seek
+class_name Flee
+
+
+func _init(agent: SteeringAgent, target: AgentLocation).(agent, target) -> void:
+	pass
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	acceleration.linear = (agent.position - target.position).normalized() * agent.max_linear_acceleration
+	acceleration.angular = 0
+	
+	return acceleration

src/Steering/Behaviors/MatchOrientation.gd

@@ -0,0 +1,42 @@
+extends SteeringBehavior
+class_name MatchOrientation
+
+
+var target: AgentLocation
+var alignment_tolerance: float
+var deceleration_radius: float
+var time_to_target: float = 0.1
+
+
+func _init(agent: SteeringAgent, target: AgentLocation).(agent) -> void:
+	self.target = target
+
+
+func _match_orientation(acceleration: TargetAcceleration, target_orientation: float) -> TargetAcceleration:
+	var rotation: = wrapf(target_orientation - agent.orientation, -PI, PI)
+
+	var rotation_size: = -rotation if rotation < 0 else rotation
+
+	if rotation_size <= alignment_tolerance:
+		return acceleration.set_zero()
+	
+	var target_rotation: = agent.max_angular_speed
+
+	if rotation_size <= deceleration_radius:
+		target_rotation *= rotation_size / deceleration_radius
+	
+	target_rotation *= rotation / rotation_size
+
+	acceleration.angular = (target_rotation - agent.angular_velocity) / time_to_target
+
+	var limited_acceleration: = -acceleration.angular if acceleration.angular < 0 else acceleration.angular
+	if limited_acceleration > agent.max_angular_acceleration:
+		acceleration.angular *= agent.max_angular_acceleration / limited_acceleration
+	
+	acceleration.linear = Vector3.ZERO
+
+	return acceleration
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	return _match_orientation(acceleration, target.orientation)

src/Steering/Behaviors/Pursue.gd

@@ -0,0 +1,35 @@
+extends SteeringBehavior
+class_name Pursue
+
+
+var target: SteeringAgent
+var max_predict_time: float
+
+
+func _init(agent: SteeringAgent, target: SteeringAgent, max_predict_time: = 1.0).(agent) -> void:
+	self.target = target
+	self.max_predict_time = max_predict_time
+
+
+func get_max_linear_acceleration() -> float:
+	return agent.max_linear_acceleration
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	var target_position: = target.position
+	var distance_squared: = (target_position - agent.position).length_squared()
+	
+	var speed_squared: = agent.linear_velocity.length_squared()
+	var predict_time: = max_predict_time
+	
+	if speed_squared > 0:
+		var predict_time_squared: = distance_squared / speed_squared
+		if predict_time_squared < max_predict_time * max_predict_time:
+			predict_time = sqrt(predict_time_squared)
+	
+	acceleration.linear = ((target_position + (target.linear_velocity * predict_time))-agent.position).normalized()
+	acceleration.linear *= agent.max_linear_acceleration
+	
+	acceleration.angular = 0
+	
+	return acceleration

src/Steering/Behaviors/Seek.gd

@@ -0,0 +1,16 @@
+extends SteeringBehavior
+class_name Seek
+
+
+var target: AgentLocation
+
+
+func _init(agent: SteeringAgent, target: AgentLocation).(agent) -> void:
+	self.target = target
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	acceleration.linear = (target.position - agent.position).normalized() * agent.max_linear_acceleration
+	acceleration.angular = 0
+	
+	return acceleration

src/Steering/SteeringAgent.gd

@@ -0,0 +1,12 @@
+extends AgentLocation
+class_name SteeringAgent
+
+
+var zero_linear_speed_threshold: = 0.01
+var max_linear_speed: = 0.0
+var max_linear_acceleration: = 0.0
+var max_angular_speed: = 0.0
+var max_angular_acceleration: = 0.0
+var linear_velocity: = Vector3.ZERO
+var angular_velocity: = 0.0
+var bounding_radius: = 0.0

src/Steering/SteeringBehavior.gd

@@ -0,0 +1,18 @@
+extends Reference
+class_name SteeringBehavior
+
+
+var enabled: = true
+var agent: SteeringAgent
+
+
+func _init(agent: SteeringAgent) -> void:
+	self.agent = agent
+
+
+func calculate_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	return _calculate_internal_steering(acceleration) if enabled else acceleration.zero()
+
+
+func _calculate_internal_steering(acceleration: TargetAcceleration) -> TargetAcceleration:
+	return acceleration.set_zero()

src/Steering/TargetAcceleration.gd

@@ -0,0 +1,14 @@
+extends Reference
+class_name TargetAcceleration
+
+
+var linear: = Vector3.ZERO
+var angular: = 0.0
+
+
+func set_zero() -> TargetAcceleration:
+	linear.x = 0.0
+	linear.y = 0.0
+	linear.z = 0.0
+	angular = 0.0
+	return self