feat: units use navigation obstacle avoidance

src/enemy_world_state.cpp

@@ -1,5 +1,6 @@
 #include "enemy_world_state.hpp"
 #include "entity_health.hpp"
+#include "godot_cpp/variant/utility_functions.hpp"
 #include "unit.hpp"
 #include "goap/goal.hpp"
 #include "utils/godot_macros.hpp"
@@ -23,38 +24,19 @@ void EnemyWorldState::_ready() { GDGAMEONLY();
 }
 
 void EnemyWorldState::on_awareness_entered(gd::Node3D *node) {
-    Unit *unit{gd::Object::cast_to<Unit>(node)};
-    if(unit == nullptr)
-        return;
-    if(unit == this->parent_unit)
-        return;
-    if(unit->get_entity_health()->get_is_dead())
-        return;
-    unit->get_entity_health()->connect("death", this->aware_unit_death.bind(unit));
-    this->known_enemies.push_back(unit);
-    this->try_set_target(this->select_target_from_known());
+    if(Unit *unit{gd::Object::cast_to<Unit>(node)}) {
+        gd::UtilityFunctions::print("!!! ", this->get_path(), ": ", node->get_path(), " entered awareness");
+        this->add_aware_unit(unit);
+    }
 }
 
 void EnemyWorldState::on_awareness_exited(gd::Node3D *node) {
     Unit *unit{gd::Object::cast_to<Unit>(node)};
-    if(unit == nullptr)
-        return;
-    if(this->known_enemies.has(unit)) {
-        unit->get_entity_health()->disconnect("death", this->aware_unit_death.bind(unit));
-        this->known_enemies.erase(unit);
-    }
-    if(unit == this->target_node) {
-        this->select_and_set_target();
-    }
+    this->remove_aware_unit(unit);
 }
 
 void EnemyWorldState::on_damaged(int remaining, int delta, Unit *source) {
-    if(!this->known_enemies.has(source)) {
-        source->get_entity_health()->connect("death", this->aware_unit_death.bind(source));
-        this->known_enemies.push_back(source);
-    }
-    if(!this->parent_unit->has_plan())
-        this->try_set_target(this->select_target_from_known());
+    this->add_aware_unit(source);
 }
 
 Unit *EnemyWorldState::select_target_from_known_with_priority(float *out_priority) {
@@ -75,6 +57,37 @@ Unit *EnemyWorldState::select_target_from_known() {
     return this->select_target_from_known_with_priority(&dummy);
 }
 
+void EnemyWorldState::add_aware_unit(Unit *unit) {
+    if(unit == nullptr)
+        return;
+    if(unit == this->parent_unit)
+        return;
+    if(this->known_enemies.has(unit))
+        return;
+    if(unit->get_entity_health()->get_is_dead())
+        return;
+    if(!this->get_is_unit_enemy(unit))
+        return;
+    unit->get_entity_health()->connect("death", this->aware_unit_death.bind(unit));
+    this->known_enemies.insert(0, unit);
+    if(!this->parent_unit->has_plan())
+        this->try_set_target(this->select_target_from_known());
+    else
+        gd::UtilityFunctions::print("!!! ", this->get_path(), " not replanning because a plan is already in motion");
+    gd::UtilityFunctions::print("!!! ", this->get_path(), " found ", unit->get_path());
+}
+
+void EnemyWorldState::remove_aware_unit(Unit *unit) {
+    if(unit == nullptr)
+        return;
+    if(!this->known_enemies.has(unit))
+        return;
+    unit->get_entity_health()->disconnect("death", this->aware_unit_death.bind(unit));
+    this->known_enemies.erase(unit);
+    if(unit == this->target_node)
+        this->select_and_set_target();
+}
+
 void EnemyWorldState::on_aware_unit_death(Unit *_, Unit *dead_entity) {
     this->on_awareness_exited(dead_entity);
 }

src/enemy_world_state.hpp

@@ -28,6 +28,8 @@ public:
     //! Shorthand for select_target_from_known_with_priority(nullptr)
     Unit *select_target_from_known();
 private:
+    void add_aware_unit(Unit *unit);
+    void remove_aware_unit(Unit *unit);
     void select_and_set_target();
     void on_aware_unit_death(Unit *_, Unit *dead_entity);
     float calculate_priority(Unit *target);

src/goap/planner.cpp

@@ -1,10 +1,10 @@
 #include "planner.hpp"
 #include "action_db.hpp"
-#include "godot_cpp/templates/hashfuncs.hpp"
 #include "utils/godot_macros.hpp"
 #include <cstdint>
 #include <godot_cpp/classes/engine.hpp>
 #include <godot_cpp/classes/global_constants.hpp>
+#include <godot_cpp/templates/hashfuncs.hpp>
 #include <godot_cpp/variant/utility_functions.hpp>
 
 namespace goap {
@@ -85,8 +85,8 @@ void Planner::_bind_methods() {
     GDPROPERTY_HINTED(actions_inspector, gd::Variant::ARRAY, gd::PROPERTY_HINT_ARRAY_TYPE, ActionDB::get_array_hint());
 }
 
-void Planner::_enter_tree() { GDGAMEONLY();
-    this->world_state = this->get_node<ActorWorldState>("../ActorWorldState");
+void Planner::_ready() { GDGAMEONLY();
+    this->world_state = this->get_node<ActorWorldState>("%ActorWorldState");
 }
 
 Plan Planner::plan_for_goal(gd::Ref<Goal> goal) {

src/goap/planner.hpp

@@ -53,7 +53,7 @@ class Planner : public gd::Node {
     GDCLASS(Planner, gd::Node);
     static void _bind_methods();
 public:
-    virtual void _enter_tree() override;
+    virtual void _ready() override;
 
     //! Compute a plan to achieve the passed `Goal`.
     Plan plan_for_goal(gd::Ref<Goal> goal);

src/register_types.cpp

@@ -21,11 +21,11 @@
 #include <godot_cpp/core/defs.hpp>
 #include <godot_cpp/godot.hpp>
 
-using namespace godot;
+namespace gd = godot;
 
-void initialize_gdextension_types(ModuleInitializationLevel p_level)
+void initialize_gdextension_types(gd::ModuleInitializationLevel p_level)
 {
-    if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) {
+    if (p_level != gd::MODULE_INITIALIZATION_LEVEL_SCENE) {
         return;
     }
     utils::godot_cpp_utils_register_types();
@@ -40,24 +40,24 @@ void initialize_gdextension_types(ModuleInitializationLevel p_level)
     goap::ActionDB::register_action<TankSelfHeal>();
     goap::ActionDB::register_action<TakeCover>();
 
-    ClassDB::register_class<goap::ActorWorldState>();
-    ClassDB::register_class<goap::Goal>();
-    ClassDB::register_class<goap::Planner>();
-    ClassDB::register_class<goap::State>();
+    gd::ClassDB::register_class<goap::ActorWorldState>();
+    gd::ClassDB::register_class<goap::Goal>();
+    gd::ClassDB::register_class<goap::Planner>();
+    gd::ClassDB::register_class<goap::State>();
 
-    ClassDB::register_class<MoveTo>();
-    ClassDB::register_class<Animate>();
-    ClassDB::register_class<Activate>();
+    gd::ClassDB::register_class<MoveTo>();
+    gd::ClassDB::register_class<Animate>();
+    gd::ClassDB::register_class<Activate>();
 
-    ClassDB::register_class<UnitWorldState>();
-    ClassDB::register_class<EnemyWorldState>();
-    ClassDB::register_class<GoalMarker>();
-    ClassDB::register_class<Unit>();
-    ClassDB::register_class<RTSGameMode>();
-    ClassDB::register_class<RTSPlayer>();
-    ClassDB::register_class<EntityHealth>();
-    ClassDB::register_class<NavMarker>();
-    ClassDB::register_class<NavRoom>();
+    gd::ClassDB::register_class<UnitWorldState>();
+    gd::ClassDB::register_class<EnemyWorldState>();
+    gd::ClassDB::register_class<GoalMarker>();
+    gd::ClassDB::register_class<Unit>();
+    gd::ClassDB::register_class<RTSGameMode>();
+    gd::ClassDB::register_class<RTSPlayer>();
+    gd::ClassDB::register_class<EntityHealth>();
+    gd::ClassDB::register_class<NavMarker>();
+    gd::ClassDB::register_class<NavRoom>();
 }
 
 extern "C"
@@ -65,9 +65,9 @@ extern "C"
     // Initialization
     GDExtensionBool GDE_EXPORT metro_rts_library_init(GDExtensionInterfaceGetProcAddress p_get_proc_address, GDExtensionClassLibraryPtr p_library, GDExtensionInitialization *r_initialization)
     {
-        GDExtensionBinding::InitObject init_obj(p_get_proc_address, p_library, r_initialization);
+        gd::GDExtensionBinding::InitObject init_obj(p_get_proc_address, p_library, r_initialization);
         init_obj.register_initializer(initialize_gdextension_types);
-        init_obj.set_minimum_library_initialization_level(MODULE_INITIALIZATION_LEVEL_SCENE);
+        init_obj.set_minimum_library_initialization_level(gd::MODULE_INITIALIZATION_LEVEL_SCENE);
 
         return init_obj.init();
     }

src/rts_states.cpp

@@ -7,37 +7,39 @@
 void MoveTo::_bind_methods() {}
 
 void MoveTo::_ready() {
-    this->parent_node3d = Object::cast_to<gd::Node3D>(this->get_parent());
+    this->parent_unit = Object::cast_to<Unit>(this->get_parent());
     this->agent = this->get_node<gd::NavigationAgent3D>("../NavigationAgent3D");
     if(this->target_node == nullptr) {
         this->state_ended();
         gd::UtilityFunctions::push_warning("failed to start MoveTo state due to missing target");
         return;
     }
-    this->agent->set_target_position(this->target_node->get_global_position());
+    this->agent->connect("velocity_computed", this->nav_velocity_computed);
     this->calculate_path();
-    gd::UtilityFunctions::print(this->parent_node3d->get_path(), " MoveTo ", this->target_node->get_path());
+    gd::UtilityFunctions::print(this->parent_unit->get_path(), " MoveTo ", this->target_node->get_path());
 }
 
 void MoveTo::_end_state() {
-    this->agent->set_target_position(this->parent_node3d->get_global_position());
+    this->agent->disconnect("velocity_computed", this->nav_velocity_computed);
+    this->agent->set_target_position(this->parent_unit->get_global_position());
 }
 
 void MoveTo::_process(double delta_time) {
-    gd::Vector3 const pos = this->parent_node3d->get_global_position();
+    gd::Vector3 const pos = this->parent_unit->get_global_position();
     gd::Vector3 const target = this->agent->get_next_path_position();
     gd::Vector3 const direction = (target - pos).normalized();
-    this->parent_node3d->set_global_position(pos + direction * delta_time);
-    this->parent_node3d->look_at(pos - gd::Vector3{direction.x, 0.f, direction.z});
-
+    this->agent->set_velocity(direction);
     bool const navigation_finished{this->agent->is_navigation_finished()};
     if(this->is_action_done_interrupt() || navigation_finished)
         this->state_ended();
-
     if((utils::time_seconds() - this->last_repath) > this->get_repath_interval())
         this->calculate_path();
 }
 
+void MoveTo::_physics_process(double delta) {
+    this->parent_unit->move_and_slide();
+}
+
 void MoveTo::calculate_path() {
     gd::Vector3 const target_pos{this->target_node->get_global_position()};
     this->agent->set_target_position(target_pos);
@@ -45,6 +47,14 @@ void MoveTo::calculate_path() {
     this->target_position_at_last = target_pos;
 }
 
+void MoveTo::on_velocity_computed(gd::Vector3 vel) {
+    gd::Vector3 const pos{this->parent_unit->get_global_position()};
+    gd::Vector3 const target_dir{(this->agent->get_next_path_position() - pos).normalized()};
+    // recalculate if avoidance is causing the motion to lock up
+    if(target_dir.dot(vel.normalized()) < 0.25f)
+        this->calculate_path();
+}
+
 float MoveTo::target_delta_position() const {
     return this->target_position_at_last
         .distance_to(this->target_node->get_global_position());
@@ -52,7 +62,7 @@ float MoveTo::target_delta_position() const {
 
 float MoveTo::distance_to_target() const {
     return this->target_position_at_last
-        .distance_to(this->parent_node3d->get_global_position());
+        .distance_to(this->parent_unit->get_global_position());
 }
 
 double MoveTo::get_repath_interval() const {

src/rts_states.hpp

@@ -1,6 +1,7 @@
 #ifndef RTS_STATES_HPP
 #define RTS_STATES_HPP
 
+#include "unit.hpp"
 #include "goap/state.hpp"
 #include <godot_cpp/classes/animation_player.hpp>
 #include <godot_cpp/classes/navigation_agent3d.hpp>
@@ -14,14 +15,17 @@ public:
     virtual void _ready() override;
     virtual void _end_state() override;
     virtual void _process(double delta_time) override;
+    virtual void _physics_process(double delta_time) override;
     void calculate_path();
+    void on_velocity_computed(gd::Vector3 vel);
     float target_delta_position() const;
     float distance_to_target() const;
     double get_repath_interval() const;
 public:
     gd::Node3D *target_node{nullptr};
 private:
-    gd::Node3D *parent_node3d{nullptr};
+    gd::Callable nav_velocity_computed{callable_mp(this, &MoveTo::on_velocity_computed)};
+    Unit *parent_unit{nullptr};
     gd::NavigationAgent3D *agent{nullptr};
     gd::Vector3 target_position_at_last{};
     double last_repath{0.0};

src/unit.cpp

@@ -2,6 +2,7 @@
 #include "entity_health.hpp"
 #include "goap/action.hpp"
 #include "goap/goal.hpp"
+#include "rts_states.hpp"
 #include "utils/godot_macros.hpp"
 #include <godot_cpp/classes/navigation_agent3d.hpp>
 #include <godot_cpp/variant/callable_method_pointer.hpp>
@@ -18,6 +19,7 @@ void Unit::_bind_methods() {
 
 void Unit::_enter_tree() { GDGAMEONLY();
     this->agent = this->get_node<gd::NavigationAgent3D>("%NavigationAgent3D");
+    this->agent->connect("velocity_computed", callable_mp(this, &Unit::on_velocity_computed));
     this->planner = this->get_node<goap::Planner>("%Planner");
     this->world_state = this->get_node<UnitWorldState>("%ActorWorldState");
     this->world_state->connect("attention_changed", callable_mp(this, &Unit::stop_plan));
@@ -111,16 +113,15 @@ EntityHealth *Unit::get_entity_health() {
     return this->health;
 }
 
-void Unit::set_goal_and_plan(gd::Ref<goap::Goal> goal) {
-    this->current_goal = goal;
-    if(goal.is_null()) {
-        this->current_plan.clear();
-    } else {
-        this->current_plan = this->planner->plan_for_goal(goal);
-        if(this->current_plan.is_empty()) {
-            this->current_goal.unref();
-        }
-    }
+void Unit::on_velocity_computed(gd::Vector3 vel) {
+    gd::Vector3 const pos{this->get_global_position()};
+    gd::Vector3 const target_dir{(this->agent->get_next_path_position() - pos).normalized()};
+    if(vel.x == 0 && vel.z == 0)
+        return;
+    if(gd::Object::cast_to<MoveTo>(this->state) == nullptr)
+        return;
+    this->set_velocity(vel);
+    this->look_at(pos - gd::Vector3{vel.x, 0.f, vel.z});
 }
 
 void Unit::destroy_state() {
@@ -166,3 +167,15 @@ void Unit::next_action() {
 void Unit::replan_goal() {
     this->begin_goal(this->current_goal);
 }
+
+void Unit::set_goal_and_plan(gd::Ref<goap::Goal> goal) {
+    this->current_goal = goal;
+    if(goal.is_null()) {
+        this->current_plan.clear();
+    } else {
+        this->current_plan = this->planner->plan_for_goal(goal);
+        if(this->current_plan.is_empty()) {
+            this->current_goal.unref();
+        }
+    }
+}

src/unit.hpp

@@ -44,11 +44,12 @@ public:
     bool has_plan() const;
     EntityHealth *get_entity_health();
 private:
-    void set_goal_and_plan(gd::Ref<goap::Goal> goal);
+    void on_velocity_computed(gd::Vector3 vel);
     void destroy_state();
     void state_finished();
     void next_action();
     void replan_goal();
+    void set_goal_and_plan(gd::Ref<goap::Goal> goal);
 private:
     gd::Callable on_state_finished{callable_mp(this, &Unit::state_finished)};
     gd::Callable on_plan_failed{callable_mp(this, &Unit::replan_goal)};

src/unit_world_state.cpp

@@ -14,6 +14,7 @@ void UnitWorldState::_bind_methods() {
     GDFUNCTION(get_can_see_target);
     GDFUNCTION(get_is_target_dead);
     GDFUNCTION(get_is_at_target);
+    GDFUNCTION(get_has_target);
     GDFUNCTION(get_target_node);
     GDFUNCTION(get_is_target_enemy);
     GDFUNCTION(get_is_in_melee_range);
@@ -24,6 +25,7 @@ void UnitWorldState::_bind_methods() {
 
 void UnitWorldState::_enter_tree() { GDGAMEONLY();
     this->parent_unit = gd::Object::cast_to<Unit>(this->get_parent());
+    gd::UtilityFunctions::print("!!! ", this->get_path(), ": is part of ", this->parent_unit->get_path());
     this->agent = this->get_node<gd::NavigationAgent3D>("%NavigationAgent3D");
     this->eye_location = this->get_node<gd::Node3D>("%Eyes");
     this->health = this->get_node<EntityHealth>("%EntityHealth");
@@ -61,8 +63,12 @@ bool UnitWorldState::get_is_at_target() const {
     return (target - current).length_squared() <= min_dist * min_dist;
 }
 
+bool UnitWorldState::get_has_target() const {
+    return this->target_node != nullptr;
+}
+
 bool UnitWorldState::get_is_target_dead() const {
-    if(EntityHealth *health{this->target_node->get_node<EntityHealth>("EntityHealth")})
+    if(EntityHealth *health{this->target_node->get_node<EntityHealth>("%EntityHealth")})
         return health->get_injury_current() <= 0.f;
     return false;
 }
@@ -73,6 +79,10 @@ bool UnitWorldState::get_is_target_unit() const {
 
 bool UnitWorldState::get_is_target_enemy() const {
     Unit *unit{gd::Object::cast_to<Unit>(this->target_node)};
+    return unit != nullptr && this->get_is_unit_enemy(unit);
+}
+
+bool UnitWorldState::get_is_unit_enemy(Unit *unit) const {
     return unit != nullptr
         && unit->get_team() != UnitTeam::Neutral
         && unit->get_team() != this->parent_unit->get_team();

src/unit_world_state.hpp

@@ -15,9 +15,11 @@ public:
     virtual void _enter_tree() override;
     bool get_can_see_target() const;
     bool get_is_at_target() const;
+    bool get_has_target() const;
     bool get_is_target_dead() const;
     bool get_is_target_unit() const;
     bool get_is_target_enemy() const;
+    bool get_is_unit_enemy(Unit *unit) const;
     bool get_is_in_melee_range() const;
     bool get_is_health_safe() const;
     gd::Vector3 get_parent_global_position() const;