From fa26fb865ff1c1f6a107b45717c435e36abaa0e2 Mon Sep 17 00:00:00 2001 From: Pawel Lampe Date: Thu, 21 Oct 2021 22:25:06 +0200 Subject: [PATCH] Improve NavigationObstacle3D usability Fix NavigationObstacle3D to be attached to navigation map - without it the NavigationObstacle3D is not working. Replace radius approximation algorithm with simple "radius" property. --- doc/classes/NavigationObstacle2D.xml | 8 ++++ doc/classes/NavigationObstacle3D.xml | 8 ++++ scene/2d/navigation_obstacle_2d.cpp | 65 +++++++++++++++++++++++----- scene/2d/navigation_obstacle_2d.h | 17 +++++++- scene/3d/navigation_obstacle_3d.cpp | 65 +++++++++++++++++++++++----- scene/3d/navigation_obstacle_3d.h | 17 +++++++- 6 files changed, 156 insertions(+), 24 deletions(-) diff --git a/doc/classes/NavigationObstacle2D.xml b/doc/classes/NavigationObstacle2D.xml index 462532d49a..b73d7f2bec 100644 --- a/doc/classes/NavigationObstacle2D.xml +++ b/doc/classes/NavigationObstacle2D.xml @@ -8,4 +8,12 @@ + + + Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius. + + + The radius of the agent. Used only if [member estimate_radius] is set to false. + + diff --git a/doc/classes/NavigationObstacle3D.xml b/doc/classes/NavigationObstacle3D.xml index c0cb7befef..c126c0430f 100644 --- a/doc/classes/NavigationObstacle3D.xml +++ b/doc/classes/NavigationObstacle3D.xml @@ -8,4 +8,12 @@ + + + Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius. + + + The radius of the agent. Used only if [member estimate_radius] is set to false. + + diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp index 0a105826c0..4cfc7fe89e 100644 --- a/scene/2d/navigation_obstacle_2d.cpp +++ b/scene/2d/navigation_obstacle_2d.cpp @@ -34,19 +34,41 @@ #include "servers/navigation_server_2d.h" void NavigationObstacle2D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_estimate_radius", "estimate_radius"), &NavigationObstacle2D::set_estimate_radius); + ClassDB::bind_method(D_METHOD("is_radius_estimated"), &NavigationObstacle2D::is_radius_estimated); + ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle2D::set_radius); + ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle2D::get_radius); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "estimate_radius"), "set_estimate_radius", "is_radius_estimated"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,500,0.01"), "set_radius", "get_radius"); +} + +void NavigationObstacle2D::_validate_property(PropertyInfo &p_property) const { + if (p_property.name == "radius") { + if (estimate_radius) { + p_property.usage = PROPERTY_USAGE_NOEDITOR; + } + } } void NavigationObstacle2D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_READY: { + initialize_agent(); + parent_node2d = Object::cast_to(get_parent()); + if (parent_node2d != nullptr) { + // place agent on navigation map first or else the RVO agent callback creation fails silently later + NavigationServer2D::get_singleton()->agent_set_map(get_rid(), parent_node2d->get_world_2d()->get_navigation_map()); + } set_physics_process_internal(true); } break; case NOTIFICATION_EXIT_TREE: { + parent_node2d = nullptr; set_physics_process_internal(false); } break; case NOTIFICATION_PARENTED: { parent_node2d = Object::cast_to(get_parent()); - update_agent_shape(); + reevaluate_agent_radius(); } break; case NOTIFICATION_UNPARENTED: { parent_node2d = nullptr; @@ -78,7 +100,22 @@ TypedArray NavigationObstacle2D::get_configuration_warnings() const { return warnings; } -void NavigationObstacle2D::update_agent_shape() { +void NavigationObstacle2D::initialize_agent() { + NavigationServer2D::get_singleton()->agent_set_neighbor_dist(agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_max_neighbors(agent, 0); + NavigationServer2D::get_singleton()->agent_set_time_horizon(agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_max_speed(agent, 0.0); +} + +void NavigationObstacle2D::reevaluate_agent_radius() { + if (!estimate_radius) { + NavigationServer2D::get_singleton()->agent_set_radius(agent, radius); + } else if (parent_node2d) { + NavigationServer2D::get_singleton()->agent_set_radius(agent, estimate_agent_radius()); + } +} + +real_t NavigationObstacle2D::estimate_agent_radius() const { if (parent_node2d) { // Estimate the radius of this physics body real_t radius = 0.0; @@ -101,15 +138,21 @@ void NavigationObstacle2D::update_agent_shape() { Vector2 s = parent_node2d->get_global_scale(); radius *= MAX(s.x, s.y); - if (radius == 0.0) { - radius = 1.0; // Never a 0 radius + if (radius > 0.0) { + return radius; } - - // Initialize the Agent as an object - NavigationServer2D::get_singleton()->agent_set_neighbor_dist(agent, 0.0); - NavigationServer2D::get_singleton()->agent_set_max_neighbors(agent, 0); - NavigationServer2D::get_singleton()->agent_set_time_horizon(agent, 0.0); - NavigationServer2D::get_singleton()->agent_set_radius(agent, radius); - NavigationServer2D::get_singleton()->agent_set_max_speed(agent, 0.0); } + return 1.0; // Never a 0 radius +} + +void NavigationObstacle2D::set_estimate_radius(bool p_estimate_radius) { + estimate_radius = p_estimate_radius; + notify_property_list_changed(); + reevaluate_agent_radius(); +} + +void NavigationObstacle2D::set_radius(real_t p_radius) { + ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0."); + radius = p_radius; + reevaluate_agent_radius(); } diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h index 9cffc2c0c3..a5603f059f 100644 --- a/scene/2d/navigation_obstacle_2d.h +++ b/scene/2d/navigation_obstacle_2d.h @@ -40,8 +40,12 @@ class NavigationObstacle2D : public Node { Node2D *parent_node2d = nullptr; RID agent; + bool estimate_radius = true; + real_t radius = 1.0; + protected: static void _bind_methods(); + void _validate_property(PropertyInfo &p_property) const override; void _notification(int p_what); public: @@ -52,10 +56,21 @@ public: return agent; } + void set_estimate_radius(bool p_estimate_radius); + bool is_radius_estimated() const { + return estimate_radius; + } + void set_radius(real_t p_radius); + real_t get_radius() const { + return radius; + } + TypedArray get_configuration_warnings() const override; private: - void update_agent_shape(); + void initialize_agent(); + void reevaluate_agent_radius(); + real_t estimate_agent_radius() const; }; #endif diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp index 20ffc3b00e..ef15a6319e 100644 --- a/scene/3d/navigation_obstacle_3d.cpp +++ b/scene/3d/navigation_obstacle_3d.cpp @@ -35,19 +35,41 @@ #include "servers/navigation_server_3d.h" void NavigationObstacle3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_estimate_radius", "estimate_radius"), &NavigationObstacle3D::set_estimate_radius); + ClassDB::bind_method(D_METHOD("is_radius_estimated"), &NavigationObstacle3D::is_radius_estimated); + ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle3D::set_radius); + ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle3D::get_radius); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "estimate_radius"), "set_estimate_radius", "is_radius_estimated"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,100,0.01"), "set_radius", "get_radius"); +} + +void NavigationObstacle3D::_validate_property(PropertyInfo &p_property) const { + if (p_property.name == "radius") { + if (estimate_radius) { + p_property.usage = PROPERTY_USAGE_NOEDITOR; + } + } } void NavigationObstacle3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_READY: { + initialize_agent(); + parent_node3d = Object::cast_to(get_parent()); + if (parent_node3d != nullptr) { + // place agent on navigation map first or else the RVO agent callback creation fails silently later + NavigationServer3D::get_singleton()->agent_set_map(get_rid(), parent_node3d->get_world_3d()->get_navigation_map()); + } set_physics_process_internal(true); } break; case NOTIFICATION_EXIT_TREE: { + parent_node3d = nullptr; set_physics_process_internal(false); } break; case NOTIFICATION_PARENTED: { parent_node3d = Object::cast_to(get_parent()); - update_agent_shape(); + reevaluate_agent_radius(); } break; case NOTIFICATION_UNPARENTED: { parent_node3d = nullptr; @@ -86,7 +108,22 @@ TypedArray NavigationObstacle3D::get_configuration_warnings() const { return warnings; } -void NavigationObstacle3D::update_agent_shape() { +void NavigationObstacle3D::initialize_agent() { + NavigationServer3D::get_singleton()->agent_set_neighbor_dist(agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_max_neighbors(agent, 0); + NavigationServer3D::get_singleton()->agent_set_time_horizon(agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_max_speed(agent, 0.0); +} + +void NavigationObstacle3D::reevaluate_agent_radius() { + if (!estimate_radius) { + NavigationServer3D::get_singleton()->agent_set_radius(agent, radius); + } else if (parent_node3d) { + NavigationServer3D::get_singleton()->agent_set_radius(agent, estimate_agent_radius()); + } +} + +real_t NavigationObstacle3D::estimate_agent_radius() const { if (parent_node3d) { // Estimate the radius of this physics body real_t radius = 0.0; @@ -110,15 +147,21 @@ void NavigationObstacle3D::update_agent_shape() { Vector3 s = parent_node3d->get_global_transform().basis.get_scale(); radius *= MAX(s.x, MAX(s.y, s.z)); - if (radius == 0.0) { - radius = 1.0; // Never a 0 radius + if (radius > 0.0) { + return radius; } - - // Initialize the Agent as an object - NavigationServer3D::get_singleton()->agent_set_neighbor_dist(agent, 0.0); - NavigationServer3D::get_singleton()->agent_set_max_neighbors(agent, 0); - NavigationServer3D::get_singleton()->agent_set_time_horizon(agent, 0.0); - NavigationServer3D::get_singleton()->agent_set_radius(agent, radius); - NavigationServer3D::get_singleton()->agent_set_max_speed(agent, 0.0); } + return 1.0; // Never a 0 radius +} + +void NavigationObstacle3D::set_estimate_radius(bool p_estimate_radius) { + estimate_radius = p_estimate_radius; + notify_property_list_changed(); + reevaluate_agent_radius(); +} + +void NavigationObstacle3D::set_radius(real_t p_radius) { + ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0."); + radius = p_radius; + reevaluate_agent_radius(); } diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h index ab0b158303..12c813ab08 100644 --- a/scene/3d/navigation_obstacle_3d.h +++ b/scene/3d/navigation_obstacle_3d.h @@ -39,8 +39,12 @@ class NavigationObstacle3D : public Node { Node3D *parent_node3d = nullptr; RID agent; + bool estimate_radius = true; + real_t radius = 1.0; + protected: static void _bind_methods(); + void _validate_property(PropertyInfo &p_property) const override; void _notification(int p_what); public: @@ -51,10 +55,21 @@ public: return agent; } + void set_estimate_radius(bool p_estimate_radius); + bool is_radius_estimated() const { + return estimate_radius; + } + void set_radius(real_t p_radius); + real_t get_radius() const { + return radius; + } + TypedArray get_configuration_warnings() const override; private: - void update_agent_shape(); + void initialize_agent(); + void reevaluate_agent_radius(); + real_t estimate_agent_radius() const; }; #endif