diff options
Diffstat (limited to 'modules/navigation/3d')
| -rw-r--r-- | modules/navigation/3d/godot_navigation_server_3d.cpp | 151 | ||||
| -rw-r--r-- | modules/navigation/3d/godot_navigation_server_3d.h | 10 | ||||
| -rw-r--r-- | modules/navigation/3d/nav_mesh_generator_3d.cpp | 66 | ||||
| -rw-r--r-- | modules/navigation/3d/nav_mesh_generator_3d.h | 15 | ||||
| -rw-r--r-- | modules/navigation/3d/navigation_mesh_generator.cpp | 2 |
5 files changed, 240 insertions, 4 deletions
diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 15de33f58f..61a128e004 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -1202,6 +1202,11 @@ COMMAND_1(free, RID, p_object) { } else if (obstacle_owner.owns(p_object)) { internal_free_obstacle(p_object); +#ifndef _3D_DISABLED + } else if (navmesh_generator_3d && navmesh_generator_3d->owns(p_object)) { + navmesh_generator_3d->free(p_object); +#endif // _3D_DISABLED + } else { ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed)."); } @@ -1381,11 +1386,157 @@ PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters & // add path postprocessing + if (r_query_result.path.size() > 2 && p_parameters.simplify_path) { + const LocalVector<uint32_t> &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + { + Vector3 *w = r_query_result.path.ptrw(); + const Vector3 *r = r_query_result.path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + int32_t *w = r_query_result.path_types.ptrw(); + const int32_t *r = r_query_result.path_types.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_types.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + TypedArray<RID> simplified_path_rids; + simplified_path_rids.resize(indices_count); + for (uint32_t i = 0; i < indices_count; i++) { + simplified_path_rids[i] = r_query_result.path_rids[i]; + } + r_query_result.path_rids = simplified_path_rids; + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + int64_t *w = r_query_result.path_owner_ids.ptrw(); + const int64_t *r = r_query_result.path_owner_ids.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_owner_ids.resize(indices_count); + } + } + // add path stats return r_query_result; } +RID GodotNavigationServer3D::source_geometry_parser_create() { +#ifndef _3D_DISABLED + if (navmesh_generator_3d) { + return navmesh_generator_3d->source_geometry_parser_create(); + } +#endif // _3D_DISABLED + return RID(); +} + +void GodotNavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { +#ifndef _3D_DISABLED + if (navmesh_generator_3d) { + navmesh_generator_3d->source_geometry_parser_set_callback(p_parser, p_callback); + } +#endif // _3D_DISABLED +} + +Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) { + if (p_path.size() <= 2) { + return p_path; + } + + p_epsilon = MAX(0.0, p_epsilon); + + LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + Vector<Vector3> simplified_path; + simplified_path.resize(indices_count); + + Vector3 *w = simplified_path.ptrw(); + const Vector3 *r = p_path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + + return simplified_path; +} + +LocalVector<uint32_t> GodotNavigationServer3D::get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon) { + p_epsilon = MAX(0.0, p_epsilon); + real_t squared_epsilon = p_epsilon * p_epsilon; + + LocalVector<bool> valid_points; + valid_points.resize(p_path.size()); + for (uint32_t i = 0; i < valid_points.size(); i++) { + valid_points[i] = false; + } + + simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points); + + int valid_point_index = 0; + + for (bool valid : valid_points) { + if (valid) { + valid_point_index += 1; + } + } + + LocalVector<uint32_t> simplified_path_indices; + simplified_path_indices.resize(valid_point_index); + valid_point_index = 0; + + for (uint32_t i = 0; i < valid_points.size(); i++) { + if (valid_points[i]) { + simplified_path_indices[valid_point_index] = i; + valid_point_index += 1; + } + } + + return simplified_path_indices; +} + +void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) { + r_valid_points[p_start_inx] = true; + r_valid_points[p_end_inx] = true; + + const Vector3 &start_point = p_points[p_start_inx]; + const Vector3 &end_point = p_points[p_end_inx]; + + Vector3 path_segment[2] = { start_point, end_point }; + + real_t point_max_distance = 0.0; + int point_max_index = 0; + + for (int i = p_start_inx; i < p_end_inx; i++) { + const Vector3 &checked_point = p_points[i]; + + const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment); + real_t distance_squared = closest_point.distance_squared_to(checked_point); + + if (distance_squared > point_max_distance) { + point_max_index = i; + point_max_distance = distance_squared; + } + } + + if (point_max_distance > p_epsilon) { + simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points); + simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points); + } +} + int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const { switch (p_info) { case INFO_ACTIVE_MAPS: { diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h index f7d991d47a..5ba7ed1088 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.h +++ b/modules/navigation/3d/godot_navigation_server_3d.h @@ -264,6 +264,16 @@ public: virtual void bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override; virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override; + virtual RID source_geometry_parser_create() override; + virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override; + + virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override; + +private: + static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points); + static LocalVector<uint32_t> get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon); + +public: COMMAND_1(free, RID, p_object); virtual void set_active(bool p_active) override; diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp index 8b43eba080..cc3bbdbf01 100644 --- a/modules/navigation/3d/nav_mesh_generator_3d.cpp +++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp @@ -45,12 +45,12 @@ #include "scene/resources/3d/convex_polygon_shape_3d.h" #include "scene/resources/3d/cylinder_shape_3d.h" #include "scene/resources/3d/height_map_shape_3d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" #include "scene/resources/3d/primitive_meshes.h" #include "scene/resources/3d/shape_3d.h" #include "scene/resources/3d/sphere_shape_3d.h" #include "scene/resources/3d/world_boundary_shape_3d.h" #include "scene/resources/navigation_mesh.h" -#include "scene/resources/navigation_mesh_source_geometry_data_3d.h" #include "modules/modules_enabled.gen.h" // For csg, gridmap. @@ -66,11 +66,14 @@ NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr; Mutex NavMeshGenerator3D::baking_navmesh_mutex; Mutex NavMeshGenerator3D::generator_task_mutex; +RWLock NavMeshGenerator3D::generator_rid_rwlock; bool NavMeshGenerator3D::use_threads = true; bool NavMeshGenerator3D::baking_use_multiple_threads = true; bool NavMeshGenerator3D::baking_use_high_priority_threads = true; HashSet<Ref<NavigationMesh>> NavMeshGenerator3D::baking_navmeshes; HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks; +RID_Owner<NavMeshGenerator3D::NavMeshGeometryParser3D> NavMeshGenerator3D::generator_parser_owner; +LocalVector<NavMeshGenerator3D::NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers; NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() { return singleton; @@ -85,7 +88,7 @@ NavMeshGenerator3D::NavMeshGenerator3D() { // Using threads might cause problems on certain exports or with the Editor on certain devices. // This is the main switch to turn threaded navmesh baking off should the need arise. - use_threads = baking_use_multiple_threads && !Engine::get_singleton()->is_editor_hint(); + use_threads = baking_use_multiple_threads; } NavMeshGenerator3D::~NavMeshGenerator3D() { @@ -139,6 +142,13 @@ void NavMeshGenerator3D::cleanup() { } generator_tasks.clear(); + generator_rid_rwlock.write_lock(); + for (NavMeshGeometryParser3D *parser : generator_parsers) { + generator_parser_owner.free(parser->self); + } + generator_parsers.clear(); + generator_rid_rwlock.write_unlock(); + generator_task_mutex.unlock(); baking_navmesh_mutex.unlock(); } @@ -254,6 +264,15 @@ void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh> #endif generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node); + generator_rid_rwlock.read_lock(); + for (const NavMeshGeometryParser3D *parser : generator_parsers) { + if (!parser->callback.is_valid()) { + continue; + } + parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node); + } + generator_rid_rwlock.read_unlock(); + if (p_recurse_children) { for (int i = 0; i < p_node->get_child_count(); i++) { generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children); @@ -700,7 +719,7 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation cfg.detailSampleDist = MAX(p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance(), 0.1f); cfg.detailSampleMaxError = p_navigation_mesh->get_cell_height() * p_navigation_mesh->get_detail_sample_max_error(); - if (p_navigation_mesh->get_border_size() > 0.0 && !Math::is_equal_approx(p_navigation_mesh->get_cell_size(), p_navigation_mesh->get_border_size())) { + if (p_navigation_mesh->get_border_size() > 0.0 && Math::fmod(p_navigation_mesh->get_border_size(), p_navigation_mesh->get_cell_size()) != 0.0) { WARN_PRINT("Property border_size is ceiled to cell_size voxel units and loses precision."); } if (!Math::is_equal_approx((float)cfg.walkableHeight * cfg.ch, p_navigation_mesh->get_agent_height())) { @@ -920,4 +939,45 @@ bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) { return ce.error == Callable::CallError::CALL_OK; } +RID NavMeshGenerator3D::source_geometry_parser_create() { + RWLockWrite write_lock(generator_rid_rwlock); + + RID rid = generator_parser_owner.make_rid(); + + NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(rid); + parser->self = rid; + + generator_parsers.push_back(parser); + + return rid; +} + +void NavMeshGenerator3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { + RWLockWrite write_lock(generator_rid_rwlock); + + NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_parser); + ERR_FAIL_NULL(parser); + + parser->callback = p_callback; +} + +bool NavMeshGenerator3D::owns(RID p_object) { + RWLockRead read_lock(generator_rid_rwlock); + return generator_parser_owner.owns(p_object); +} + +void NavMeshGenerator3D::free(RID p_object) { + RWLockWrite write_lock(generator_rid_rwlock); + + if (generator_parser_owner.owns(p_object)) { + NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_object); + + generator_parsers.erase(parser); + + generator_parser_owner.free(p_object); + } else { + ERR_PRINT("Attempted to free a NavMeshGenerator3D RID that did not exist (or was already freed)."); + } +} + #endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_mesh_generator_3d.h b/modules/navigation/3d/nav_mesh_generator_3d.h index 9c9b3bdefe..b46a1736e0 100644 --- a/modules/navigation/3d/nav_mesh_generator_3d.h +++ b/modules/navigation/3d/nav_mesh_generator_3d.h @@ -35,6 +35,7 @@ #include "core/object/class_db.h" #include "core/object/worker_thread_pool.h" +#include "core/templates/rid_owner.h" #include "modules/modules_enabled.gen.h" // For csg, gridmap. class Node; @@ -47,6 +48,14 @@ class NavMeshGenerator3D : public Object { static Mutex baking_navmesh_mutex; static Mutex generator_task_mutex; + static RWLock generator_rid_rwlock; + struct NavMeshGeometryParser3D { + RID self; + Callable callback; + }; + static RID_Owner<NavMeshGeometryParser3D> generator_parser_owner; + static LocalVector<NavMeshGeometryParser3D *> generator_parsers; + static bool use_threads; static bool baking_use_multiple_threads; static bool baking_use_high_priority_threads; @@ -102,6 +111,12 @@ public: static void bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable()); static bool is_baking(Ref<NavigationMesh> p_navigation_mesh); + static RID source_geometry_parser_create(); + static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback); + + static bool owns(RID p_object); + static void free(RID p_object); + NavMeshGenerator3D(); ~NavMeshGenerator3D(); }; diff --git a/modules/navigation/3d/navigation_mesh_generator.cpp b/modules/navigation/3d/navigation_mesh_generator.cpp index 8393896db1..54df42e266 100644 --- a/modules/navigation/3d/navigation_mesh_generator.cpp +++ b/modules/navigation/3d/navigation_mesh_generator.cpp @@ -32,7 +32,7 @@ #include "navigation_mesh_generator.h" -#include "scene/resources/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" #include "servers/navigation_server_3d.h" NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr; |