From ac89de735e5264956477e5726306cbbaade49589 Mon Sep 17 00:00:00 2001
From: Sara <sara@saragerretsen.nl>
Date: Sat, 23 Mar 2024 22:19:35 +0100
Subject: [PATCH] feat: implemented avoidance for player characters

---
 godot/player_character.tscn |  2 ++
 src/player_character.cpp    | 24 +++++++++++++++++++++---
 2 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/godot/player_character.tscn b/godot/player_character.tscn
index 6a371aa..309d7cd 100644
--- a/godot/player_character.tscn
+++ b/godot/player_character.tscn
@@ -31,6 +31,8 @@ initial_state = &"FollowingPlayer"
 [node name="ProjectilePool" type="ProjectilePool" parent="."]
 
 [node name="NavigationAgent3D" type="NavigationAgent3D" parent="."]
+avoidance_enabled = true
+radius = 1.0
 
 [node name="CollisionShape3D" type="CollisionShape3D" parent="."]
 transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.802835, 0)
diff --git a/src/player_character.cpp b/src/player_character.cpp
index 520c844..799355c 100644
--- a/src/player_character.cpp
+++ b/src/player_character.cpp
@@ -2,18 +2,22 @@
 #include "projectile_pool.hpp"
 #include "state_machine.hpp"
 #include "utils/godot_macros.h"
+#include <cmath>
 #include <godot_cpp/classes/navigation_agent3d.hpp>
 #include <godot_cpp/classes/time.hpp>
+#include <godot_cpp/variant/callable.hpp>
 #include <godot_cpp/variant/utility_functions.hpp>
 
 namespace godot {
 void PlayerCharacter::_bind_methods() {
 #define CLASSNAME PlayerCharacter
     GDPROPERTY_HINTED(rotation_speed_curve, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Curve");
+    GDFUNCTION_ARGS(set_velocity_target, "value");
 }
 
 void PlayerCharacter::_enter_tree() { GDGAMEONLY();
     this->nav_agent = this->get_node<NavigationAgent3D>("NavigationAgent3D");
+    this->nav_agent->connect("velocity_computed", Callable(this, "set_velocity_target"));
     this->target_rotation = this->get_global_transform().get_basis().get_quaternion();
     this->health = this->get_node<Health>("Health");
     this->primary_weapon_pool = this->get_node<ProjectilePool>("ProjectilePool");
@@ -47,10 +51,16 @@ void PlayerCharacter::aim(Vector3 at) {
     // calculate the forward vector by normalized difference between player character and the target on the XZ plane
     Vector3 const position{this->weapon_muzzle->get_global_position()};
     Vector3 const forward{(Vector3{at.x, 0.f, at.z} - Vector3{position.x, 0.f, position.z}).normalized()};
+    this->aim_direction(forward);
+}
+
+void PlayerCharacter::aim_direction(Vector3 direction) {
+    if(direction.is_zero_approx())
+        return; // there is no situation in which rotation target would be 0
     // we always want up to be the global unit up
     Vector3 const up{0.f, 1.f, 0.f};
     // left is the cross product of the two
-    this->target_rotation = Basis{up.cross(forward), up, forward};
+    this->target_rotation = Basis{up.cross(direction), up, direction};
 }
 
 void PlayerCharacter::move_to(Vector3 to, float target_distance) {
@@ -72,7 +82,8 @@ void PlayerCharacter::set_firing(bool firing) {
 void PlayerCharacter::set_manual_mode(bool value) {
     this->mode_manual = value;
     ProcessMode const mode = value ?  ProcessMode::PROCESS_MODE_DISABLED : ProcessMode::PROCESS_MODE_PAUSABLE;
-    this->nav_agent->set_process_mode(mode);
+    //this->nav_agent->set_process_mode(mode);
+    this->nav_agent->set_avoidance_priority(value ? 1.f : 0.9f);
     this->state_machine->set_process_mode(mode);
 }
 
@@ -102,6 +113,10 @@ void PlayerCharacter::set_weapon_muzzle(Node3D *node) {
     this->weapon_muzzle = node;
 }
 
+void PlayerCharacter::set_velocity_target(Vector3 value) {
+    this->velocity_target = value;
+}
+
 Vector3 PlayerCharacter::get_velocity_target() const {
     return this->velocity_target;
 }
@@ -112,7 +127,10 @@ void PlayerCharacter::process_ai(double delta_time) {
     if(!this->nav_agent->is_navigation_finished() && distance >= target_distance_sqr) {
         Vector3 const target_position = this->nav_agent->get_next_path_position();
         Vector3 const direction = (target_position - this->get_global_position()).normalized();
-        this->move(direction);
+        if(this->nav_agent->get_avoidance_enabled())
+            this->nav_agent->set_velocity(direction * PlayerCharacter::WALK_SPEED);
+        else
+            this->move(direction);
     }
 }