diff options
Diffstat (limited to 'modules/navigation')
| -rw-r--r-- | modules/navigation/2d/godot_navigation_server_2d.cpp | 5 | ||||
| -rw-r--r-- | modules/navigation/2d/godot_navigation_server_2d.h | 1 | ||||
| -rw-r--r-- | modules/navigation/2d/nav_mesh_generator_2d.cpp | 134 | ||||
| -rw-r--r-- | modules/navigation/3d/godot_navigation_server_3d.cpp | 48 | ||||
| -rw-r--r-- | modules/navigation/3d/godot_navigation_server_3d.h | 4 | ||||
| -rw-r--r-- | modules/navigation/3d/nav_mesh_generator_3d.cpp | 134 | ||||
| -rw-r--r-- | modules/navigation/3d/nav_mesh_queries_3d.cpp | 715 | ||||
| -rw-r--r-- | modules/navigation/3d/nav_mesh_queries_3d.h | 54 | ||||
| -rw-r--r-- | modules/navigation/editor/navigation_mesh_editor_plugin.cpp | 5 | ||||
| -rw-r--r-- | modules/navigation/editor/navigation_mesh_editor_plugin.h | 1 | ||||
| -rw-r--r-- | modules/navigation/nav_base.h | 2 | ||||
| -rw-r--r-- | modules/navigation/nav_map.cpp | 645 | ||||
| -rw-r--r-- | modules/navigation/nav_map.h | 22 | ||||
| -rw-r--r-- | modules/navigation/nav_region.cpp | 172 | ||||
| -rw-r--r-- | modules/navigation/nav_region.h | 23 | ||||
| -rw-r--r-- | modules/navigation/nav_utils.h | 178 |
16 files changed, 1265 insertions, 878 deletions
diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index bf69adc14c..2af125d434 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -318,6 +318,11 @@ int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid); Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int); Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int); +Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const { + Vector3 result = NavigationServer3D::get_singleton()->region_get_closest_point(p_region, v2_to_v3(p_point)); + return v3_to_v2(result); +} + Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const { Vector3 result = NavigationServer3D::get_singleton()->region_get_random_point(p_region, p_navigation_layers, p_uniformly); return v3_to_v2(result); diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h index ea77fa5e6e..1579ca2907 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.h +++ b/modules/navigation/2d/godot_navigation_server_2d.h @@ -101,6 +101,7 @@ public: virtual int region_get_connections_count(RID p_region) const override; virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override; virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override; + virtual Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const override; virtual Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override; virtual RID link_create() override; diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation/2d/nav_mesh_generator_2d.cpp index 2198158f9c..78983187c7 100644 --- a/modules/navigation/2d/nav_mesh_generator_2d.cpp +++ b/modules/navigation/2d/nav_mesh_generator_2d.cpp @@ -87,57 +87,55 @@ void NavMeshGenerator2D::sync() { return; } - baking_navmesh_mutex.lock(); - generator_task_mutex.lock(); + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + { + MutexLock generator_task_lock(generator_task_mutex); - LocalVector<WorkerThreadPool::TaskID> finished_task_ids; + LocalVector<WorkerThreadPool::TaskID> finished_task_ids; - for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) { - if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) { - WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); - finished_task_ids.push_back(E.key); + for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) { + if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); + finished_task_ids.push_back(E.key); - NavMeshGeneratorTask2D *generator_task = E.value; - DEV_ASSERT(generator_task->status == NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED); + NavMeshGeneratorTask2D *generator_task = E.value; + DEV_ASSERT(generator_task->status == NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED); - baking_navmeshes.erase(generator_task->navigation_mesh); - if (generator_task->callback.is_valid()) { - generator_emit_callback(generator_task->callback); + baking_navmeshes.erase(generator_task->navigation_mesh); + if (generator_task->callback.is_valid()) { + generator_emit_callback(generator_task->callback); + } + memdelete(generator_task); } - memdelete(generator_task); } - } - for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) { - generator_tasks.erase(finished_task_id); + for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) { + generator_tasks.erase(finished_task_id); + } } - - generator_task_mutex.unlock(); - baking_navmesh_mutex.unlock(); } void NavMeshGenerator2D::cleanup() { - baking_navmesh_mutex.lock(); - generator_task_mutex.lock(); + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + { + MutexLock generator_task_lock(generator_task_mutex); - baking_navmeshes.clear(); + baking_navmeshes.clear(); - for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) { - WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); - NavMeshGeneratorTask2D *generator_task = E.value; - memdelete(generator_task); - } - generator_tasks.clear(); + for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); + NavMeshGeneratorTask2D *generator_task = E.value; + memdelete(generator_task); + } + generator_tasks.clear(); - generator_rid_rwlock.write_lock(); - for (NavMeshGeometryParser2D *parser : generator_parsers) { - generator_parser_owner.free(parser->self); + generator_rid_rwlock.write_lock(); + for (NavMeshGeometryParser2D *parser : generator_parsers) { + generator_parser_owner.free(parser->self); + } + generator_parsers.clear(); + generator_rid_rwlock.write_unlock(); } - generator_parsers.clear(); - generator_rid_rwlock.write_unlock(); - - generator_task_mutex.unlock(); - baking_navmesh_mutex.unlock(); } void NavMeshGenerator2D::finish() { @@ -212,7 +210,7 @@ void NavMeshGenerator2D::bake_from_source_geometry_data_async(Ref<NavigationPoly baking_navmeshes.insert(p_navigation_mesh); baking_navmesh_mutex.unlock(); - generator_task_mutex.lock(); + MutexLock generator_task_lock(generator_task_mutex); NavMeshGeneratorTask2D *generator_task = memnew(NavMeshGeneratorTask2D); generator_task->navigation_mesh = p_navigation_mesh; generator_task->source_geometry_data = p_source_geometry_data; @@ -220,14 +218,11 @@ void NavMeshGenerator2D::bake_from_source_geometry_data_async(Ref<NavigationPoly generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_STARTED; generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator2D::generator_thread_bake, generator_task, NavMeshGenerator2D::baking_use_high_priority_threads, "NavMeshGeneratorBake2D"); generator_tasks.insert(generator_task->thread_task_id, generator_task); - generator_task_mutex.unlock(); } bool NavMeshGenerator2D::is_baking(Ref<NavigationPolygon> p_navigation_polygon) { - baking_navmesh_mutex.lock(); - bool baking = baking_navmeshes.has(p_navigation_polygon); - baking_navmesh_mutex.unlock(); - return baking; + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + return baking_navmeshes.has(p_navigation_polygon); } void NavMeshGenerator2D::generator_thread_bake(void *p_arg) { @@ -263,7 +258,7 @@ void NavMeshGenerator2D::generator_parse_geometry_node(Ref<NavigationPolygon> p_ // Special case for TileMap, so that internal layer get parsed even if p_recurse_children is false. for (int i = 0; i < p_node->get_child_count(); i++) { TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node->get_child(i)); - if (tile_map_layer->get_index_in_tile_map() >= 0) { + if (tile_map_layer && tile_map_layer->get_index_in_tile_map() >= 0) { generator_parse_tile_map_layer_node(p_navigation_mesh, p_source_geometry_data, tile_map_layer); } } @@ -852,8 +847,15 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation } int outline_count = p_navigation_mesh->get_outline_count(); - const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->_get_traversable_outlines(); - const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->_get_obstruction_outlines(); + + Vector<Vector<Vector2>> traversable_outlines; + Vector<Vector<Vector2>> obstruction_outlines; + Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> projected_obstructions; + + p_source_geometry_data->get_data( + traversable_outlines, + obstruction_outlines, + projected_obstructions); if (outline_count == 0 && traversable_outlines.size() == 0) { return; @@ -898,8 +900,6 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation obstruction_polygon_paths.push_back(clip_path); } - const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_get_projected_obstructions(); - if (!projected_obstructions.is_empty()) { for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) { if (projected_obstruction.carve) { @@ -1005,8 +1005,7 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation } if (new_baked_outlines.size() == 0) { - p_navigation_mesh->set_vertices(Vector<Vector2>()); - p_navigation_mesh->clear_polygons(); + p_navigation_mesh->clear(); return; } @@ -1038,11 +1037,32 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation } TPPLPartition tpart; - if (tpart.ConvexPartition_HM(&tppl_in_polygon, &tppl_out_polygon) == 0) { //failed! - ERR_PRINT("NavigationPolygon Convex partition failed. Unable to create a valid NavigationMesh from defined polygon outline paths."); - p_navigation_mesh->set_vertices(Vector<Vector2>()); - p_navigation_mesh->clear_polygons(); - return; + + NavigationPolygon::SamplePartitionType sample_partition_type = p_navigation_mesh->get_sample_partition_type(); + + switch (sample_partition_type) { + case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_CONVEX_PARTITION: + if (tpart.ConvexPartition_HM(&tppl_in_polygon, &tppl_out_polygon) == 0) { + ERR_PRINT("NavigationPolygon polygon convex partition failed. Unable to create a valid navigation mesh polygon layout from provided source geometry."); + p_navigation_mesh->set_vertices(Vector<Vector2>()); + p_navigation_mesh->clear_polygons(); + return; + } + break; + case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_TRIANGULATE: + if (tpart.Triangulate_EC(&tppl_in_polygon, &tppl_out_polygon) == 0) { + ERR_PRINT("NavigationPolygon polygon triangulation failed. Unable to create a valid navigation mesh polygon layout from provided source geometry."); + p_navigation_mesh->set_vertices(Vector<Vector2>()); + p_navigation_mesh->clear_polygons(); + return; + } + break; + default: { + ERR_PRINT("NavigationPolygon polygon partitioning failed. Unrecognized partition type."); + p_navigation_mesh->set_vertices(Vector<Vector2>()); + p_navigation_mesh->clear_polygons(); + return; + } } Vector<Vector2> new_vertices; @@ -1066,11 +1086,7 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation new_polygons.push_back(new_polygon); } - p_navigation_mesh->set_vertices(new_vertices); - p_navigation_mesh->clear_polygons(); - for (int i = 0; i < new_polygons.size(); i++) { - p_navigation_mesh->add_polygon(new_polygons[i]); - } + p_navigation_mesh->set_data(new_vertices, new_polygons); } #endif // CLIPPER2_ENABLED diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 6cbfd93088..5dfc39f6f5 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -486,7 +486,7 @@ COMMAND_2(region_set_navigation_mesh, RID, p_region, Ref<NavigationMesh>, p_navi NavRegion *region = region_owner.get_or_null(p_region); ERR_FAIL_NULL(region); - region->set_mesh(p_navigation_mesh); + region->set_navigation_mesh(p_navigation_mesh); } #ifndef DISABLE_DEPRECATED @@ -509,22 +509,52 @@ void GodotNavigationServer3D::region_bake_navigation_mesh(Ref<NavigationMesh> p_ int GodotNavigationServer3D::region_get_connections_count(RID p_region) const { NavRegion *region = region_owner.get_or_null(p_region); ERR_FAIL_NULL_V(region, 0); - - return region->get_connections_count(); + NavMap *map = region->get_map(); + if (map) { + return map->get_region_connections_count(region); + } + return 0; } Vector3 GodotNavigationServer3D::region_get_connection_pathway_start(RID p_region, int p_connection_id) const { NavRegion *region = region_owner.get_or_null(p_region); ERR_FAIL_NULL_V(region, Vector3()); - - return region->get_connection_pathway_start(p_connection_id); + NavMap *map = region->get_map(); + if (map) { + return map->get_region_connection_pathway_start(region, p_connection_id); + } + return Vector3(); } Vector3 GodotNavigationServer3D::region_get_connection_pathway_end(RID p_region, int p_connection_id) const { NavRegion *region = region_owner.get_or_null(p_region); ERR_FAIL_NULL_V(region, Vector3()); + NavMap *map = region->get_map(); + if (map) { + return map->get_region_connection_pathway_end(region, p_connection_id); + } + return Vector3(); +} - return region->get_connection_pathway_end(p_connection_id); +Vector3 GodotNavigationServer3D::region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const { + const NavRegion *region = region_owner.get_or_null(p_region); + ERR_FAIL_NULL_V(region, Vector3()); + + return region->get_closest_point_to_segment(p_from, p_to, p_use_collision); +} + +Vector3 GodotNavigationServer3D::region_get_closest_point(RID p_region, const Vector3 &p_point) const { + const NavRegion *region = region_owner.get_or_null(p_region); + ERR_FAIL_NULL_V(region, Vector3()); + + return region->get_closest_point_info(p_point).point; +} + +Vector3 GodotNavigationServer3D::region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const { + const NavRegion *region = region_owner.get_or_null(p_region); + ERR_FAIL_NULL_V(region, Vector3()); + + return region->get_closest_point_info(p_point).normal; } Vector3 GodotNavigationServer3D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const { @@ -1298,6 +1328,7 @@ void GodotNavigationServer3D::process(real_t p_delta_time) { int _new_pm_edge_merge_count = 0; int _new_pm_edge_connection_count = 0; int _new_pm_edge_free_count = 0; + int _new_pm_obstacle_count = 0; // In c++ we can't be sure that this is performed in the main thread // even with mutable functions. @@ -1315,6 +1346,7 @@ void GodotNavigationServer3D::process(real_t p_delta_time) { _new_pm_edge_merge_count += active_maps[i]->get_pm_edge_merge_count(); _new_pm_edge_connection_count += active_maps[i]->get_pm_edge_connection_count(); _new_pm_edge_free_count += active_maps[i]->get_pm_edge_free_count(); + _new_pm_obstacle_count += active_maps[i]->get_pm_obstacle_count(); // Emit a signal if a map changed. const uint32_t new_map_iteration_id = active_maps[i]->get_iteration_id(); @@ -1332,6 +1364,7 @@ void GodotNavigationServer3D::process(real_t p_delta_time) { pm_edge_merge_count = _new_pm_edge_merge_count; pm_edge_connection_count = _new_pm_edge_connection_count; pm_edge_free_count = _new_pm_edge_free_count; + pm_obstacle_count = _new_pm_obstacle_count; } void GodotNavigationServer3D::init() { @@ -1566,6 +1599,9 @@ int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const { case INFO_EDGE_FREE_COUNT: { return pm_edge_free_count; } break; + case INFO_OBSTACLE_COUNT: { + return pm_obstacle_count; + } break; } return 0; diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h index 5ba7ed1088..eae6ea2860 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.h +++ b/modules/navigation/3d/godot_navigation_server_3d.h @@ -95,6 +95,7 @@ class GodotNavigationServer3D : public NavigationServer3D { int pm_edge_merge_count = 0; int pm_edge_connection_count = 0; int pm_edge_free_count = 0; + int pm_obstacle_count = 0; public: GodotNavigationServer3D(); @@ -177,6 +178,9 @@ public: virtual int region_get_connections_count(RID p_region) const override; virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override; virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override; + virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision = false) const override; + virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override; + virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override; virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override; virtual RID link_create() override; diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp index cc3bbdbf01..e92a9d304b 100644 --- a/modules/navigation/3d/nav_mesh_generator_3d.cpp +++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp @@ -100,57 +100,55 @@ void NavMeshGenerator3D::sync() { return; } - baking_navmesh_mutex.lock(); - generator_task_mutex.lock(); + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + { + MutexLock generator_task_lock(generator_task_mutex); - LocalVector<WorkerThreadPool::TaskID> finished_task_ids; + LocalVector<WorkerThreadPool::TaskID> finished_task_ids; - for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) { - if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) { - WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); - finished_task_ids.push_back(E.key); + for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) { + if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); + finished_task_ids.push_back(E.key); - NavMeshGeneratorTask3D *generator_task = E.value; - DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED); + NavMeshGeneratorTask3D *generator_task = E.value; + DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED); - baking_navmeshes.erase(generator_task->navigation_mesh); - if (generator_task->callback.is_valid()) { - generator_emit_callback(generator_task->callback); + baking_navmeshes.erase(generator_task->navigation_mesh); + if (generator_task->callback.is_valid()) { + generator_emit_callback(generator_task->callback); + } + memdelete(generator_task); } - memdelete(generator_task); } - } - for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) { - generator_tasks.erase(finished_task_id); + for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) { + generator_tasks.erase(finished_task_id); + } } - - generator_task_mutex.unlock(); - baking_navmesh_mutex.unlock(); } void NavMeshGenerator3D::cleanup() { - baking_navmesh_mutex.lock(); - generator_task_mutex.lock(); + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + { + MutexLock generator_task_lock(generator_task_mutex); - baking_navmeshes.clear(); + baking_navmeshes.clear(); - for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) { - WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); - NavMeshGeneratorTask3D *generator_task = E.value; - memdelete(generator_task); - } - generator_tasks.clear(); + for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key); + NavMeshGeneratorTask3D *generator_task = E.value; + memdelete(generator_task); + } + generator_tasks.clear(); - generator_rid_rwlock.write_lock(); - for (NavMeshGeometryParser3D *parser : generator_parsers) { - generator_parser_owner.free(parser->self); + 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_parsers.clear(); - generator_rid_rwlock.write_unlock(); - - generator_task_mutex.unlock(); - baking_navmesh_mutex.unlock(); } void NavMeshGenerator3D::finish() { @@ -226,7 +224,7 @@ void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh baking_navmeshes.insert(p_navigation_mesh); baking_navmesh_mutex.unlock(); - generator_task_mutex.lock(); + MutexLock generator_task_lock(generator_task_mutex); NavMeshGeneratorTask3D *generator_task = memnew(NavMeshGeneratorTask3D); generator_task->navigation_mesh = p_navigation_mesh; generator_task->source_geometry_data = p_source_geometry_data; @@ -234,14 +232,11 @@ void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED; generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator3D::generator_thread_bake, generator_task, NavMeshGenerator3D::baking_use_high_priority_threads, SNAME("NavMeshGeneratorBake3D")); generator_tasks.insert(generator_task->thread_task_id, generator_task); - generator_task_mutex.unlock(); } bool NavMeshGenerator3D::is_baking(Ref<NavigationMesh> p_navigation_mesh) { - baking_navmesh_mutex.lock(); - bool baking = baking_navmeshes.has(p_navigation_mesh); - baking_navmesh_mutex.unlock(); - return baking; + MutexLock baking_navmesh_lock(baking_navmesh_mutex); + return baking_navmeshes.has(p_navigation_mesh); } void NavMeshGenerator3D::generator_thread_bake(void *p_arg) { @@ -672,10 +667,16 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation return; } - const Vector<float> &vertices = p_source_geometry_data->get_vertices(); - const Vector<int> &indices = p_source_geometry_data->get_indices(); + Vector<float> source_geometry_vertices; + Vector<int> source_geometry_indices; + Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> projected_obstructions; - if (vertices.size() < 3 || indices.size() < 3) { + p_source_geometry_data->get_data( + source_geometry_vertices, + source_geometry_indices, + projected_obstructions); + + if (source_geometry_vertices.size() < 3 || source_geometry_indices.size() < 3) { return; } @@ -691,10 +692,10 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation bake_state = "Setting up Configuration..."; // step #1 - const float *verts = vertices.ptr(); - const int nverts = vertices.size() / 3; - const int *tris = indices.ptr(); - const int ntris = indices.size() / 3; + const float *verts = source_geometry_vertices.ptr(); + const int nverts = source_geometry_vertices.size() / 3; + const int *tris = source_geometry_indices.ptr(); + const int ntris = source_geometry_indices.size() / 3; float bmin[3], bmax[3]; rcCalcBounds(verts, nverts, bmin, bmax); @@ -818,8 +819,6 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation rcFreeHeightField(hf); hf = nullptr; - const Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_get_projected_obstructions(); - // Add obstacles to the source geometry. Those will be affected by e.g. agent_radius. if (!projected_obstructions.is_empty()) { for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) { @@ -894,13 +893,25 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation bake_state = "Converting to native navigation mesh..."; // step #10 Vector<Vector3> nav_vertices; + Vector<Vector<int>> nav_polygons; + + HashMap<Vector3, int> recast_vertex_to_native_index; + LocalVector<int> recast_index_to_native_index; + recast_index_to_native_index.resize(detail_mesh->nverts); for (int i = 0; i < detail_mesh->nverts; i++) { const float *v = &detail_mesh->verts[i * 3]; - nav_vertices.push_back(Vector3(v[0], v[1], v[2])); + const Vector3 vertex = Vector3(v[0], v[1], v[2]); + int *existing_index_ptr = recast_vertex_to_native_index.getptr(vertex); + if (!existing_index_ptr) { + int new_index = recast_vertex_to_native_index.size(); + recast_index_to_native_index[i] = new_index; + recast_vertex_to_native_index[vertex] = new_index; + nav_vertices.push_back(vertex); + } else { + recast_index_to_native_index[i] = *existing_index_ptr; + } } - p_navigation_mesh->set_vertices(nav_vertices); - p_navigation_mesh->clear_polygons(); for (int i = 0; i < detail_mesh->nmeshes; i++) { const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4]; @@ -912,13 +923,20 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation Vector<int> nav_indices; nav_indices.resize(3); // Polygon order in recast is opposite than godot's - nav_indices.write[0] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0])); - nav_indices.write[1] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2])); - nav_indices.write[2] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1])); - p_navigation_mesh->add_polygon(nav_indices); + int index1 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0])); + int index2 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2])); + int index3 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1])); + + nav_indices.write[0] = recast_index_to_native_index[index1]; + nav_indices.write[1] = recast_index_to_native_index[index2]; + nav_indices.write[2] = recast_index_to_native_index[index3]; + + nav_polygons.push_back(nav_indices); } } + p_navigation_mesh->set_data(nav_vertices, nav_polygons); + bake_state = "Cleanup..."; // step #11 rcFreePolyMesh(poly_mesh); diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp new file mode 100644 index 0000000000..70207f86ce --- /dev/null +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -0,0 +1,715 @@ +/**************************************************************************/ +/* nav_mesh_queries_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef _3D_DISABLED + +#include "nav_mesh_queries_3d.h" + +#include "../nav_base.h" + +#include "core/math/geometry_3d.h" + +#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) + +#define APPEND_METADATA(poly) \ + if (r_path_types) { \ + r_path_types->push_back(poly->owner->get_type()); \ + } \ + if (r_path_rids) { \ + r_path_rids->push_back(poly->owner->get_self()); \ + } \ + if (r_path_owners) { \ + r_path_owners->push_back(poly->owner->get_owner_id()); \ + } + +Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) { + const LocalVector<gd::Polygon> ®ion_polygons = p_polygons; + + if (region_polygons.is_empty()) { + return Vector3(); + } + + if (p_uniformly) { + real_t accumulated_area = 0; + RBMap<real_t, uint32_t> region_area_map; + + for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) { + const gd::Polygon ®ion_polygon = region_polygons[rp_index]; + real_t polyon_area = region_polygon.surface_area; + + if (polyon_area == 0.0) { + continue; + } + region_area_map[accumulated_area] = rp_index; + accumulated_area += polyon_area; + } + if (region_area_map.is_empty() || accumulated_area == 0) { + // All polygons have no real surface / no area. + return Vector3(); + } + + real_t region_area_map_pos = Math::random(real_t(0), accumulated_area); + + RBMap<real_t, uint32_t>::Iterator region_E = region_area_map.find_closest(region_area_map_pos); + ERR_FAIL_COND_V(!region_E, Vector3()); + uint32_t rrp_polygon_index = region_E->value; + ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3()); + + const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; + + real_t accumulated_polygon_area = 0; + RBMap<real_t, uint32_t> polygon_area_map; + + for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) { + real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area(); + + if (face_area == 0.0) { + continue; + } + polygon_area_map[accumulated_polygon_area] = rpp_index; + accumulated_polygon_area += face_area; + } + if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) { + // All faces have no real surface / no area. + return Vector3(); + } + + real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area); + + RBMap<real_t, uint32_t>::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos); + ERR_FAIL_COND_V(!polygon_E, Vector3()); + uint32_t rrp_face_index = polygon_E->value; + ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3()); + + const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); + + Vector3 face_random_position = face.get_random_point_inside(); + return face_random_position; + + } else { + uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1); + + const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; + + uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1); + + const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); + + Vector3 face_random_position = face.get_random_point_inside(); + return face_random_position; + } +} + +Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size) { + // Clear metadata outputs. + if (r_path_types) { + r_path_types->clear(); + } + if (r_path_rids) { + r_path_rids->clear(); + } + if (r_path_owners) { + r_path_owners->clear(); + } + + // Find the start poly and the end poly on this map. + const gd::Polygon *begin_poly = nullptr; + const gd::Polygon *end_poly = nullptr; + Vector3 begin_point; + Vector3 end_point; + real_t begin_d = FLT_MAX; + real_t end_d = FLT_MAX; + // Find the initial poly and the end poly on this map. + for (const gd::Polygon &p : p_polygons) { + // Only consider the polygon if it in a region with compatible layers. + if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) { + continue; + } + + // For each face check the distance between the origin/destination + for (size_t point_id = 2; point_id < p.points.size(); point_id++) { + const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + + Vector3 point = face.get_closest_point_to(p_origin); + real_t distance_to_point = point.distance_to(p_origin); + if (distance_to_point < begin_d) { + begin_d = distance_to_point; + begin_poly = &p; + begin_point = point; + } + + point = face.get_closest_point_to(p_destination); + distance_to_point = point.distance_to(p_destination); + if (distance_to_point < end_d) { + end_d = distance_to_point; + end_poly = &p; + end_point = point; + } + } + } + + // Check for trivial cases + if (!begin_poly || !end_poly) { + return Vector<Vector3>(); + } + if (begin_poly == end_poly) { + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = end_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = end_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = end_poly->owner->get_owner_id(); + } + + Vector<Vector3> path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + // List of all reachable navigation polys. + LocalVector<gd::NavigationPoly> navigation_polys; + navigation_polys.resize(p_polygons.size() + p_link_polygons_size); + + // Initialize the matching navigation polygon. + gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; + begin_navigation_poly.poly = begin_poly; + begin_navigation_poly.entry = begin_point; + begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; + begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; + + // Heap of polygons to travel next. + gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer> + traversable_polys; + traversable_polys.reserve(p_polygons.size() * 0.25); + + // This is an implementation of the A* algorithm. + int least_cost_id = begin_poly->id; + int prev_least_cost_id = -1; + bool found_route = false; + + const gd::Polygon *reachable_end = nullptr; + real_t distance_to_reachable_end = FLT_MAX; + bool is_reachable = true; + + while (true) { + // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. + for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) { + // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. + for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { + const gd::Edge::Connection &connection = edge.connections[connection_index]; + + // Only consider the connection to another polygon if this polygon is in a region with compatible layers. + if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { + continue; + } + + const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; + real_t poly_enter_cost = 0.0; + real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); + + if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { + poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); + } + prev_least_cost_id = least_cost_id; + + Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; + const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); + const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance; + + // Check if the neighbor polygon has already been processed. + gd::NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; + if (neighbor_poly.poly != nullptr) { + // If the neighbor polygon hasn't been traversed yet and the new path leading to + // it is shorter, update the polygon. + if (neighbor_poly.traversable_poly_index < traversable_polys.size() && + new_traveled_distance < neighbor_poly.traveled_distance) { + neighbor_poly.back_navigation_poly_id = least_cost_id; + neighbor_poly.back_navigation_edge = connection.edge; + neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start; + neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end; + neighbor_poly.traveled_distance = new_traveled_distance; + neighbor_poly.distance_to_destination = + new_entry.distance_to(end_point) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; + + // Update the priority of the polygon in the heap. + traversable_polys.shift(neighbor_poly.traversable_poly_index); + } + } else { + // Initialize the matching navigation polygon. + neighbor_poly.poly = connection.polygon; + neighbor_poly.back_navigation_poly_id = least_cost_id; + neighbor_poly.back_navigation_edge = connection.edge; + neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start; + neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end; + neighbor_poly.traveled_distance = new_traveled_distance; + neighbor_poly.distance_to_destination = + new_entry.distance_to(end_point) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; + + // Add the polygon to the heap of polygons to traverse next. + traversable_polys.push(&neighbor_poly); + } + } + } + + // When the heap of traversable polygons is empty at this point it means the end polygon is + // unreachable. + if (traversable_polys.is_empty()) { + // Thus use the further reachable polygon + ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); + is_reachable = false; + if (reachable_end == nullptr) { + // The path is not found and there is not a way out. + break; + } + + // Set as end point the furthest reachable point. + end_poly = reachable_end; + end_d = FLT_MAX; + for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { + Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + if (dpoint < end_d) { + end_point = spoint; + end_d = dpoint; + } + } + + // Search all faces of start polygon as well. + bool closest_point_on_start_poly = false; + for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { + Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + if (dpoint < end_d) { + end_point = spoint; + end_d = dpoint; + closest_point_on_start_poly = true; + } + } + + if (closest_point_on_start_poly) { + // No point to run PostProcessing when start and end convex polygon is the same. + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = begin_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = begin_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = begin_poly->owner->get_owner_id(); + } + + Vector<Vector3> path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + for (gd::NavigationPoly &nav_poly : navigation_polys) { + nav_poly.poly = nullptr; + } + navigation_polys[begin_poly->id].poly = begin_poly; + + least_cost_id = begin_poly->id; + prev_least_cost_id = -1; + + reachable_end = nullptr; + + continue; + } + + // Pop the polygon with the lowest travel cost from the heap of traversable polygons. + least_cost_id = traversable_polys.pop()->poly->id; + + // Store the farthest reachable end polygon in case our goal is not reachable. + if (is_reachable) { + real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_destination); + if (distance_to_reachable_end > distance) { + distance_to_reachable_end = distance; + reachable_end = navigation_polys[least_cost_id].poly; + } + } + + // Check if we reached the end + if (navigation_polys[least_cost_id].poly == end_poly) { + found_route = true; + break; + } + } + + // We did not find a route but we have both a start polygon and an end polygon at this point. + // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon. + if (!found_route) { + end_d = FLT_MAX; + // Search all faces of the start polygon for the closest point to our target position. + for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { + Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + if (dpoint < end_d) { + end_point = spoint; + end_d = dpoint; + } + } + + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = begin_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = begin_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = begin_poly->owner->get_owner_id(); + } + + Vector<Vector3> path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + Vector<Vector3> path; + // Optimize the path. + if (p_optimize) { + // Set the apex poly/point to the end point + gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; + + Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; + const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway); + if (end_point.is_equal_approx(back_edge_closest_point)) { + // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. + // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. + if (apex_poly->back_navigation_poly_id != -1) { + apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id]; + } + } + + Vector3 apex_point = end_point; + + gd::NavigationPoly *left_poly = apex_poly; + Vector3 left_portal = apex_point; + gd::NavigationPoly *right_poly = apex_poly; + Vector3 right_portal = apex_point; + + gd::NavigationPoly *p = apex_poly; + + path.push_back(end_point); + APPEND_METADATA(end_poly); + + while (p) { + // Set left and right points of the pathway between polygons. + Vector3 left = p->back_navigation_edge_pathway_start; + Vector3 right = p->back_navigation_edge_pathway_end; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) { + SWAP(left, right); + } + + bool skip = false; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) { + //process + if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) { + left_poly = p; + left_portal = left; + } else { + clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); + + apex_point = right_portal; + p = right_poly; + left_poly = p; + apex_poly = p; + left_portal = apex_point; + right_portal = apex_point; + + path.push_back(apex_point); + APPEND_METADATA(apex_poly->poly); + skip = true; + } + } + + if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) { + //process + if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) { + right_poly = p; + right_portal = right; + } else { + clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); + + apex_point = left_portal; + p = left_poly; + right_poly = p; + apex_poly = p; + right_portal = apex_point; + left_portal = apex_point; + + path.push_back(apex_point); + APPEND_METADATA(apex_poly->poly); + } + } + + // Go to the previous polygon. + if (p->back_navigation_poly_id != -1) { + p = &navigation_polys[p->back_navigation_poly_id]; + } else { + // The end + p = nullptr; + } + } + + // If the last point is not the begin point, add it to the list. + if (path[path.size() - 1] != begin_point) { + path.push_back(begin_point); + APPEND_METADATA(begin_poly); + } + + path.reverse(); + if (r_path_types) { + r_path_types->reverse(); + } + if (r_path_rids) { + r_path_rids->reverse(); + } + if (r_path_owners) { + r_path_owners->reverse(); + } + + } else { + path.push_back(end_point); + APPEND_METADATA(end_poly); + + // Add mid points + int np_id = least_cost_id; + while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { + if (navigation_polys[np_id].back_navigation_edge != -1) { + int prev = navigation_polys[np_id].back_navigation_edge; + int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); + Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; + + path.push_back(point); + APPEND_METADATA(navigation_polys[np_id].poly); + } else { + path.push_back(navigation_polys[np_id].entry); + APPEND_METADATA(navigation_polys[np_id].poly); + } + + np_id = navigation_polys[np_id].back_navigation_poly_id; + } + + path.push_back(begin_point); + APPEND_METADATA(begin_poly); + + path.reverse(); + if (r_path_types) { + r_path_types->reverse(); + } + if (r_path_rids) { + r_path_rids->reverse(); + } + if (r_path_owners) { + r_path_owners->reverse(); + } + } + + // Ensure post conditions (path arrays MUST match in size). + CRASH_COND(r_path_types && path.size() != r_path_types->size()); + CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); + CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); + + return path; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) { + bool use_collision = p_use_collision; + Vector3 closest_point; + real_t closest_point_distance = FLT_MAX; + + for (const gd::Polygon &polygon : p_polygons) { + // For each face check the distance to the segment. + for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) { + const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos); + Vector3 intersection_point; + if (face.intersects_segment(p_from, p_to, &intersection_point)) { + const real_t d = p_from.distance_to(intersection_point); + if (!use_collision) { + closest_point = intersection_point; + use_collision = true; + closest_point_distance = d; + } else if (closest_point_distance > d) { + closest_point = intersection_point; + closest_point_distance = d; + } + } + // If segment does not itersect face, check the distance from segment's endpoints. + else if (!use_collision) { + const Vector3 p_from_closest = face.get_closest_point_to(p_from); + const real_t d_p_from = p_from.distance_to(p_from_closest); + if (closest_point_distance > d_p_from) { + closest_point = p_from_closest; + closest_point_distance = d_p_from; + } + + const Vector3 p_to_closest = face.get_closest_point_to(p_to); + const real_t d_p_to = p_to.distance_to(p_to_closest); + if (closest_point_distance > d_p_to) { + closest_point = p_to_closest; + closest_point_distance = d_p_to; + } + } + } + // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment. + if (!use_collision) { + for (size_t point_id = 0; point_id < polygon.points.size(); point_id += 1) { + Vector3 a, b; + + Geometry3D::get_closest_points_between_segments( + p_from, + p_to, + polygon.points[point_id].pos, + polygon.points[(point_id + 1) % polygon.points.size()].pos, + a, + b); + + const real_t d = a.distance_to(b); + if (d < closest_point_distance) { + closest_point_distance = d; + closest_point = b; + } + } + } + } + + return closest_point; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.point; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.normal; +} + +gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult result; + real_t closest_point_distance_squared = FLT_MAX; + + for (const gd::Polygon &polygon : p_polygons) { + for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) { + const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos); + const Vector3 closest_point_on_face = face.get_closest_point_to(p_point); + const real_t distance_squared_to_point = closest_point_on_face.distance_squared_to(p_point); + if (distance_squared_to_point < closest_point_distance_squared) { + result.point = closest_point_on_face; + result.normal = face.get_plane().normal; + result.owner = polygon.owner->get_self(); + closest_point_distance_squared = distance_squared_to_point; + } + } + } + + return result; +} + +RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.owner; +} + +void NavMeshQueries3D::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up) { + Vector3 from = path[path.size() - 1]; + + if (from.is_equal_approx(p_to_point)) { + return; + } + + Plane cut_plane; + cut_plane.normal = (from - p_to_point).cross(p_map_up); + if (cut_plane.normal == Vector3()) { + return; + } + cut_plane.normal.normalize(); + cut_plane.d = cut_plane.normal.dot(from); + + while (from_poly != p_to_poly) { + Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; + Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end; + + ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1); + from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id]; + + if (!pathway_start.is_equal_approx(pathway_end)) { + Vector3 inters; + if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { + if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { + path.push_back(inters); + APPEND_METADATA(from_poly->poly); + } + } + } + } +} + +#endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h new file mode 100644 index 0000000000..109bb2f971 --- /dev/null +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -0,0 +1,54 @@ +/**************************************************************************/ +/* nav_mesh_queries_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef NAV_MESH_QUERIES_3D_H +#define NAV_MESH_QUERIES_3D_H + +#ifndef _3D_DISABLED + +#include "../nav_map.h" + +class NavMeshQueries3D { +public: + static Vector3 polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly); + + static Vector<Vector3> polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size); + static Vector3 polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision); + static Vector3 polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point); + static Vector3 polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point); + static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point); + static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point); + + static void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up); +}; + +#endif // _3D_DISABLED + +#endif // NAV_MESH_QUERIES_3D_H diff --git a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp index d07d3cdff5..7f0cbc7b5e 100644 --- a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp +++ b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp @@ -126,9 +126,6 @@ void NavigationMeshEditor::edit(NavigationRegion3D *p_nav_region) { node = p_nav_region; } -void NavigationMeshEditor::_bind_methods() { -} - NavigationMeshEditor::NavigationMeshEditor() { bake_hbox = memnew(HBoxContainer); @@ -179,7 +176,7 @@ void NavigationMeshEditorPlugin::make_visible(bool p_visible) { NavigationMeshEditorPlugin::NavigationMeshEditorPlugin() { navigation_mesh_editor = memnew(NavigationMeshEditor); - EditorNode::get_singleton()->get_main_screen_control()->add_child(navigation_mesh_editor); + EditorNode::get_singleton()->get_gui_base()->add_child(navigation_mesh_editor); add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, navigation_mesh_editor->bake_hbox); navigation_mesh_editor->hide(); navigation_mesh_editor->bake_hbox->hide(); diff --git a/modules/navigation/editor/navigation_mesh_editor_plugin.h b/modules/navigation/editor/navigation_mesh_editor_plugin.h index 6114c62ebf..f5a471d531 100644 --- a/modules/navigation/editor/navigation_mesh_editor_plugin.h +++ b/modules/navigation/editor/navigation_mesh_editor_plugin.h @@ -60,7 +60,6 @@ class NavigationMeshEditor : public Control { protected: void _node_removed(Node *p_node); - static void _bind_methods(); void _notification(int p_what); public: diff --git a/modules/navigation/nav_base.h b/modules/navigation/nav_base.h index c28392acf7..d2308abfaf 100644 --- a/modules/navigation/nav_base.h +++ b/modules/navigation/nav_base.h @@ -64,7 +64,7 @@ public: void set_owner_id(ObjectID p_owner_id) { owner_id = p_owner_id; } ObjectID get_owner_id() const { return owner_id; } - virtual ~NavBase(){}; + virtual ~NavBase() {} }; #endif // NAV_BASE_H diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index dfbc92a919..dd77e81b45 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -35,25 +35,13 @@ #include "nav_obstacle.h" #include "nav_region.h" +#include "3d/nav_mesh_queries_3d.h" + #include "core/config/project_settings.h" #include "core/object/worker_thread_pool.h" #include <Obstacle2d.h> -#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) - -// Helper macro -#define APPEND_METADATA(poly) \ - if (r_path_types) { \ - r_path_types->push_back(poly->owner->get_type()); \ - } \ - if (r_path_rids) { \ - r_path_rids->push_back(poly->owner->get_self()); \ - } \ - if (r_path_owners) { \ - r_path_owners->push_back(poly->owner->get_owner_id()); \ - } - #ifdef DEBUG_ENABLED #define NAVMAP_ITERATION_ZERO_ERROR_MSG() \ ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\ @@ -142,462 +130,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return Vector<Vector3>(); } - // Clear metadata outputs. - if (r_path_types) { - r_path_types->clear(); - } - if (r_path_rids) { - r_path_rids->clear(); - } - if (r_path_owners) { - r_path_owners->clear(); - } - - // Find the start poly and the end poly on this map. - const gd::Polygon *begin_poly = nullptr; - const gd::Polygon *end_poly = nullptr; - Vector3 begin_point; - Vector3 end_point; - real_t begin_d = FLT_MAX; - real_t end_d = FLT_MAX; - // Find the initial poly and the end poly on this map. - for (const gd::Polygon &p : polygons) { - // Only consider the polygon if it in a region with compatible layers. - if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) { - continue; - } - - // For each face check the distance between the origin/destination - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - - Vector3 point = face.get_closest_point_to(p_origin); - real_t distance_to_point = point.distance_to(p_origin); - if (distance_to_point < begin_d) { - begin_d = distance_to_point; - begin_poly = &p; - begin_point = point; - } - - point = face.get_closest_point_to(p_destination); - distance_to_point = point.distance_to(p_destination); - if (distance_to_point < end_d) { - end_d = distance_to_point; - end_poly = &p; - end_point = point; - } - } - } - - // Check for trivial cases - if (!begin_poly || !end_poly) { - return Vector<Vector3>(); - } - if (begin_poly == end_poly) { - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = end_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = end_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = end_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // List of all reachable navigation polys. - LocalVector<gd::NavigationPoly> navigation_polys; - navigation_polys.reserve(polygons.size() * 0.75); - - // Add the start polygon to the reachable navigation polygons. - gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly); - begin_navigation_poly.self_id = 0; - begin_navigation_poly.entry = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; - navigation_polys.push_back(begin_navigation_poly); - - // List of polygon IDs to visit. - List<uint32_t> to_visit; - to_visit.push_back(0); - - // This is an implementation of the A* algorithm. - int least_cost_id = 0; - int prev_least_cost_id = -1; - bool found_route = false; - - const gd::Polygon *reachable_end = nullptr; - real_t reachable_d = FLT_MAX; - bool is_reachable = true; - - while (true) { - // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. - for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) { - // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. - for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { - const gd::Edge::Connection &connection = edge.connections[connection_index]; - - // Only consider the connection to another polygon if this polygon is in a region with compatible layers. - if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { - continue; - } - - const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; - real_t poly_enter_cost = 0.0; - real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); - - if (prev_least_cost_id != -1 && (navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self())) { - poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); - } - prev_least_cost_id = least_cost_id; - - Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; - const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); - const real_t new_distance = (least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly.traveled_distance; - - int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon)); - - if (already_visited_polygon_index != -1) { - // Polygon already visited, check if we can reduce the travel cost. - gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index]; - if (new_distance < avp.traveled_distance) { - avp.back_navigation_poly_id = least_cost_id; - avp.back_navigation_edge = connection.edge; - avp.back_navigation_edge_pathway_start = connection.pathway_start; - avp.back_navigation_edge_pathway_end = connection.pathway_end; - avp.traveled_distance = new_distance; - avp.entry = new_entry; - } - } else { - // Add the neighbor polygon to the reachable ones. - gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon); - new_navigation_poly.self_id = navigation_polys.size(); - new_navigation_poly.back_navigation_poly_id = least_cost_id; - new_navigation_poly.back_navigation_edge = connection.edge; - new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start; - new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end; - new_navigation_poly.traveled_distance = new_distance; - new_navigation_poly.entry = new_entry; - navigation_polys.push_back(new_navigation_poly); - - // Add the neighbor polygon to the polygons to visit. - to_visit.push_back(navigation_polys.size() - 1); - } - } - } - - // Removes the least cost polygon from the list of polygons to visit so we can advance. - to_visit.erase(least_cost_id); - - // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable - if (to_visit.size() == 0) { - // Thus use the further reachable polygon - ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); - is_reachable = false; - if (reachable_end == nullptr) { - // The path is not found and there is not a way out. - break; - } - - // Set as end point the furthest reachable point. - end_poly = reachable_end; - end_d = FLT_MAX; - for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { - Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - } - } - - // Search all faces of start polygon as well. - bool closest_point_on_start_poly = false; - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - closest_point_on_start_poly = true; - } - } - - if (closest_point_on_start_poly) { - // No point to run PostProcessing when start and end convex polygon is the same. - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // Reset open and navigation_polys - gd::NavigationPoly np = navigation_polys[0]; - navigation_polys.clear(); - navigation_polys.push_back(np); - to_visit.clear(); - to_visit.push_back(0); - least_cost_id = 0; - prev_least_cost_id = -1; - - reachable_end = nullptr; - - continue; - } - - // Find the polygon with the minimum cost from the list of polygons to visit. - least_cost_id = -1; - real_t least_cost = FLT_MAX; - for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) { - gd::NavigationPoly *np = &navigation_polys[element->get()]; - real_t cost = np->traveled_distance; - cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost()); - if (cost < least_cost) { - least_cost_id = np->self_id; - least_cost = cost; - } - } - - ERR_BREAK(least_cost_id == -1); - - // Stores the further reachable end polygon, in case our goal is not reachable. - if (is_reachable) { - real_t d = navigation_polys[least_cost_id].entry.distance_to(p_destination); - if (reachable_d > d) { - reachable_d = d; - reachable_end = navigation_polys[least_cost_id].poly; - } - } - - // Check if we reached the end - if (navigation_polys[least_cost_id].poly == end_poly) { - found_route = true; - break; - } - } - - // We did not find a route but we have both a start polygon and an end polygon at this point. - // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon. - if (!found_route) { - end_d = FLT_MAX; - // Search all faces of the start polygon for the closest point to our target position. - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - } - } - - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - Vector<Vector3> path; - // Optimize the path. - if (p_optimize) { - // Set the apex poly/point to the end point - gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; - - Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; - const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway); - if (end_point.is_equal_approx(back_edge_closest_point)) { - // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. - // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. - if (apex_poly->back_navigation_poly_id != -1) { - apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id]; - } - } - - Vector3 apex_point = end_point; - - gd::NavigationPoly *left_poly = apex_poly; - Vector3 left_portal = apex_point; - gd::NavigationPoly *right_poly = apex_poly; - Vector3 right_portal = apex_point; - - gd::NavigationPoly *p = apex_poly; - - path.push_back(end_point); - APPEND_METADATA(end_poly); - - while (p) { - // Set left and right points of the pathway between polygons. - Vector3 left = p->back_navigation_edge_pathway_start; - Vector3 right = p->back_navigation_edge_pathway_end; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) { - SWAP(left, right); - } - - bool skip = false; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) { - //process - if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) { - left_poly = p; - left_portal = left; - } else { - clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = right_portal; - p = right_poly; - left_poly = p; - apex_poly = p; - left_portal = apex_point; - right_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - skip = true; - } - } - - if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) { - //process - if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) { - right_poly = p; - right_portal = right; - } else { - clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = left_portal; - p = left_poly; - right_poly = p; - apex_poly = p; - right_portal = apex_point; - left_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - } - } - - // Go to the previous polygon. - if (p->back_navigation_poly_id != -1) { - p = &navigation_polys[p->back_navigation_poly_id]; - } else { - // The end - p = nullptr; - } - } - - // If the last point is not the begin point, add it to the list. - if (path[path.size() - 1] != begin_point) { - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - } - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - - } else { - path.push_back(end_point); - APPEND_METADATA(end_poly); - - // Add mid points - int np_id = least_cost_id; - while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { - if (navigation_polys[np_id].back_navigation_edge != -1) { - int prev = navigation_polys[np_id].back_navigation_edge; - int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); - Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; - - path.push_back(point); - APPEND_METADATA(navigation_polys[np_id].poly); - } else { - path.push_back(navigation_polys[np_id].entry); - APPEND_METADATA(navigation_polys[np_id].poly); - } - - np_id = navigation_polys[np_id].back_navigation_poly_id; - } - - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - } - - // Ensure post conditions (path arrays MUST match in size). - CRASH_COND(r_path_types && path.size() != r_path_types->size()); - CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); - CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); - - return path; + return NavMeshQueries3D::polygons_get_path( + polygons, p_origin, p_destination, p_optimize, p_navigation_layers, + r_path_types, r_path_rids, r_path_owners, up, link_polygons.size()); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -607,50 +142,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector return Vector3(); } - bool use_collision = p_use_collision; - Vector3 closest_point; - real_t closest_point_d = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the segment - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - Vector3 inters; - if (f.intersects_segment(p_from, p_to, &inters)) { - const real_t d = closest_point_d = p_from.distance_to(inters); - if (use_collision == false) { - closest_point = inters; - use_collision = true; - closest_point_d = d; - } else if (closest_point_d > d) { - closest_point = inters; - closest_point_d = d; - } - } - } - - if (use_collision == false) { - for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { - Vector3 a, b; - - Geometry3D::get_closest_points_between_segments( - p_from, - p_to, - p.points[point_id].pos, - p.points[(point_id + 1) % p.points.size()].pos, - a, - b); - - const real_t d = a.distance_to(b); - if (d < closest_point_d) { - closest_point_d = d; - closest_point = b; - } - } - } - } - - return closest_point; + return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision); } Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { @@ -659,8 +151,8 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.point; + + return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point); } Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { @@ -669,8 +161,8 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.normal; + + return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point); } RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { @@ -679,32 +171,14 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return RID(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.owner; + + return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point); } gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const { RWLockRead read_lock(map_rwlock); - gd::ClosestPointQueryResult result; - real_t closest_point_ds = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the point - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - const Vector3 inters = f.get_closest_point_to(p_point); - const real_t ds = inters.distance_squared_to(p_point); - if (ds < closest_point_ds) { - result.point = inters; - result.normal = f.get_plane().normal; - result.owner = p.owner->get_self(); - closest_point_ds = ds; - } - } - } - - return result; + return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point); } void NavMap::add_region(NavRegion *p_region) { @@ -891,6 +365,7 @@ void NavMap::sync() { int _new_pm_edge_merge_count = pm_edge_merge_count; int _new_pm_edge_connection_count = pm_edge_connection_count; int _new_pm_edge_free_count = pm_edge_free_count; + int _new_pm_obstacle_count = obstacles.size(); // Check if we need to update the links. if (regenerate_polygons) { @@ -920,34 +395,36 @@ void NavMap::sync() { _new_pm_edge_free_count = 0; // Remove regions connections. + region_external_connections.clear(); for (NavRegion *region : regions) { - region->get_connections().clear(); + region_external_connections[region] = LocalVector<gd::Edge::Connection>(); } // Resize the polygon count. - int count = 0; + int polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } - count += region->get_polygons().size(); + polygon_count += region->get_polygons().size(); } - polygons.resize(count); + polygons.resize(polygon_count); // Copy all region polygons in the map. - count = 0; + polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } const LocalVector<gd::Polygon> &polygons_source = region->get_polygons(); for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons[count + n] = polygons_source[n]; + polygons[polygon_count] = polygons_source[n]; + polygons[polygon_count].id = polygon_count; + polygon_count++; } - count += region->get_polygons().size(); } - _new_pm_polygon_count = polygons.size(); + _new_pm_polygon_count = polygon_count; // Group all edges per key. HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections; @@ -1055,7 +532,7 @@ void NavMap::sync() { free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection); // Add the connection to the region_connection map. - ((NavRegion *)free_edge.polygon->owner)->get_connections().push_back(new_connection); + region_external_connections[(NavRegion *)free_edge.polygon->owner].push_back(new_connection); _new_pm_edge_connection_count += 1; } } @@ -1118,6 +595,7 @@ void NavMap::sync() { // If we have both a start and end point, then create a synthetic polygon to route through. if (closest_start_polygon && closest_end_polygon) { gd::Polygon &new_polygon = link_polygons[link_poly_idx++]; + new_polygon.id = polygon_count++; new_polygon.owner = link; new_polygon.edges.clear(); @@ -1131,13 +609,6 @@ void NavMap::sync() { new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) }); new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) }); - Vector3 center; - for (int p = 0; p < 4; ++p) { - center += new_polygon.points[p].pos; - } - new_polygon.center = center / real_t(new_polygon.points.size()); - new_polygon.clockwise = true; - // Setup connections to go forward in the link. { gd::Edge::Connection entry_connection; @@ -1210,6 +681,7 @@ void NavMap::sync() { pm_edge_merge_count = _new_pm_edge_merge_count; pm_edge_connection_count = _new_pm_edge_connection_count; pm_edge_free_count = _new_pm_edge_free_count; + pm_obstacle_count = _new_pm_obstacle_count; } void NavMap::_update_rvo_obstacles_tree_2d() { @@ -1379,42 +851,43 @@ void NavMap::dispatch_callbacks() { } } -void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const { - Vector3 from = path[path.size() - 1]; +void NavMap::_update_merge_rasterizer_cell_dimensions() { + merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale; + merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale; +} + +int NavMap::get_region_connections_count(NavRegion *p_region) const { + ERR_FAIL_NULL_V(p_region, 0); - if (from.is_equal_approx(p_to_point)) { - return; + HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region); + if (found_connections) { + return found_connections->value.size(); } - Plane cut_plane; - cut_plane.normal = (from - p_to_point).cross(up); - if (cut_plane.normal == Vector3()) { - return; - } - cut_plane.normal.normalize(); - cut_plane.d = cut_plane.normal.dot(from); - - while (from_poly != p_to_poly) { - Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; - Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end; - - ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1); - from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id]; - - if (!pathway_start.is_equal_approx(pathway_end)) { - Vector3 inters; - if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { - if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { - path.push_back(inters); - APPEND_METADATA(from_poly->poly); - } - } - } + return 0; +} + +Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const { + ERR_FAIL_NULL_V(p_region, Vector3()); + + HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region); + if (found_connections) { + ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); + return found_connections->value[p_connection_id].pathway_start; } + + return Vector3(); } -void NavMap::_update_merge_rasterizer_cell_dimensions() { - merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale; - merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale; +Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const { + ERR_FAIL_NULL_V(p_region, Vector3()); + + HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region); + if (found_connections) { + ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); + return found_connections->value[p_connection_id].pathway_end; + } + + return Vector3(); } NavMap::NavMap() { diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index d6215ea57f..b9120c04d9 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -36,6 +36,7 @@ #include "core/math/math_defs.h" #include "core/object/worker_thread_pool.h" +#include "servers/navigation/navigation_globals.h" #include <KdTree2d.h> #include <KdTree3d.h> @@ -55,21 +56,21 @@ class NavMap : public NavRid { /// To find the polygons edges the vertices are displaced in a grid where /// each cell has the following cell_size and cell_height. - real_t cell_size = 0.25; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size. - real_t cell_height = 0.25; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height. + real_t cell_size = NavigationDefaults3D::navmesh_cell_size; + real_t cell_height = NavigationDefaults3D::navmesh_cell_height; // For the inter-region merging to work, internal rasterization is performed. - float merge_rasterizer_cell_size = 0.25; - float merge_rasterizer_cell_height = 0.25; + float merge_rasterizer_cell_size = NavigationDefaults3D::navmesh_cell_size; + float merge_rasterizer_cell_height = NavigationDefaults3D::navmesh_cell_height; // This value is used to control sensitivity of internal rasterizer. float merge_rasterizer_cell_scale = 1.0; bool use_edge_connections = true; /// This value is used to detect the near edges to connect. - real_t edge_connection_margin = 0.25; + real_t edge_connection_margin = NavigationDefaults3D::edge_connection_margin; /// This value is used to limit how far links search to find polygons to connect to. - real_t link_connection_radius = 1.0; + real_t link_connection_radius = NavigationDefaults3D::link_connection_radius; bool regenerate_polygons = true; bool regenerate_links = true; @@ -123,6 +124,9 @@ class NavMap : public NavRid { int pm_edge_merge_count = 0; int pm_edge_connection_count = 0; int pm_edge_free_count = 0; + int pm_obstacle_count = 0; + + HashMap<NavRegion *, LocalVector<gd::Edge::Connection>> region_external_connections; public: NavMap(); @@ -216,6 +220,11 @@ public: int get_pm_edge_merge_count() const { return pm_edge_merge_count; } int get_pm_edge_connection_count() const { return pm_edge_connection_count; } int get_pm_edge_free_count() const { return pm_edge_free_count; } + int get_pm_obstacle_count() const { return pm_obstacle_count; } + + int get_region_connections_count(NavRegion *p_region) const; + Vector3 get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const; + Vector3 get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const; private: void compute_single_step(uint32_t index, NavAgent **agent); @@ -223,7 +232,6 @@ private: void compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent); void compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent); - void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const; void _update_rvo_simulation(); void _update_rvo_obstacles_tree_2d(); void _update_rvo_agents_tree_2d(); diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp index 9cb235d79f..2c91b80af2 100644 --- a/modules/navigation/nav_region.cpp +++ b/modules/navigation/nav_region.cpp @@ -32,6 +32,8 @@ #include "nav_map.h" +#include "3d/nav_mesh_queries_3d.h" + void NavRegion::set_map(NavMap *p_map) { if (map == p_map) { return; @@ -44,8 +46,6 @@ void NavRegion::set_map(NavMap *p_map) { map = p_map; polygons_dirty = true; - connections.clear(); - if (map) { map->add_region(this); } @@ -74,115 +74,63 @@ void NavRegion::set_transform(Transform3D p_transform) { } transform = p_transform; polygons_dirty = true; -} - -void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) { - mesh = p_mesh; - polygons_dirty = true; -} -int NavRegion::get_connections_count() const { - if (!map) { - return 0; +#ifdef DEBUG_ENABLED + if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) { + ERR_PRINT_ONCE("Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation."); } - return connections.size(); -} - -Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const { - ERR_FAIL_NULL_V(map, Vector3()); - ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3()); - return connections[p_connection_id].pathway_start; -} - -Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const { - ERR_FAIL_NULL_V(map, Vector3()); - ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3()); - return connections[p_connection_id].pathway_end; +#endif // DEBUG_ENABLED } -Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const { - if (!get_enabled()) { - return Vector3(); +void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) { +#ifdef DEBUG_ENABLED + if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) { + ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size()))); } - const LocalVector<gd::Polygon> ®ion_polygons = get_polygons(); - - if (region_polygons.is_empty()) { - return Vector3(); + if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_height()), double(p_navigation_mesh->get_cell_height()))) { + ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_height()), double(map->get_cell_height()))); } +#endif // DEBUG_ENABLED - if (p_uniformly) { - real_t accumulated_area = 0; - RBMap<real_t, uint32_t> region_area_map; - - for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) { - const gd::Polygon ®ion_polygon = region_polygons[rp_index]; - real_t polyon_area = region_polygon.surface_area; - - if (polyon_area == 0.0) { - continue; - } - region_area_map[accumulated_area] = rp_index; - accumulated_area += polyon_area; - } - if (region_area_map.is_empty() || accumulated_area == 0) { - // All polygons have no real surface / no area. - return Vector3(); - } - - real_t region_area_map_pos = Math::random(real_t(0), accumulated_area); - - RBMap<real_t, uint32_t>::Iterator region_E = region_area_map.find_closest(region_area_map_pos); - ERR_FAIL_COND_V(!region_E, Vector3()); - uint32_t rrp_polygon_index = region_E->value; - ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3()); - - const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; - - real_t accumulated_polygon_area = 0; - RBMap<real_t, uint32_t> polygon_area_map; + RWLockWrite write_lock(navmesh_rwlock); - for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) { - real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area(); + pending_navmesh_vertices.clear(); + pending_navmesh_polygons.clear(); - if (face_area == 0.0) { - continue; - } - polygon_area_map[accumulated_polygon_area] = rpp_index; - accumulated_polygon_area += face_area; - } - if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) { - // All faces have no real surface / no area. - return Vector3(); - } - - real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area); - - RBMap<real_t, uint32_t>::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos); - ERR_FAIL_COND_V(!polygon_E, Vector3()); - uint32_t rrp_face_index = polygon_E->value; - ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3()); + if (p_navigation_mesh.is_valid()) { + p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons); + } - const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); + polygons_dirty = true; +} - Vector3 face_random_position = face.get_random_point_inside(); - return face_random_position; +Vector3 NavRegion::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const { + RWLockRead read_lock(region_rwlock); - } else { - uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1); + return NavMeshQueries3D::polygons_get_closest_point_to_segment( + get_polygons(), p_from, p_to, p_use_collision); +} - const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; +gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const { + RWLockRead read_lock(region_rwlock); - uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1); + return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point); +} - const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); +Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const { + RWLockRead read_lock(region_rwlock); - Vector3 face_random_position = face.get_random_point_inside(); - return face_random_position; + if (!get_enabled()) { + return Vector3(); } + + return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly); } bool NavRegion::sync() { + RWLockWrite write_lock(region_rwlock); + bool something_changed = polygons_dirty /* || something_dirty? */; update_polygons(); @@ -202,33 +150,20 @@ void NavRegion::update_polygons() { return; } - if (mesh.is_null()) { - return; - } - -#ifdef DEBUG_ENABLED - if (!Math::is_equal_approx(double(map->get_cell_size()), double(mesh->get_cell_size()))) { - ERR_PRINT_ONCE(vformat("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(mesh->get_cell_size()), double(map->get_cell_size()))); - } - - if (!Math::is_equal_approx(double(map->get_cell_height()), double(mesh->get_cell_height()))) { - ERR_PRINT_ONCE(vformat("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(mesh->get_cell_height()), double(map->get_cell_height()))); - } + RWLockRead read_lock(navmesh_rwlock); - if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) { - ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation."); + if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) { + return; } -#endif // DEBUG_ENABLED - Vector<Vector3> vertices = mesh->get_vertices(); - int len = vertices.size(); + int len = pending_navmesh_vertices.size(); if (len == 0) { return; } - const Vector3 *vertices_r = vertices.ptr(); + const Vector3 *vertices_r = pending_navmesh_vertices.ptr(); - polygons.resize(mesh->get_polygon_count()); + polygons.resize(pending_navmesh_polygons.size()); real_t _new_region_surface_area = 0.0; @@ -238,7 +173,7 @@ void NavRegion::update_polygons() { polygon.owner = this; polygon.surface_area = 0.0; - Vector<int> navigation_mesh_polygon = mesh->get_polygon(navigation_mesh_polygon_index); + Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index]; navigation_mesh_polygon_index += 1; int navigation_mesh_polygon_size = navigation_mesh_polygon.size(); @@ -266,9 +201,6 @@ void NavRegion::update_polygons() { polygon.surface_area = _new_polygon_surface_area; _new_region_surface_area += _new_polygon_surface_area; - Vector3 polygon_center; - real_t sum(0); - for (int j(0); j < navigation_mesh_polygon_size; j++) { int idx = indices[j]; if (idx < 0 || idx >= len) { @@ -279,25 +211,11 @@ void NavRegion::update_polygons() { Vector3 point_position = transform.xform(vertices_r[idx]); polygon.points[j].pos = point_position; polygon.points[j].key = map->get_point_key(point_position); - - polygon_center += point_position; // Composing the center of the polygon - - if (j >= 2) { - Vector3 epa = transform.xform(vertices_r[indices[j - 2]]); - Vector3 epb = transform.xform(vertices_r[indices[j - 1]]); - - sum += map->get_up().dot((epb - epa).cross(point_position - epa)); - } } if (!valid) { ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!"); } - - polygon.clockwise = sum > 0; - if (!navigation_mesh_polygon.is_empty()) { - polygon.center = polygon_center / real_t(navigation_mesh_polygon.size()); - } } surface_area = _new_region_surface_area; diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h index a9cfc53c7e..c015802b92 100644 --- a/modules/navigation/nav_region.h +++ b/modules/navigation/nav_region.h @@ -34,13 +34,14 @@ #include "nav_base.h" #include "nav_utils.h" +#include "core/os/rw_lock.h" #include "scene/resources/navigation_mesh.h" class NavRegion : public NavBase { + RWLock region_rwlock; + NavMap *map = nullptr; Transform3D transform; - Ref<NavigationMesh> mesh; - Vector<gd::Edge::Connection> connections; bool enabled = true; bool use_edge_connections = true; @@ -52,6 +53,10 @@ class NavRegion : public NavBase { real_t surface_area = 0.0; + RWLock navmesh_rwlock; + Vector<Vector3> pending_navmesh_vertices; + Vector<Vector<int>> pending_navmesh_polygons; + public: NavRegion() { type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION; @@ -79,22 +84,14 @@ public: return transform; } - void set_mesh(Ref<NavigationMesh> p_mesh); - const Ref<NavigationMesh> get_mesh() const { - return mesh; - } - - Vector<gd::Edge::Connection> &get_connections() { - return connections; - } - int get_connections_count() const; - Vector3 get_connection_pathway_start(int p_connection_id) const; - Vector3 get_connection_pathway_end(int p_connection_id) const; + void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh); LocalVector<gd::Polygon> const &get_polygons() const { return polygons; } + Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const; + gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const; Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const; real_t get_surface_area() const { return surface_area; }; diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h index 175d08ca6d..ba4c44b748 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -98,28 +98,27 @@ struct Edge { }; struct Polygon { + /// Id of the polygon in the map. + uint32_t id = UINT32_MAX; + /// Navigation region or link that contains this polygon. const NavBase *owner = nullptr; /// The points of this `Polygon` LocalVector<Point> points; - /// Are the points clockwise? - bool clockwise; - /// The edges of this `Polygon` LocalVector<Edge> edges; - /// The center of this `Polygon` - Vector3 center; - real_t surface_area = 0.0; }; struct NavigationPoly { - uint32_t self_id = 0; /// This poly. - const Polygon *poly; + const Polygon *poly = nullptr; + + /// Index in the heap of traversable polygons. + uint32_t traversable_poly_index = UINT32_MAX; /// Those 4 variables are used to travel the path backwards. int back_navigation_poly_id = -1; @@ -129,20 +128,44 @@ struct NavigationPoly { /// The entry position of this poly. Vector3 entry; - /// The distance to the destination. + /// The distance traveled until now (g cost). real_t traveled_distance = 0.0; + /// The distance to the destination (h cost). + real_t distance_to_destination = 0.0; - NavigationPoly() { poly = nullptr; } + /// The total travel cost (f cost). + real_t total_travel_cost() const { + return traveled_distance + distance_to_destination; + } - NavigationPoly(const Polygon *p_poly) : - poly(p_poly) {} + bool operator==(const NavigationPoly &p_other) const { + return poly == p_other.poly; + } - bool operator==(const NavigationPoly &other) const { - return poly == other.poly; + bool operator!=(const NavigationPoly &p_other) const { + return !(*this == p_other); } +}; + +struct NavPolyTravelCostGreaterThan { + // Returns `true` if the travel cost of `a` is higher than that of `b`. + bool operator()(const NavigationPoly *p_poly_a, const NavigationPoly *p_poly_b) const { + real_t f_cost_a = p_poly_a->total_travel_cost(); + real_t h_cost_a = p_poly_a->distance_to_destination; + real_t f_cost_b = p_poly_b->total_travel_cost(); + real_t h_cost_b = p_poly_b->distance_to_destination; + + if (f_cost_a != f_cost_b) { + return f_cost_a > f_cost_b; + } else { + return h_cost_a > h_cost_b; + } + } +}; - bool operator!=(const NavigationPoly &other) const { - return !operator==(other); +struct NavPolyHeapIndexer { + void operator()(NavigationPoly *p_poly, uint32_t p_heap_index) const { + p_poly->traversable_poly_index = p_heap_index; } }; @@ -152,6 +175,129 @@ struct ClosestPointQueryResult { RID owner; }; +template <typename T> +struct NoopIndexer { + void operator()(const T &p_value, uint32_t p_index) {} +}; + +/** + * A max-heap implementation that notifies of element index changes. + */ +template <typename T, typename LessThan = Comparator<T>, typename Indexer = NoopIndexer<T>> +class Heap { + LocalVector<T> _buffer; + + LessThan _less_than; + Indexer _indexer; + +public: + void reserve(uint32_t p_size) { + _buffer.reserve(p_size); + } + + uint32_t size() const { + return _buffer.size(); + } + + bool is_empty() const { + return _buffer.is_empty(); + } + + void push(const T &p_element) { + _buffer.push_back(p_element); + _indexer(p_element, _buffer.size() - 1); + _shift_up(_buffer.size() - 1); + } + + T pop() { + ERR_FAIL_COND_V_MSG(_buffer.is_empty(), T(), "Can't pop an empty heap."); + T value = _buffer[0]; + _indexer(value, UINT32_MAX); + if (_buffer.size() > 1) { + _buffer[0] = _buffer[_buffer.size() - 1]; + _indexer(_buffer[0], 0); + _buffer.remove_at(_buffer.size() - 1); + _shift_down(0); + } else { + _buffer.remove_at(_buffer.size() - 1); + } + return value; + } + + /** + * Update the position of the element in the heap if necessary. + */ + void shift(uint32_t p_index) { + ERR_FAIL_UNSIGNED_INDEX_MSG(p_index, _buffer.size(), "Heap element index is out of range."); + if (!_shift_up(p_index)) { + _shift_down(p_index); + } + } + + void clear() { + for (const T &value : _buffer) { + _indexer(value, UINT32_MAX); + } + _buffer.clear(); + } + + Heap() {} + + Heap(const LessThan &p_less_than) : + _less_than(p_less_than) {} + + Heap(const Indexer &p_indexer) : + _indexer(p_indexer) {} + + Heap(const LessThan &p_less_than, const Indexer &p_indexer) : + _less_than(p_less_than), _indexer(p_indexer) {} + +private: + bool _shift_up(uint32_t p_index) { + T value = _buffer[p_index]; + uint32_t current_index = p_index; + uint32_t parent_index = (current_index - 1) / 2; + while (current_index > 0 && _less_than(_buffer[parent_index], value)) { + _buffer[current_index] = _buffer[parent_index]; + _indexer(_buffer[current_index], current_index); + current_index = parent_index; + parent_index = (current_index - 1) / 2; + } + if (current_index != p_index) { + _buffer[current_index] = value; + _indexer(value, current_index); + return true; + } else { + return false; + } + } + + bool _shift_down(uint32_t p_index) { + T value = _buffer[p_index]; + uint32_t current_index = p_index; + uint32_t child_index = 2 * current_index + 1; + while (child_index < _buffer.size()) { + if (child_index + 1 < _buffer.size() && + _less_than(_buffer[child_index], _buffer[child_index + 1])) { + child_index++; + } + if (_less_than(_buffer[child_index], value)) { + break; + } + _buffer[current_index] = _buffer[child_index]; + _indexer(_buffer[current_index], current_index); + current_index = child_index; + child_index = 2 * current_index + 1; + } + if (current_index != p_index) { + _buffer[current_index] = value; + _indexer(value, current_index); + return true; + } else { + return false; + } + } +}; } // namespace gd #endif // NAV_UTILS_H |