From f531e835fd75d26ec03ceb5515164a9adee6acf8 Mon Sep 17 00:00:00 2001
From: Francois Belair <razoric480@gmail.com>
Date: Thu, 19 Dec 2019 15:24:40 -0500
Subject: [PATCH] Implement LookWhereYouGo, and demo for Pursue

---
 docs/mvp.md                                   |   1 +
 .../demos/pursue_vs_seek/BoundaryManager.gd   |  68 ++++++++++++
 project/demos/pursue_vs_seek/Player.gd        | 101 ++++++++++++++++++
 .../demos/pursue_vs_seek/PursueVSSeek.tscn    |  42 ++++++++
 project/demos/pursue_vs_seek/Pursuer.gd       |  68 ++++++++++++
 project/demos/pursue_vs_seek/Ship.gd          |  29 +++++
 project/project.godot                         |   6 ++
 project/src/GSTAgentLocation.gd               |   1 -
 project/src/GSTSteeringBehavior.gd            |   1 -
 project/src/GSTTargetAcceleration.gd          |   1 -
 project/src/Utils.gd                          |   3 +
 project/src/behaviors/GSTLookWhereYouGo.gd    |  18 ++++
 project/src/behaviors/GSTMatchOrientation.gd  |  28 ++---
 13 files changed, 350 insertions(+), 17 deletions(-)
 create mode 100644 project/demos/pursue_vs_seek/BoundaryManager.gd
 create mode 100644 project/demos/pursue_vs_seek/Player.gd
 create mode 100644 project/demos/pursue_vs_seek/PursueVSSeek.tscn
 create mode 100644 project/demos/pursue_vs_seek/Pursuer.gd
 create mode 100644 project/demos/pursue_vs_seek/Ship.gd
 create mode 100644 project/src/behaviors/GSTLookWhereYouGo.gd

diff --git a/docs/mvp.md b/docs/mvp.md
index 2e738ff..059d985 100644
--- a/docs/mvp.md
+++ b/docs/mvp.md
@@ -8,6 +8,7 @@
 - Arrive
 - Pursue/Evade
 - Face
+- LookWhereYouGo
 - FollowPath
 - Separation
 - AvoidCollisions
diff --git a/project/demos/pursue_vs_seek/BoundaryManager.gd b/project/demos/pursue_vs_seek/BoundaryManager.gd
new file mode 100644
index 0000000..a78469e
--- /dev/null
+++ b/project/demos/pursue_vs_seek/BoundaryManager.gd
@@ -0,0 +1,68 @@
+extends Node2D
+"""
+Wraps the ships' positions around the world border, and controls their rendering clones.
+"""
+
+
+onready var _ships: = [$Player, $Pursuer, $Seeker]
+onready var ShipType: = preload("res://demos/pursue_vs_seek/Ship.gd")
+
+var _clones: = {}
+var _world_bounds: Vector2
+
+
+func _ready() -> void:
+	_world_bounds = Vector2(
+			ProjectSettings["display/window/size/width"],
+			ProjectSettings["display/window/size/height"]
+	)
+	
+	for i in range(_ships.size()):
+		var ship: Node2D = _ships[i]
+		var world_pos: = ship.position
+		
+		for i in range(3):
+			var ship_clone: = ShipType.new($Player/CollisionPolygon2D.polygon)
+			
+			ship_clone.position.x = world_pos.x if i == 1 else (world_pos.x - _world_bounds.x)
+			ship_clone.position.y = world_pos.y if i == 0 else (world_pos.y - _world_bounds.y)
+			ship_clone.rotation = ship.rotation
+			ship_clone.color = ship.color
+			ship_clone.tag = i
+			
+			add_child(ship_clone)
+			_clones[ship_clone] = ship
+
+
+func _physics_process(delta: float) -> void:
+	for clone in _clones.keys():
+		var original: Node2D = _clones[clone]
+		var world_pos: Vector2 = original.position
+		
+		if world_pos.y < 0:
+			original.position.y = _world_bounds.y + world_pos.y
+		elif world_pos.y > _world_bounds.y:
+			original.position.y = (world_pos.y - _world_bounds.y)
+		
+		if world_pos.x < 0:
+			original.position.x = _world_bounds.x + world_pos.x
+		elif world_pos.x > _world_bounds.x:
+			original.position.x = (world_pos.x - _world_bounds.x)
+		
+		var tag: int = clone.tag
+		if tag != 2:
+			if world_pos.x < _world_bounds.x/2:
+				clone.position.x = world_pos.x + _world_bounds.x
+			else:
+				clone.position.x = world_pos.x - _world_bounds.x
+		else:
+			clone.position.x = world_pos.x
+		
+		if tag != 0:
+			if world_pos.y < _world_bounds.y/2:
+				clone.position.y = world_pos.y + _world_bounds.y
+			else:
+				clone.position.y = world_pos.y - _world_bounds.y
+		else:
+			clone.position.y = world_pos.y
+		clone.rotation = original.rotation
diff --git a/project/demos/pursue_vs_seek/Player.gd b/project/demos/pursue_vs_seek/Player.gd
new file mode 100644
index 0000000..111c013
--- /dev/null
+++ b/project/demos/pursue_vs_seek/Player.gd
@@ -0,0 +1,101 @@
+extends "res://demos/pursue_vs_seek/Ship.gd"
+"""
+Controls the player ship's movements based on player input.
+"""
+
+
+onready var agent: = GSTSteeringAgent.new()
+
+export var thruster_strength: = 150.0
+export var side_thruster_strength: = 10.0
+export var max_velocity: = 150.0
+export var max_angular_velocity: = 2.0
+export var angular_drag: = 5.0
+export var linear_drag: = 100.0
+
+var _linear_velocity: = Vector2()
+var _angular_velocity: = 0.0
+
+
+func _physics_process(delta: float) -> void:
+	var movement: = _get_movement()
+	_angular_velocity = _calculate_angular_velocity(
+		movement.x,
+		_angular_velocity,
+		side_thruster_strength,
+		max_angular_velocity,
+		angular_drag,
+		delta
+	)
+	rotation += (_angular_velocity * delta)
+	
+	_linear_velocity = _calculate_linear_velocity(
+		movement.y,
+		_linear_velocity,
+		Vector2.UP.rotated(rotation),
+		linear_drag,
+		thruster_strength,
+		max_velocity,
+		delta
+	)
+	
+	_linear_velocity = move_and_slide(_linear_velocity)
+	
+	_update_agent(_linear_velocity, rotation)
+	update()
+
+
+func _calculate_angular_velocity(
+		horizontal_movement: float,
+		current_velocity: float,
+		thruster_strength: float,
+		max_velocity: float,
+		ship_drag: float,
+		delta: float) -> float:
+	
+	var velocity: = clamp(
+		current_velocity + thruster_strength * horizontal_movement * delta,
+		-max_velocity,
+		max_velocity
+	)
+	
+	if velocity > 0:
+		velocity -= ship_drag * delta
+	elif velocity < 0:
+		velocity += ship_drag * delta
+	if abs(velocity) < 0.01:
+		velocity = 0
+	
+	return velocity
+
+
+func _calculate_linear_velocity(
+		vertical_movement: float,
+		current_velocity: Vector2,
+		facing_direction: Vector2,
+		ship_drag: float,
+		strength: float,
+		max_speed: float,
+		delta: float) -> Vector2:
+	
+	var actual_strength: = 0.0
+	if vertical_movement > 0:
+		actual_strength = strength
+	elif vertical_movement < 0:
+		actual_strength = -strength/1.5
+	
+	var velocity: = current_velocity + facing_direction * actual_strength * delta
+	velocity -= current_velocity.normalized() * (ship_drag * delta)
+	
+	return velocity.clamped(max_speed)
+
+
+func _get_movement() -> Vector2:
+	return Vector2(	Input.get_action_strength("sf_right") - Input.get_action_strength("sf_left"),
+					Input.get_action_strength("sf_up") - Input.get_action_strength("sf_down"))
+
+
+func _update_agent(velocity: Vector2, orientation: float) -> void:
+	agent.position = Vector3(global_position.x, global_position.y, 0)
+	agent.linear_velocity = Vector3(velocity.x, velocity.y, 0)
+	agent.orientation = orientation
diff --git a/project/demos/pursue_vs_seek/PursueVSSeek.tscn b/project/demos/pursue_vs_seek/PursueVSSeek.tscn
new file mode 100644
index 0000000..7f3109b
--- /dev/null
+++ b/project/demos/pursue_vs_seek/PursueVSSeek.tscn
@@ -0,0 +1,42 @@
+[gd_scene load_steps=4 format=2]
+
+[ext_resource path="res://demos/pursue_vs_seek/Pursuer.gd" type="Script" id=1]
+[ext_resource path="res://demos/pursue_vs_seek/BoundaryManager.gd" type="Script" id=2]
+[ext_resource path="res://demos/pursue_vs_seek/Player.gd" type="Script" id=3]
+
+[node name="PursueVSSeek" type="Node2D"]
+__meta__ = {
+"_editor_description_": "Toy demo to demonstrate the use of the Pursue contrasted to the more naive Seek steering behavior."
+}
+
+[node name="BoundaryManager" type="Node2D" parent="."]
+script = ExtResource( 2 )
+
+[node name="Player" type="KinematicBody2D" parent="BoundaryManager"]
+position = Vector2( 49.2031, 556.936 )
+rotation = 1.5708
+collision_mask = 2
+script = ExtResource( 3 )
+color = Color( 1, 0, 0, 1 )
+
+[node name="CollisionPolygon2D" type="CollisionPolygon2D" parent="BoundaryManager/Player"]
+polygon = PoolVector2Array( 0, -30, -25, 25, 25, 25 )
+
+[node name="Pursuer" type="KinematicBody2D" parent="BoundaryManager"]
+position = Vector2( 868.495, 87.9043 )
+collision_layer = 2
+script = ExtResource( 1 )
+color = Color( 0.811765, 0.909804, 0.113725, 1 )
+
+[node name="CollisionPolygon2D" type="CollisionPolygon2D" parent="BoundaryManager/Pursuer"]
+polygon = PoolVector2Array( 0, -30, -25, 25, 25, 25 )
+
+[node name="Seeker" type="KinematicBody2D" parent="BoundaryManager"]
+position = Vector2( 821.24, 87.9043 )
+collision_layer = 2
+script = ExtResource( 1 )
+color = Color( 0.113725, 0.909804, 0.219608, 1 )
+use_seek = true
+
+[node name="CollisionPolygon2D" type="CollisionPolygon2D" parent="BoundaryManager/Seeker"]
+polygon = PoolVector2Array( 0, -30, -25, 25, 25, 25 )
diff --git a/project/demos/pursue_vs_seek/Pursuer.gd b/project/demos/pursue_vs_seek/Pursuer.gd
new file mode 100644
index 0000000..dd4d01b
--- /dev/null
+++ b/project/demos/pursue_vs_seek/Pursuer.gd
@@ -0,0 +1,68 @@
+extends "res://demos/pursue_vs_seek/Ship.gd"
+"""
+Represents a ship that chases after the player.
+"""
+
+
+onready var agent: = GSTSteeringAgent.new()
+onready var accel: = GSTTargetAcceleration.new()
+onready var player_agent: GSTSteeringAgent = owner.find_node("Player", true, false).agent
+
+export var use_seek: bool = false
+
+var _orient_behavior: GSTSteeringBehavior
+var _behavior: GSTSteeringBehavior
+
+var _linear_velocity: = Vector2()
+var _angular_velocity: = 0.0
+var _angular_drag: = 1.0
+
+
+func _ready() -> void:
+	_setup()
+
+
+func _setup() -> void:
+	if use_seek:
+		_behavior = GSTSeek.new(agent, player_agent)
+	else:
+		_behavior = GSTPursue.new(agent, player_agent, 2)
+	
+	_orient_behavior = GSTLookWhereYouGo.new(agent)
+	_orient_behavior.alignment_tolerance = 0.001
+	_orient_behavior.deceleration_radius = PI/2
+	
+	agent.max_angular_acceleration = 2
+	agent.max_angular_speed = 5
+	agent.max_linear_acceleration = 120
+	agent.max_linear_speed = 200
+	_update_agent()
+
+
+func _physics_process(delta: float) -> void:
+	accel = _orient_behavior.calculate_steering(accel)
+	_angular_velocity += accel.angular
+	
+	if _angular_velocity < 0:
+		_angular_velocity += _angular_drag * delta
+	elif _angular_velocity > 0:
+		_angular_velocity -= _angular_drag * delta
+	
+	rotation = rotation + _angular_velocity * delta
+	
+	accel = _behavior.calculate_steering(accel)
+	_linear_velocity = (
+		_linear_velocity + Vector2(accel.linear.x, accel.linear.y) * delta).clamped(agent.max_linear_speed)
+	_linear_velocity -= _linear_velocity * 1 * delta
+	_linear_velocity = move_and_slide(_linear_velocity)
+	
+	_update_agent()
+
+
+func _update_agent() -> void:
+	agent.position.x = global_position.x
+	agent.position.y = global_position.y
+	agent.orientation = rotation
+	agent.linear_velocity.x = _linear_velocity.x
+	agent.linear_velocity.y = _linear_velocity.y
+	agent.angular_velocity = _angular_velocity
diff --git a/project/demos/pursue_vs_seek/Ship.gd b/project/demos/pursue_vs_seek/Ship.gd
new file mode 100644
index 0000000..5ef0e9c
--- /dev/null
+++ b/project/demos/pursue_vs_seek/Ship.gd
@@ -0,0 +1,29 @@
+extends KinematicBody2D
+"""
+Draws a notched triangle based on the vertices of the ship's polygon collider.
+"""
+
+
+export var color: = Color()
+
+var tag: int = 0
+
+var _vertices: PoolVector2Array
+var _colors: PoolColorArray
+
+
+func _init(verts: = PoolVector2Array()) -> void:
+	_vertices = verts
+
+
+func _ready() -> void:
+	if not _vertices:
+		_vertices = $CollisionPolygon2D.polygon
+	var centroid: = (_vertices[0] + _vertices[1] + _vertices[2])/3
+	_vertices.insert(2, centroid)
+	for i in range(_vertices.size()):
+		_colors.append(color)
+
+
+func _draw() -> void:
+	draw_polygon(_vertices, _colors)
diff --git a/project/project.godot b/project/project.godot
index 92be641..8531524 100644
--- a/project/project.godot
+++ b/project/project.godot
@@ -34,6 +34,11 @@ _global_script_classes=[ {
 "language": "GDScript",
 "path": "res://src/behaviors/GSTFlee.gd"
 }, {
+"base": "GSTMatchOrientation",
+"class": "GSTLookWhereYouGo",
+"language": "GDScript",
+"path": "res://src/behaviors/GSTLookWhereYouGo.gd"
+}, {
 "base": "GSTSteeringBehavior",
 "class": "GSTMatchOrientation",
 "language": "GDScript",
@@ -75,6 +80,7 @@ _global_script_class_icons={
 "GSTEvade": "",
 "GSTFace": "",
 "GSTFlee": "",
+"GSTLookWhereYouGo": "",
 "GSTMatchOrientation": "",
 "GSTPursue": "",
 "GSTSeek": "",
diff --git a/project/src/GSTAgentLocation.gd b/project/src/GSTAgentLocation.gd
index 577e878..4e5720e 100644
--- a/project/src/GSTAgentLocation.gd
+++ b/project/src/GSTAgentLocation.gd
@@ -1,4 +1,3 @@
-extends Reference
 class_name GSTAgentLocation
 """
 Data type to represent an agent with a location and an orientation
diff --git a/project/src/GSTSteeringBehavior.gd b/project/src/GSTSteeringBehavior.gd
index 9ba6f54..16177ae 100644
--- a/project/src/GSTSteeringBehavior.gd
+++ b/project/src/GSTSteeringBehavior.gd
@@ -1,4 +1,3 @@
-extends Reference
 class_name GSTSteeringBehavior
 """
 Base class to calculate how an AI agent steers itself.
diff --git a/project/src/GSTTargetAcceleration.gd b/project/src/GSTTargetAcceleration.gd
index 6ce7259..82d7553 100644
--- a/project/src/GSTTargetAcceleration.gd
+++ b/project/src/GSTTargetAcceleration.gd
@@ -1,4 +1,3 @@
-extends Reference
 class_name GSTTargetAcceleration
 """
 A linear and angular amount of acceleration.
diff --git a/project/src/Utils.gd b/project/src/Utils.gd
index 6fad795..07f0181 100644
--- a/project/src/Utils.gd
+++ b/project/src/Utils.gd
@@ -1,4 +1,7 @@
 class_name Utils
+"""
+Useful math and utility functions to complement Godot's own.
+"""
 
 
 static func clmapedv3(vector: Vector3, limit: float) -> Vector3:
diff --git a/project/src/behaviors/GSTLookWhereYouGo.gd b/project/src/behaviors/GSTLookWhereYouGo.gd
new file mode 100644
index 0000000..26a261b
--- /dev/null
+++ b/project/src/behaviors/GSTLookWhereYouGo.gd
@@ -0,0 +1,18 @@
+extends GSTMatchOrientation
+class_name GSTLookWhereYouGo
+"""
+Calculates an angular acceleration to match an agent's orientation to its direction of travel.
+"""
+
+
+func _init(agent: GSTSteeringAgent).(agent, null) -> void:
+	pass
+
+
+func _calculate_steering(accel: GSTTargetAcceleration) -> GSTTargetAcceleration:
+	if agent.linear_velocity.length_squared() < agent.zero_linear_speed_threshold:
+		accel.set_zero()
+		return accel
+	else:
+		var orientation: = atan2(agent.linear_velocity.x, -agent.linear_velocity.y)
+		return _match_orientation(accel, orientation)
diff --git a/project/src/behaviors/GSTMatchOrientation.gd b/project/src/behaviors/GSTMatchOrientation.gd
index 90eaf03..182eefe 100644
--- a/project/src/behaviors/GSTMatchOrientation.gd
+++ b/project/src/behaviors/GSTMatchOrientation.gd
@@ -22,23 +22,23 @@ func _match_orientation(acceleration: GSTTargetAcceleration, desired_orientation
 	var rotation_size: = -rotation if rotation < 0 else rotation
 
 	if rotation_size <= alignment_tolerance:
-		return acceleration.set_zero()
+		acceleration.set_zero()
+	else:
+		var desired_rotation: = agent.max_angular_speed
 	
-	var desired_rotation: = agent.max_angular_speed
-
-	if rotation_size <= deceleration_radius:
-		desired_rotation *= rotation_size / deceleration_radius
+		if rotation_size <= deceleration_radius:
+			desired_rotation *= rotation_size / deceleration_radius
+		
+		desired_rotation *= rotation / rotation_size
 	
-	desired_rotation *= rotation / rotation_size
-
-	acceleration.angular = (desired_rotation - agent.angular_velocity) / time_to_reach
-
-	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.angular = (desired_rotation - agent.angular_velocity) / time_to_reach
+	
+		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
 	
-	acceleration.linear = Vector3.ZERO
-
 	return acceleration