From 55c8b8ec91f6e899eb808fa267de5b774331a4ca Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sat, 28 Dec 2024 01:33:40 +0100 Subject: [PATCH] Make nodes handle their respective navigation source geometry Makes nodes handle their respective navigation source geometry. --- main/main.cpp | 50 +- modules/csg/csg_shape.cpp | 35 ++ modules/csg/csg_shape.h | 11 + modules/csg/register_types.cpp | 1 + modules/gridmap/grid_map.cpp | 150 +++++ modules/gridmap/grid_map.h | 10 + modules/gridmap/register_types.cpp | 1 + .../2d/godot_navigation_server_2d.cpp | 44 +- .../navigation/2d/nav_mesh_generator_2d.cpp | 559 +----------------- modules/navigation/2d/nav_mesh_generator_2d.h | 23 +- .../3d/godot_navigation_server_3d.cpp | 42 +- .../navigation/3d/nav_mesh_generator_3d.cpp | 465 +-------------- modules/navigation/3d/nav_mesh_generator_3d.h | 28 +- modules/navigation/register_types.cpp | 3 + scene/2d/mesh_instance_2d.cpp | 105 ++++ scene/2d/mesh_instance_2d.h | 11 + scene/2d/multimesh_instance_2d.cpp | 114 ++++ scene/2d/multimesh_instance_2d.h | 11 + scene/2d/navigation_obstacle_2d.cpp | 66 +++ scene/2d/navigation_obstacle_2d.h | 11 + scene/2d/physics/static_body_2d.cpp | 136 +++++ scene/2d/physics/static_body_2d.h | 11 + scene/2d/polygon_2d.cpp | 35 ++ scene/2d/polygon_2d.h | 11 + scene/2d/tile_map.cpp | 32 + scene/2d/tile_map.h | 9 + scene/2d/tile_map_layer.cpp | 111 ++++ scene/2d/tile_map_layer.h | 9 + scene/3d/mesh_instance_3d.cpp | 32 + scene/3d/mesh_instance_3d.h | 11 + scene/3d/multimesh_instance_3d.cpp | 41 ++ scene/3d/multimesh_instance_3d.h | 11 + scene/3d/navigation_obstacle_3d.cpp | 67 +++ scene/3d/navigation_obstacle_3d.h | 11 + scene/3d/physics/static_body_3d.cpp | 151 +++++ scene/3d/physics/static_body_3d.h | 10 + scene/register_scene_types.cpp | 20 + servers/navigation_server_2d.cpp | 71 +++ servers/navigation_server_2d.h | 14 + servers/navigation_server_3d.cpp | 72 +++ servers/navigation_server_3d.h | 14 + 41 files changed, 1498 insertions(+), 1121 deletions(-) diff --git a/main/main.cpp b/main/main.cpp index 191665effd14..544a43b117f6 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -164,10 +164,8 @@ static DisplayServer *display_server = nullptr; static RenderingServer *rendering_server = nullptr; static TextServerManager *tsman = nullptr; static ThemeDB *theme_db = nullptr; -static NavigationServer2D *navigation_server_2d = nullptr; static PhysicsServer2DManager *physics_server_2d_manager = nullptr; static PhysicsServer2D *physics_server_2d = nullptr; -static NavigationServer3D *navigation_server_3d = nullptr; #ifndef _3D_DISABLED static PhysicsServer3DManager *physics_server_3d_manager = nullptr; static PhysicsServer3D *physics_server_3d = nullptr; @@ -375,44 +373,6 @@ void finalize_display() { memdelete(display_server); } -void initialize_navigation_server() { - ERR_FAIL_COND(navigation_server_3d != nullptr); - ERR_FAIL_COND(navigation_server_2d != nullptr); - - // Init 3D Navigation Server - navigation_server_3d = NavigationServer3DManager::new_default_server(); - - // Fall back to dummy if no default server has been registered. - if (!navigation_server_3d) { - navigation_server_3d = memnew(NavigationServer3DDummy); - } - - // Should be impossible, but make sure it's not null. - ERR_FAIL_NULL_MSG(navigation_server_3d, "Failed to initialize NavigationServer3D."); - navigation_server_3d->init(); - - // Init 2D Navigation Server - navigation_server_2d = NavigationServer2DManager::new_default_server(); - if (!navigation_server_2d) { - navigation_server_2d = memnew(NavigationServer2DDummy); - } - - ERR_FAIL_NULL_MSG(navigation_server_2d, "Failed to initialize NavigationServer2D."); - navigation_server_2d->init(); -} - -void finalize_navigation_server() { - ERR_FAIL_NULL(navigation_server_3d); - navigation_server_3d->finish(); - memdelete(navigation_server_3d); - navigation_server_3d = nullptr; - - ERR_FAIL_NULL(navigation_server_2d); - navigation_server_2d->finish(); - memdelete(navigation_server_2d); - navigation_server_2d = nullptr; -} - void initialize_theme_db() { theme_db = memnew(ThemeDB); } @@ -790,8 +750,6 @@ Error Main::test_setup() { // Theme needs modules to be initialized so that sub-resources can be loaded. theme_db->initialize_theme_noproject(); - initialize_navigation_server(); - ERR_FAIL_COND_V(TextServerManager::get_singleton()->get_interface_count() == 0, ERR_CANT_CREATE); /* Use one with the most features available. */ @@ -852,7 +810,8 @@ void Main::test_cleanup() { finalize_theme_db(); - finalize_navigation_server(); + NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene. + NavigationServer3DManager::finalize_server(); GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS); uninitialize_modules(MODULE_INITIALIZATION_LEVEL_SERVERS); @@ -3441,8 +3400,6 @@ Error Main::setup2(bool p_show_boot_logo) { MAIN_PRINT("Main: Load Navigation"); - initialize_navigation_server(); - register_server_singletons(); // This loads global classes, so it must happen before custom loaders and savers are registered @@ -4677,7 +4634,8 @@ void Main::cleanup(bool p_force) { finalize_theme_db(); // Before deinitializing server extensions, finalize servers which may be loaded as extensions. - finalize_navigation_server(); + NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene. + NavigationServer3DManager::finalize_server(); finalize_physics(); GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS); diff --git a/modules/csg/csg_shape.cpp b/modules/csg/csg_shape.cpp index a908b1441251..2c041297118c 100644 --- a/modules/csg/csg_shape.cpp +++ b/modules/csg/csg_shape.cpp @@ -34,9 +34,44 @@ #include "core/io/json.h" #endif // DEV_ENABLED #include "core/math/geometry_2d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/navigation_mesh.h" +#include "servers/navigation_server_3d.h" #include +Callable CSGShape3D::_navmesh_source_geometry_parsing_callback; +RID CSGShape3D::_navmesh_source_geometry_parser; + +void CSGShape3D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&CSGShape3D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void CSGShape3D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + CSGShape3D *csgshape3d = Object::cast_to(p_node); + + if (csgshape3d == nullptr) { + return; + } + + NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); + + if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS && csgshape3d->is_using_collision() && (csgshape3d->get_collision_layer() & parsed_collision_mask)) || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { + Array meshes = csgshape3d->get_meshes(); + if (!meshes.is_empty()) { + Ref mesh = meshes[1]; + if (mesh.is_valid()) { + p_source_geometry_data->add_mesh(mesh, csgshape3d->get_global_transform()); + } + } + } +} + void CSGShape3D::set_use_collision(bool p_enable) { if (use_collision == p_enable) { return; diff --git a/modules/csg/csg_shape.h b/modules/csg/csg_shape.h index e71baae85633..ea015be296c7 100644 --- a/modules/csg/csg_shape.h +++ b/modules/csg/csg_shape.h @@ -39,6 +39,9 @@ #include "thirdparty/misc/mikktspace.h" +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; + class CSGShape3D : public GeometryInstance3D { GDCLASS(CSGShape3D, GeometryInstance3D); @@ -169,6 +172,14 @@ class CSGShape3D : public GeometryInstance3D { Ref bake_static_mesh(); Ref bake_collision_shape(); +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + CSGShape3D(); ~CSGShape3D(); }; diff --git a/modules/csg/register_types.cpp b/modules/csg/register_types.cpp index 19e6be4be17b..f3f8cd5ad186 100644 --- a/modules/csg/register_types.cpp +++ b/modules/csg/register_types.cpp @@ -47,6 +47,7 @@ void initialize_csg_module(ModuleInitializationLevel p_level) { GDREGISTER_CLASS(CSGTorus3D); GDREGISTER_CLASS(CSGPolygon3D); GDREGISTER_CLASS(CSGCombiner3D); + CSGShape3D::_navmesh_parse_init(); } #ifdef TOOLS_ENABLED if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) { diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp index 5431ea97a8cf..519ac91de7eb 100644 --- a/modules/gridmap/grid_map.cpp +++ b/modules/gridmap/grid_map.cpp @@ -31,12 +31,26 @@ #include "grid_map.h" #include "core/io/marshalls.h" +#include "core/math/convex_hull.h" +#include "scene/resources/3d/box_shape_3d.h" +#include "scene/resources/3d/capsule_shape_3d.h" +#include "scene/resources/3d/concave_polygon_shape_3d.h" +#include "scene/resources/3d/convex_polygon_shape_3d.h" +#include "scene/resources/3d/cylinder_shape_3d.h" +#include "scene/resources/3d/height_map_shape_3d.h" #include "scene/resources/3d/mesh_library.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/3d/primitive_meshes.h" +#include "scene/resources/3d/shape_3d.h" +#include "scene/resources/3d/sphere_shape_3d.h" #include "scene/resources/physics_material.h" #include "scene/resources/surface_tool.h" #include "servers/navigation_server_3d.h" #include "servers/rendering_server.h" +Callable GridMap::_navmesh_source_geometry_parsing_callback; +RID GridMap::_navmesh_source_geometry_parser; + bool GridMap::_set(const StringName &p_name, const Variant &p_value) { String name = p_name; @@ -1336,6 +1350,142 @@ GridMap::GridMap() { #endif // DEBUG_ENABLED } +void GridMap::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&GridMap::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void GridMap::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + GridMap *gridmap = Object::cast_to(p_node); + + if (gridmap == nullptr) { + return; + } + + NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); + + if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { + Array meshes = gridmap->get_meshes(); + Transform3D xform = gridmap->get_global_transform(); + for (int i = 0; i < meshes.size(); i += 2) { + Ref mesh = meshes[i + 1]; + if (mesh.is_valid()) { + p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]); + } + } + } + + else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) { + Array shapes = gridmap->get_collision_shapes(); + for (int i = 0; i < shapes.size(); i += 2) { + RID shape = shapes[i + 1]; + PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape); + Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape); + + switch (type) { + case PhysicsServer3D::SHAPE_SPHERE: { + real_t radius = data; + Array arr; + arr.resize(RS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, radius, radius * 2.0); + p_source_geometry_data->add_mesh_array(arr, shapes[i]); + } break; + case PhysicsServer3D::SHAPE_BOX: { + Vector3 extents = data; + Array arr; + arr.resize(RS::ARRAY_MAX); + BoxMesh::create_mesh_array(arr, extents * 2.0); + p_source_geometry_data->add_mesh_array(arr, shapes[i]); + } break; + case PhysicsServer3D::SHAPE_CAPSULE: { + Dictionary dict = data; + real_t radius = dict["radius"]; + real_t height = dict["height"]; + Array arr; + arr.resize(RS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, radius, height); + p_source_geometry_data->add_mesh_array(arr, shapes[i]); + } break; + case PhysicsServer3D::SHAPE_CYLINDER: { + Dictionary dict = data; + real_t radius = dict["radius"]; + real_t height = dict["height"]; + Array arr; + arr.resize(RS::ARRAY_MAX); + CylinderMesh::create_mesh_array(arr, radius, radius, height); + p_source_geometry_data->add_mesh_array(arr, shapes[i]); + } break; + case PhysicsServer3D::SHAPE_CONVEX_POLYGON: { + PackedVector3Array vertices = data; + Geometry3D::MeshData md; + + Error err = ConvexHullComputer::convex_hull(vertices, md); + + if (err == OK) { + PackedVector3Array faces; + + for (const Geometry3D::MeshData::Face &face : md.faces) { + for (uint32_t k = 2; k < face.indices.size(); ++k) { + faces.push_back(md.vertices[face.indices[0]]); + faces.push_back(md.vertices[face.indices[k - 1]]); + faces.push_back(md.vertices[face.indices[k]]); + } + } + + p_source_geometry_data->add_faces(faces, shapes[i]); + } + } break; + case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: { + Dictionary dict = data; + PackedVector3Array faces = Variant(dict["faces"]); + p_source_geometry_data->add_faces(faces, shapes[i]); + } break; + case PhysicsServer3D::SHAPE_HEIGHTMAP: { + Dictionary dict = data; + ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights" + int heightmap_depth = dict["depth"]; + int heightmap_width = dict["width"]; + + if (heightmap_depth >= 2 && heightmap_width >= 2) { + const Vector &map_data = dict["heights"]; + + Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); + Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; + + Vector vertex_array; + vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); + Vector3 *vertex_array_ptrw = vertex_array.ptrw(); + const real_t *map_data_ptr = map_data.ptr(); + int vertex_index = 0; + + for (int d = 0; d < heightmap_depth - 1; d++) { + for (int w = 0; w < heightmap_width - 1; w++) { + vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); + vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); + vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_index += 6; + } + } + if (vertex_array.size() > 0) { + p_source_geometry_data->add_faces(vertex_array, shapes[i]); + } + } + } break; + default: { + WARN_PRINT("Unsupported collision shape type."); + } break; + } + } + } +} + #ifdef DEBUG_ENABLED void GridMap::_update_navigation_debug_edge_connections() { if (bake_navigation) { diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h index 1f006f661a3b..6d9cfb5e7663 100644 --- a/modules/gridmap/grid_map.h +++ b/modules/gridmap/grid_map.h @@ -38,6 +38,8 @@ //heh heh, godotsphir!! this shares no code and the design is completely different with previous projects i've done.. //should scale better with hardware that supports instancing +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; class PhysicsMaterial; class GridMap : public Node3D { @@ -300,6 +302,14 @@ class GridMap : public Node3D { Array get_bake_meshes(); RID get_bake_mesh_instance(int p_idx); +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + GridMap(); ~GridMap(); }; diff --git a/modules/gridmap/register_types.cpp b/modules/gridmap/register_types.cpp index e293dc72cee8..b3557a9cdfa7 100644 --- a/modules/gridmap/register_types.cpp +++ b/modules/gridmap/register_types.cpp @@ -43,6 +43,7 @@ void initialize_gridmap_module(ModuleInitializationLevel p_level) { if (p_level == MODULE_INITIALIZATION_LEVEL_SCENE) { GDREGISTER_CLASS(GridMap); + GridMap::_navmesh_parse_init(); } #ifdef TOOLS_ENABLED if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) { diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index 6522770e0f72..34957cf7fbd6 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -162,6 +162,8 @@ void GodotNavigationServer2D::init() { #ifdef CLIPPER2_ENABLED navmesh_generator_2d = memnew(NavMeshGenerator2D); ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D."); + RWLockRead read_lock(geometry_parser_rwlock); + navmesh_generator_2d->set_generator_parsers(generator_parsers); #endif // CLIPPER2_ENABLED } @@ -399,12 +401,19 @@ void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_ bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid); void GodotNavigationServer2D::free(RID p_object) { -#ifdef CLIPPER2_ENABLED - if (navmesh_generator_2d && navmesh_generator_2d->owns(p_object)) { - navmesh_generator_2d->free(p_object); + if (geometry_parser_owner.owns(p_object)) { + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object); + ERR_FAIL_NULL(parser); + + generator_parsers.erase(parser); +#ifndef CLIPPER2_ENABLED + NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers); +#endif + geometry_parser_owner.free(parser->self); return; } -#endif // CLIPPER2_ENABLED NavigationServer3D::get_singleton()->free(p_object); } @@ -505,18 +514,25 @@ void GodotNavigationServer2D::query_path(const Refself = rid; + + generator_parsers.push_back(parser); #ifdef CLIPPER2_ENABLED - if (navmesh_generator_2d) { - return navmesh_generator_2d->source_geometry_parser_create(); - } -#endif // CLIPPER2_ENABLED - return RID(); + NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers); +#endif + return rid; } void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { -#ifdef CLIPPER2_ENABLED - if (navmesh_generator_2d) { - navmesh_generator_2d->source_geometry_parser_set_callback(p_parser, p_callback); - } -#endif // CLIPPER2_ENABLED + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser); + ERR_FAIL_NULL(parser); + + parser->callback = p_callback; } diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation/2d/nav_mesh_generator_2d.cpp index a8eb07147cc8..f4e4ea298020 100644 --- a/modules/navigation/2d/nav_mesh_generator_2d.cpp +++ b/modules/navigation/2d/nav_mesh_generator_2d.cpp @@ -33,19 +33,8 @@ #include "nav_mesh_generator_2d.h" #include "core/config/project_settings.h" -#include "scene/2d/mesh_instance_2d.h" -#include "scene/2d/multimesh_instance_2d.h" -#include "scene/2d/navigation_obstacle_2d.h" -#include "scene/2d/physics/static_body_2d.h" -#include "scene/2d/polygon_2d.h" -#include "scene/2d/tile_map.h" -#include "scene/resources/2d/capsule_shape_2d.h" -#include "scene/resources/2d/circle_shape_2d.h" -#include "scene/resources/2d/concave_polygon_shape_2d.h" -#include "scene/resources/2d/convex_polygon_shape_2d.h" #include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" #include "scene/resources/2d/navigation_polygon.h" -#include "scene/resources/2d/rectangle_shape_2d.h" #include "thirdparty/clipper2/include/clipper2/clipper.h" #include "thirdparty/misc/polypartition.h" @@ -53,14 +42,13 @@ NavMeshGenerator2D *NavMeshGenerator2D::singleton = nullptr; Mutex NavMeshGenerator2D::baking_navmesh_mutex; Mutex NavMeshGenerator2D::generator_task_mutex; -RWLock NavMeshGenerator2D::generator_rid_rwlock; +RWLock NavMeshGenerator2D::generator_parsers_rwlock; bool NavMeshGenerator2D::use_threads = true; bool NavMeshGenerator2D::baking_use_multiple_threads = true; bool NavMeshGenerator2D::baking_use_high_priority_threads = true; HashSet> NavMeshGenerator2D::baking_navmeshes; HashMap NavMeshGenerator2D::generator_tasks; -RID_Owner NavMeshGenerator2D::generator_parser_owner; -LocalVector NavMeshGenerator2D::generator_parsers; +LocalVector NavMeshGenerator2D::generator_parsers; NavMeshGenerator2D *NavMeshGenerator2D::get_singleton() { return singleton; @@ -129,12 +117,9 @@ void NavMeshGenerator2D::cleanup() { } generator_tasks.clear(); - generator_rid_rwlock.write_lock(); - for (NavMeshGeometryParser2D *parser : generator_parsers) { - generator_parser_owner.free(parser->self); - } + generator_parsers_rwlock.write_lock(); generator_parsers.clear(); - generator_rid_rwlock.write_unlock(); + generator_parsers_rwlock.write_unlock(); } } @@ -234,510 +219,25 @@ void NavMeshGenerator2D::generator_thread_bake(void *p_arg) { } void NavMeshGenerator2D::generator_parse_geometry_node(Ref p_navigation_mesh, Ref p_source_geometry_data, Node *p_node, bool p_recurse_children) { - generator_parse_meshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_multimeshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_polygon2d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_staticbody2d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_tile_map_layer_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node); - - generator_rid_rwlock.read_lock(); + generator_parsers_rwlock.read_lock(); for (const NavMeshGeometryParser2D *parser : generator_parsers) { if (!parser->callback.is_valid()) { continue; } parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node); } - generator_rid_rwlock.read_unlock(); + generator_parsers_rwlock.read_unlock(); if (p_recurse_children) { for (int i = 0; i < p_node->get_child_count(); i++) { generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children); } - } else if (Object::cast_to(p_node)) { - // 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(p_node->get_child(i)); - 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); - } - } - } -} - -void NavMeshGenerator2D::generator_parse_meshinstance2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - MeshInstance2D *mesh_instance = Object::cast_to(p_node); - - if (mesh_instance == nullptr) { - return; - } - - NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - - if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { - return; - } - - Ref mesh = mesh_instance->get_mesh(); - if (!mesh.is_valid()) { - return; - } - - const Transform2D mesh_instance_xform = p_source_geometry_data->root_node_transform * mesh_instance->get_global_transform(); - - using namespace Clipper2Lib; - - PathsD subject_paths, dummy_clip_paths; - - for (int i = 0; i < mesh->get_surface_count(); i++) { - if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) { - continue; - } - - if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) { - continue; - } - - PathD subject_path; - - int index_count = 0; - if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { - index_count = mesh->surface_get_array_index_len(i); - } else { - index_count = mesh->surface_get_array_len(i); - } - - ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0)); - - Array a = mesh->surface_get_arrays(i); - - Vector mesh_vertices = a[Mesh::ARRAY_VERTEX]; - - if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { - Vector mesh_indices = a[Mesh::ARRAY_INDEX]; - for (int vertex_index : mesh_indices) { - const Vector2 &vertex = mesh_vertices[vertex_index]; - const PointD &point = PointD(vertex.x, vertex.y); - subject_path.push_back(point); - } - } else { - for (const Vector2 &vertex : mesh_vertices) { - const PointD &point = PointD(vertex.x, vertex.y); - subject_path.push_back(point); - } - } - subject_paths.push_back(subject_path); - } - - PathsD path_solution; - - path_solution = Union(subject_paths, dummy_clip_paths, FillRule::NonZero); - - //path_solution = RamerDouglasPeucker(path_solution, 0.025); - - Vector> polypaths; - - for (const PathD &scaled_path : path_solution) { - Vector shape_outline; - for (const PointD &scaled_point : scaled_path) { - shape_outline.push_back(Point2(static_cast(scaled_point.x), static_cast(scaled_point.y))); - } - - for (int i = 0; i < shape_outline.size(); i++) { - shape_outline.write[i] = mesh_instance_xform.xform(shape_outline[i]); - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } -} - -void NavMeshGenerator2D::generator_parse_multimeshinstance2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - MultiMeshInstance2D *multimesh_instance = Object::cast_to(p_node); - - if (multimesh_instance == nullptr) { - return; - } - - NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { - return; - } - - Ref multimesh = multimesh_instance->get_multimesh(); - if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) { - return; - } - - Ref mesh = multimesh->get_mesh(); - if (!mesh.is_valid()) { - return; - } - - using namespace Clipper2Lib; - - PathsD mesh_subject_paths, dummy_clip_paths; - - for (int i = 0; i < mesh->get_surface_count(); i++) { - if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) { - continue; - } - - if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) { - continue; - } - - PathD subject_path; - - int index_count = 0; - if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { - index_count = mesh->surface_get_array_index_len(i); - } else { - index_count = mesh->surface_get_array_len(i); - } - - ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0)); - - Array a = mesh->surface_get_arrays(i); - - Vector mesh_vertices = a[Mesh::ARRAY_VERTEX]; - - if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { - Vector mesh_indices = a[Mesh::ARRAY_INDEX]; - for (int vertex_index : mesh_indices) { - const Vector2 &vertex = mesh_vertices[vertex_index]; - const PointD &point = PointD(vertex.x, vertex.y); - subject_path.push_back(point); - } - } else { - for (const Vector2 &vertex : mesh_vertices) { - const PointD &point = PointD(vertex.x, vertex.y); - subject_path.push_back(point); - } - } - mesh_subject_paths.push_back(subject_path); - } - - PathsD mesh_path_solution = Union(mesh_subject_paths, dummy_clip_paths, FillRule::NonZero); - - //path_solution = RamerDouglasPeucker(path_solution, 0.025); - - int multimesh_instance_count = multimesh->get_visible_instance_count(); - if (multimesh_instance_count == -1) { - multimesh_instance_count = multimesh->get_instance_count(); - } - - const Transform2D multimesh_instance_xform = p_source_geometry_data->root_node_transform * multimesh_instance->get_global_transform(); - - for (int i = 0; i < multimesh_instance_count; i++) { - const Transform2D multimesh_instance_mesh_instance_xform = multimesh_instance_xform * multimesh->get_instance_transform_2d(i); - - for (const PathD &mesh_path : mesh_path_solution) { - Vector shape_outline; - - for (const PointD &mesh_path_point : mesh_path) { - shape_outline.push_back(Point2(static_cast(mesh_path_point.x), static_cast(mesh_path_point.y))); - } - - for (int j = 0; j < shape_outline.size(); j++) { - shape_outline.write[j] = multimesh_instance_mesh_instance_xform.xform(shape_outline[j]); - } - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - } -} - -void NavMeshGenerator2D::generator_parse_polygon2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - Polygon2D *polygon_2d = Object::cast_to(p_node); - - if (polygon_2d == nullptr) { - return; - } - - NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - - if (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) { - const Transform2D polygon_2d_xform = p_source_geometry_data->root_node_transform * polygon_2d->get_global_transform(); - - Vector shape_outline = polygon_2d->get_polygon(); - for (int i = 0; i < shape_outline.size(); i++) { - shape_outline.write[i] = polygon_2d_xform.xform(shape_outline[i]); - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } -} - -void NavMeshGenerator2D::generator_parse_staticbody2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - StaticBody2D *static_body = Object::cast_to(p_node); - - if (static_body == nullptr) { - return; - } - - NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { - return; - } - - uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask(); - if (!(static_body->get_collision_layer() & parsed_collision_mask)) { - return; - } - - List shape_owners; - static_body->get_shape_owners(&shape_owners); - - for (uint32_t shape_owner : shape_owners) { - if (static_body->is_shape_owner_disabled(shape_owner)) { - continue; - } - - const int shape_count = static_body->shape_owner_get_shape_count(shape_owner); - - for (int shape_index = 0; shape_index < shape_count; shape_index++) { - Ref s = static_body->shape_owner_get_shape(shape_owner, shape_index); - - if (s.is_null()) { - continue; - } - - const Transform2D static_body_xform = p_source_geometry_data->root_node_transform * static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner); - - RectangleShape2D *rectangle_shape = Object::cast_to(*s); - if (rectangle_shape) { - Vector shape_outline; - - const Vector2 &rectangle_size = rectangle_shape->get_size(); - - shape_outline.resize(5); - shape_outline.write[0] = static_body_xform.xform(-rectangle_size * 0.5); - shape_outline.write[1] = static_body_xform.xform(Vector2(rectangle_size.x, -rectangle_size.y) * 0.5); - shape_outline.write[2] = static_body_xform.xform(rectangle_size * 0.5); - shape_outline.write[3] = static_body_xform.xform(Vector2(-rectangle_size.x, rectangle_size.y) * 0.5); - shape_outline.write[4] = static_body_xform.xform(-rectangle_size * 0.5); - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - - CapsuleShape2D *capsule_shape = Object::cast_to(*s); - if (capsule_shape) { - const real_t capsule_height = capsule_shape->get_height(); - const real_t capsule_radius = capsule_shape->get_radius(); - - Vector shape_outline; - const real_t turn_step = Math_TAU / 12.0; - shape_outline.resize(14); - int shape_outline_inx = 0; - for (int i = 0; i < 12; i++) { - Vector2 ofs = Vector2(0, (i > 3 && i <= 9) ? -capsule_height * 0.5 + capsule_radius : capsule_height * 0.5 - capsule_radius); - - shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius + ofs); - shape_outline_inx += 1; - if (i == 3 || i == 9) { - shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius - ofs); - shape_outline_inx += 1; - } - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - - CircleShape2D *circle_shape = Object::cast_to(*s); - if (circle_shape) { - const real_t circle_radius = circle_shape->get_radius(); - - Vector shape_outline; - int circle_edge_count = 12; - shape_outline.resize(circle_edge_count); - - const real_t turn_step = Math_TAU / real_t(circle_edge_count); - for (int i = 0; i < circle_edge_count; i++) { - shape_outline.write[i] = static_body_xform.xform(Vector2(Math::cos(i * turn_step), Math::sin(i * turn_step)) * circle_radius); - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - - ConcavePolygonShape2D *concave_polygon_shape = Object::cast_to(*s); - if (concave_polygon_shape) { - Vector shape_outline = concave_polygon_shape->get_segments(); - - for (int i = 0; i < shape_outline.size(); i++) { - shape_outline.write[i] = static_body_xform.xform(shape_outline[i]); - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - - ConvexPolygonShape2D *convex_polygon_shape = Object::cast_to(*s); - if (convex_polygon_shape) { - Vector shape_outline = convex_polygon_shape->get_points(); - - for (int i = 0; i < shape_outline.size(); i++) { - shape_outline.write[i] = static_body_xform.xform(shape_outline[i]); - } - - p_source_geometry_data->add_obstruction_outline(shape_outline); - } - } - } -} - -void NavMeshGenerator2D::generator_parse_tile_map_layer_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - TileMapLayer *tile_map_layer = Object::cast_to(p_node); - if (tile_map_layer == nullptr) { - return; - } - - Ref tile_set = tile_map_layer->get_tile_set(); - if (!tile_set.is_valid()) { - return; - } - - int physics_layers_count = tile_set->get_physics_layers_count(); - int navigation_layers_count = tile_set->get_navigation_layers_count(); - if (physics_layers_count <= 0 && navigation_layers_count <= 0) { - return; - } - - NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask(); - - const Transform2D tilemap_xform = p_source_geometry_data->root_node_transform * tile_map_layer->get_global_transform(); - - TypedArray used_cells = tile_map_layer->get_used_cells(); - for (int used_cell_index = 0; used_cell_index < used_cells.size(); used_cell_index++) { - const Vector2i &cell = used_cells[used_cell_index]; - - const TileData *tile_data = tile_map_layer->get_cell_tile_data(cell); - if (tile_data == nullptr) { - continue; - } - - // Transform flags. - const int alternative_id = tile_map_layer->get_cell_alternative_tile(cell); - bool flip_h = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_H); - bool flip_v = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_V); - bool transpose = (alternative_id & TileSetAtlasSource::TRANSFORM_TRANSPOSE); - - Transform2D tile_transform; - tile_transform.set_origin(tile_map_layer->map_to_local(cell)); - - const Transform2D tile_transform_offset = tilemap_xform * tile_transform; - - // Parse traversable polygons. - for (int navigation_layer = 0; navigation_layer < navigation_layers_count; navigation_layer++) { - Ref navigation_polygon = tile_data->get_navigation_polygon(navigation_layer, flip_h, flip_v, transpose); - if (navigation_polygon.is_valid()) { - for (int outline_index = 0; outline_index < navigation_polygon->get_outline_count(); outline_index++) { - const Vector &navigation_polygon_outline = navigation_polygon->get_outline(outline_index); - if (navigation_polygon_outline.is_empty()) { - continue; - } - - Vector traversable_outline; - traversable_outline.resize(navigation_polygon_outline.size()); - - const Vector2 *navigation_polygon_outline_ptr = navigation_polygon_outline.ptr(); - Vector2 *traversable_outline_ptrw = traversable_outline.ptrw(); - - for (int traversable_outline_index = 0; traversable_outline_index < traversable_outline.size(); traversable_outline_index++) { - traversable_outline_ptrw[traversable_outline_index] = tile_transform_offset.xform(navigation_polygon_outline_ptr[traversable_outline_index]); - } - - p_source_geometry_data->_add_traversable_outline(traversable_outline); - } - } - } - - // Parse obstacles. - for (int physics_layer = 0; physics_layer < physics_layers_count; physics_layer++) { - if ((parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) && - (tile_set->get_physics_layer_collision_layer(physics_layer) & parsed_collision_mask)) { - for (int collision_polygon_index = 0; collision_polygon_index < tile_data->get_collision_polygons_count(physics_layer); collision_polygon_index++) { - PackedVector2Array collision_polygon_points = tile_data->get_collision_polygon_points(physics_layer, collision_polygon_index); - if (collision_polygon_points.is_empty()) { - continue; - } - - if (flip_h || flip_v || transpose) { - collision_polygon_points = TileData::get_transformed_vertices(collision_polygon_points, flip_h, flip_v, transpose); - } - - Vector obstruction_outline; - obstruction_outline.resize(collision_polygon_points.size()); - - const Vector2 *collision_polygon_points_ptr = collision_polygon_points.ptr(); - Vector2 *obstruction_outline_ptrw = obstruction_outline.ptrw(); - - for (int obstruction_outline_index = 0; obstruction_outline_index < obstruction_outline.size(); obstruction_outline_index++) { - obstruction_outline_ptrw[obstruction_outline_index] = tile_transform_offset.xform(collision_polygon_points_ptr[obstruction_outline_index]); - } - - p_source_geometry_data->_add_obstruction_outline(obstruction_outline); - } - } - } } } -void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - NavigationObstacle2D *obstacle = Object::cast_to(p_node); - if (obstacle == nullptr) { - return; - } - - if (!obstacle->get_affect_navigation_mesh()) { - return; - } - - const Vector2 safe_scale = obstacle->get_global_scale().abs().maxf(0.001); - const float obstacle_radius = obstacle->get_radius(); - - if (obstacle_radius > 0.0) { - // Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis. - const float scaling_max_value = safe_scale[safe_scale.max_axis_index()]; - const Vector2 uniform_max_scale = Vector2(scaling_max_value, scaling_max_value); - const Transform2D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform2D(obstacle->get_global_rotation(), uniform_max_scale, 0.0, obstacle->get_global_position()); - - Vector obstruction_circle_vertices; - - // The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding. - // Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck. - // No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty. - static const int circle_points = 12; - - obstruction_circle_vertices.resize(circle_points); - Vector2 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw(); - const real_t circle_point_step = Math_TAU / circle_points; - - for (int i = 0; i < circle_points; i++) { - const float angle = i * circle_point_step; - circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius)); - } - - p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh()); - } - - // Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account. - const Transform2D node_xform = p_source_geometry_data->root_node_transform * obstacle->get_global_transform(); - - const Vector &obstacle_vertices = obstacle->get_vertices(); - - if (obstacle_vertices.is_empty()) { - return; - } - - Vector obstruction_shape_vertices; - obstruction_shape_vertices.resize(obstacle_vertices.size()); - - const Vector2 *obstacle_vertices_ptr = obstacle_vertices.ptr(); - Vector2 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw(); - - for (int i = 0; i < obstacle_vertices.size(); i++) { - obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]); - } - p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_carve_navigation_mesh()); +void NavMeshGenerator2D::set_generator_parsers(LocalVector p_parsers) { + RWLockWrite write_lock(generator_parsers_rwlock); + generator_parsers = p_parsers; } void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node) { @@ -801,47 +301,6 @@ bool NavMeshGenerator2D::generator_emit_callback(const Callable &p_callback) { return ce.error == Callable::CallError::CALL_OK; } -RID NavMeshGenerator2D::source_geometry_parser_create() { - RWLockWrite write_lock(generator_rid_rwlock); - - RID rid = generator_parser_owner.make_rid(); - - NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(rid); - parser->self = rid; - - generator_parsers.push_back(parser); - - return rid; -} - -void NavMeshGenerator2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { - RWLockWrite write_lock(generator_rid_rwlock); - - NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(p_parser); - ERR_FAIL_NULL(parser); - - parser->callback = p_callback; -} - -bool NavMeshGenerator2D::owns(RID p_object) { - RWLockRead read_lock(generator_rid_rwlock); - return generator_parser_owner.owns(p_object); -} - -void NavMeshGenerator2D::free(RID p_object) { - RWLockWrite write_lock(generator_rid_rwlock); - - if (generator_parser_owner.owns(p_object)) { - NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(p_object); - - generator_parsers.erase(parser); - - generator_parser_owner.free(p_object); - } else { - ERR_PRINT("Attempted to free a NavMeshGenerator2D RID that did not exist (or was already freed)."); - } -} - void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data) { if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) { return; diff --git a/modules/navigation/2d/nav_mesh_generator_2d.h b/modules/navigation/2d/nav_mesh_generator_2d.h index d5f9694242aa..1c2ae324b342 100644 --- a/modules/navigation/2d/nav_mesh_generator_2d.h +++ b/modules/navigation/2d/nav_mesh_generator_2d.h @@ -36,6 +36,7 @@ #include "core/object/class_db.h" #include "core/object/worker_thread_pool.h" #include "core/templates/rid_owner.h" +#include "servers/navigation_server_2d.h" class Node; class NavigationPolygon; @@ -47,12 +48,7 @@ class NavMeshGenerator2D : public Object { static Mutex baking_navmesh_mutex; static Mutex generator_task_mutex; - static RWLock generator_rid_rwlock; - struct NavMeshGeometryParser2D { - RID self; - Callable callback; - }; - static RID_Owner generator_parser_owner; + static RWLock generator_parsers_rwlock; static LocalVector generator_parsers; static bool use_threads; @@ -85,13 +81,6 @@ class NavMeshGenerator2D : public Object { static void generator_parse_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node); static void generator_bake_from_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data); - static void generator_parse_meshinstance2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_multimeshinstance2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_polygon2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_staticbody2d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_tile_map_layer_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_navigationobstacle_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static bool generator_emit_callback(const Callable &p_callback); public: @@ -101,17 +90,13 @@ class NavMeshGenerator2D : public Object { static void cleanup(); static void finish(); + static void set_generator_parsers(LocalVector p_parsers); + static void parse_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()); static void bake_from_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, const Callable &p_callback = Callable()); static void bake_from_source_geometry_data_async(Ref p_navigation_mesh, Ref p_source_geometry_data, const Callable &p_callback = Callable()); static bool is_baking(Ref p_navigation_polygon); - static RID source_geometry_parser_create(); - static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback); - - static bool owns(RID p_object); - static void free(RID p_object); - NavMeshGenerator2D(); ~NavMeshGenerator2D(); }; diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 1783e6bbdc95..3c0864058e0e 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -1263,10 +1263,17 @@ COMMAND_1(free, RID, p_object) { } else if (obstacle_owner.owns(p_object)) { internal_free_obstacle(p_object); + } else if (geometry_parser_owner.owns(p_object)) { + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_object); + ERR_FAIL_NULL(parser); + + generator_parsers.erase(parser); #ifndef _3D_DISABLED - } else if (navmesh_generator_3d && navmesh_generator_3d->owns(p_object)) { - navmesh_generator_3d->free(p_object); -#endif // _3D_DISABLED + NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers); +#endif + geometry_parser_owner.free(parser->self); } else { ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed)."); @@ -1401,6 +1408,8 @@ void GodotNavigationServer3D::process(real_t p_delta_time) { void GodotNavigationServer3D::init() { #ifndef _3D_DISABLED navmesh_generator_3d = memnew(NavMeshGenerator3D); + RWLockRead read_lock(geometry_parser_rwlock); + navmesh_generator_3d->set_generator_parsers(generator_parsers); #endif // _3D_DISABLED } @@ -1426,20 +1435,27 @@ void GodotNavigationServer3D::query_path(const Refself = rid; + + generator_parsers.push_back(parser); #ifndef _3D_DISABLED - if (navmesh_generator_3d) { - return navmesh_generator_3d->source_geometry_parser_create(); - } -#endif // _3D_DISABLED - return RID(); + NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers); +#endif + return rid; } void GodotNavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { -#ifndef _3D_DISABLED - if (navmesh_generator_3d) { - navmesh_generator_3d->source_geometry_parser_set_callback(p_parser, p_callback); - } -#endif // _3D_DISABLED + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_parser); + ERR_FAIL_NULL(parser); + + parser->callback = p_callback; } Vector GodotNavigationServer3D::simplify_path(const Vector &p_path, real_t p_epsilon) { diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp index e263a0ec5129..87cb3cad65ff 100644 --- a/modules/navigation/3d/nav_mesh_generator_3d.cpp +++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp @@ -33,46 +33,23 @@ #include "nav_mesh_generator_3d.h" #include "core/config/project_settings.h" -#include "core/math/convex_hull.h" #include "core/os/thread.h" -#include "scene/3d/mesh_instance_3d.h" -#include "scene/3d/multimesh_instance_3d.h" -#include "scene/3d/navigation_obstacle_3d.h" -#include "scene/3d/physics/static_body_3d.h" -#include "scene/resources/3d/box_shape_3d.h" -#include "scene/resources/3d/capsule_shape_3d.h" -#include "scene/resources/3d/concave_polygon_shape_3d.h" -#include "scene/resources/3d/convex_polygon_shape_3d.h" -#include "scene/resources/3d/cylinder_shape_3d.h" -#include "scene/resources/3d/height_map_shape_3d.h" +#include "scene/3d/node_3d.h" #include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" -#include "scene/resources/3d/primitive_meshes.h" -#include "scene/resources/3d/shape_3d.h" -#include "scene/resources/3d/sphere_shape_3d.h" #include "scene/resources/navigation_mesh.h" -#include "modules/modules_enabled.gen.h" // For csg, gridmap. - -#ifdef MODULE_CSG_ENABLED -#include "modules/csg/csg_shape.h" -#endif -#ifdef MODULE_GRIDMAP_ENABLED -#include "modules/gridmap/grid_map.h" -#endif - #include NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr; Mutex NavMeshGenerator3D::baking_navmesh_mutex; Mutex NavMeshGenerator3D::generator_task_mutex; -RWLock NavMeshGenerator3D::generator_rid_rwlock; +RWLock NavMeshGenerator3D::generator_parsers_rwlock; bool NavMeshGenerator3D::use_threads = true; bool NavMeshGenerator3D::baking_use_multiple_threads = true; bool NavMeshGenerator3D::baking_use_high_priority_threads = true; HashSet> NavMeshGenerator3D::baking_navmeshes; HashMap NavMeshGenerator3D::generator_tasks; -RID_Owner NavMeshGenerator3D::generator_parser_owner; -LocalVector NavMeshGenerator3D::generator_parsers; +LocalVector NavMeshGenerator3D::generator_parsers; NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() { return singleton; @@ -141,12 +118,9 @@ void NavMeshGenerator3D::cleanup() { } generator_tasks.clear(); - generator_rid_rwlock.write_lock(); - for (NavMeshGeometryParser3D *parser : generator_parsers) { - generator_parser_owner.free(parser->self); - } + generator_parsers_rwlock.write_lock(); generator_parsers.clear(); - generator_rid_rwlock.write_unlock(); + generator_parsers_rwlock.write_unlock(); } } @@ -247,25 +221,14 @@ void NavMeshGenerator3D::generator_thread_bake(void *p_arg) { } void NavMeshGenerator3D::generator_parse_geometry_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node, bool p_recurse_children) { - generator_parse_meshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_multimeshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node); - generator_parse_staticbody3d_node(p_navigation_mesh, p_source_geometry_data, p_node); -#ifdef MODULE_CSG_ENABLED - generator_parse_csgshape3d_node(p_navigation_mesh, p_source_geometry_data, p_node); -#endif -#ifdef MODULE_GRIDMAP_ENABLED - generator_parse_gridmap_node(p_navigation_mesh, p_source_geometry_data, p_node); -#endif - generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node); - - generator_rid_rwlock.read_lock(); + generator_parsers_rwlock.read_lock(); for (const NavMeshGeometryParser3D *parser : generator_parsers) { if (!parser->callback.is_valid()) { continue; } parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node); } - generator_rid_rwlock.read_unlock(); + generator_parsers_rwlock.read_unlock(); if (p_recurse_children) { for (int i = 0; i < p_node->get_child_count(); i++) { @@ -274,376 +237,9 @@ void NavMeshGenerator3D::generator_parse_geometry_node(const Ref } } -void NavMeshGenerator3D::generator_parse_meshinstance3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - MeshInstance3D *mesh_instance = Object::cast_to(p_node); - - if (mesh_instance) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - - if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { - Ref mesh = mesh_instance->get_mesh(); - if (mesh.is_valid()) { - p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform()); - } - } - } -} - -void NavMeshGenerator3D::generator_parse_multimeshinstance3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - MultiMeshInstance3D *multimesh_instance = Object::cast_to(p_node); - - if (multimesh_instance) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - - if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { - Ref multimesh = multimesh_instance->get_multimesh(); - if (multimesh.is_valid()) { - Ref mesh = multimesh->get_mesh(); - if (mesh.is_valid()) { - int n = multimesh->get_visible_instance_count(); - if (n == -1) { - n = multimesh->get_instance_count(); - } - for (int i = 0; i < n; i++) { - p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i)); - } - } - } - } - } -} - -void NavMeshGenerator3D::generator_parse_staticbody3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - StaticBody3D *static_body = Object::cast_to(p_node); - - if (static_body) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); - - if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) { - List shape_owners; - static_body->get_shape_owners(&shape_owners); - for (uint32_t shape_owner : shape_owners) { - if (static_body->is_shape_owner_disabled(shape_owner)) { - continue; - } - const int shape_count = static_body->shape_owner_get_shape_count(shape_owner); - for (int shape_index = 0; shape_index < shape_count; shape_index++) { - Ref s = static_body->shape_owner_get_shape(shape_owner, shape_index); - if (s.is_null()) { - continue; - } - - const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner); - - BoxShape3D *box = Object::cast_to(*s); - if (box) { - Array arr; - arr.resize(RS::ARRAY_MAX); - BoxMesh::create_mesh_array(arr, box->get_size()); - p_source_geometry_data->add_mesh_array(arr, transform); - } - - CapsuleShape3D *capsule = Object::cast_to(*s); - if (capsule) { - Array arr; - arr.resize(RS::ARRAY_MAX); - CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height()); - p_source_geometry_data->add_mesh_array(arr, transform); - } - - CylinderShape3D *cylinder = Object::cast_to(*s); - if (cylinder) { - Array arr; - arr.resize(RS::ARRAY_MAX); - CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height()); - p_source_geometry_data->add_mesh_array(arr, transform); - } - - SphereShape3D *sphere = Object::cast_to(*s); - if (sphere) { - Array arr; - arr.resize(RS::ARRAY_MAX); - SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0); - p_source_geometry_data->add_mesh_array(arr, transform); - } - - ConcavePolygonShape3D *concave_polygon = Object::cast_to(*s); - if (concave_polygon) { - p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform); - } - - ConvexPolygonShape3D *convex_polygon = Object::cast_to(*s); - if (convex_polygon) { - Vector varr = Variant(convex_polygon->get_points()); - Geometry3D::MeshData md; - - Error err = ConvexHullComputer::convex_hull(varr, md); - - if (err == OK) { - PackedVector3Array faces; - - for (const Geometry3D::MeshData::Face &face : md.faces) { - for (uint32_t k = 2; k < face.indices.size(); ++k) { - faces.push_back(md.vertices[face.indices[0]]); - faces.push_back(md.vertices[face.indices[k - 1]]); - faces.push_back(md.vertices[face.indices[k]]); - } - } - - p_source_geometry_data->add_faces(faces, transform); - } - } - - HeightMapShape3D *heightmap_shape = Object::cast_to(*s); - if (heightmap_shape) { - int heightmap_depth = heightmap_shape->get_map_depth(); - int heightmap_width = heightmap_shape->get_map_width(); - - if (heightmap_depth >= 2 && heightmap_width >= 2) { - const Vector &map_data = heightmap_shape->get_map_data(); - - Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); - Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; - - Vector vertex_array; - vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); - Vector3 *vertex_array_ptrw = vertex_array.ptrw(); - const real_t *map_data_ptr = map_data.ptr(); - int vertex_index = 0; - - for (int d = 0; d < heightmap_depth - 1; d++) { - for (int w = 0; w < heightmap_width - 1; w++) { - vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); - vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); - vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); - vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); - vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); - vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); - vertex_index += 6; - } - } - if (vertex_array.size() > 0) { - p_source_geometry_data->add_faces(vertex_array, transform); - } - } - } - } - } - } - } -} - -#ifdef MODULE_CSG_ENABLED -void NavMeshGenerator3D::generator_parse_csgshape3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - CSGShape3D *csgshape3d = Object::cast_to(p_node); - - if (csgshape3d) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); - - if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS && csgshape3d->is_using_collision() && (csgshape3d->get_collision_layer() & parsed_collision_mask)) || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { - CSGShape3D *csg_shape = Object::cast_to(p_node); - Array meshes = csg_shape->get_meshes(); - if (!meshes.is_empty()) { - Ref mesh = meshes[1]; - if (mesh.is_valid()) { - p_source_geometry_data->add_mesh(mesh, csg_shape->get_global_transform()); - } - } - } - } -} -#endif // MODULE_CSG_ENABLED - -#ifdef MODULE_GRIDMAP_ENABLED -void NavMeshGenerator3D::generator_parse_gridmap_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - GridMap *gridmap = Object::cast_to(p_node); - - if (gridmap) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); - uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); - - if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { - Array meshes = gridmap->get_meshes(); - Transform3D xform = gridmap->get_global_transform(); - for (int i = 0; i < meshes.size(); i += 2) { - Ref mesh = meshes[i + 1]; - if (mesh.is_valid()) { - p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]); - } - } - } - - else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) { - Array shapes = gridmap->get_collision_shapes(); - for (int i = 0; i < shapes.size(); i += 2) { - RID shape = shapes[i + 1]; - PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape); - Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape); - - switch (type) { - case PhysicsServer3D::SHAPE_SPHERE: { - real_t radius = data; - Array arr; - arr.resize(RS::ARRAY_MAX); - SphereMesh::create_mesh_array(arr, radius, radius * 2.0); - p_source_geometry_data->add_mesh_array(arr, shapes[i]); - } break; - case PhysicsServer3D::SHAPE_BOX: { - Vector3 extents = data; - Array arr; - arr.resize(RS::ARRAY_MAX); - BoxMesh::create_mesh_array(arr, extents * 2.0); - p_source_geometry_data->add_mesh_array(arr, shapes[i]); - } break; - case PhysicsServer3D::SHAPE_CAPSULE: { - Dictionary dict = data; - real_t radius = dict["radius"]; - real_t height = dict["height"]; - Array arr; - arr.resize(RS::ARRAY_MAX); - CapsuleMesh::create_mesh_array(arr, radius, height); - p_source_geometry_data->add_mesh_array(arr, shapes[i]); - } break; - case PhysicsServer3D::SHAPE_CYLINDER: { - Dictionary dict = data; - real_t radius = dict["radius"]; - real_t height = dict["height"]; - Array arr; - arr.resize(RS::ARRAY_MAX); - CylinderMesh::create_mesh_array(arr, radius, radius, height); - p_source_geometry_data->add_mesh_array(arr, shapes[i]); - } break; - case PhysicsServer3D::SHAPE_CONVEX_POLYGON: { - PackedVector3Array vertices = data; - Geometry3D::MeshData md; - - Error err = ConvexHullComputer::convex_hull(vertices, md); - - if (err == OK) { - PackedVector3Array faces; - - for (const Geometry3D::MeshData::Face &face : md.faces) { - for (uint32_t k = 2; k < face.indices.size(); ++k) { - faces.push_back(md.vertices[face.indices[0]]); - faces.push_back(md.vertices[face.indices[k - 1]]); - faces.push_back(md.vertices[face.indices[k]]); - } - } - - p_source_geometry_data->add_faces(faces, shapes[i]); - } - } break; - case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: { - Dictionary dict = data; - PackedVector3Array faces = Variant(dict["faces"]); - p_source_geometry_data->add_faces(faces, shapes[i]); - } break; - case PhysicsServer3D::SHAPE_HEIGHTMAP: { - Dictionary dict = data; - ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights" - int heightmap_depth = dict["depth"]; - int heightmap_width = dict["width"]; - - if (heightmap_depth >= 2 && heightmap_width >= 2) { - const Vector &map_data = dict["heights"]; - - Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); - Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; - - Vector vertex_array; - vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); - Vector3 *vertex_array_ptrw = vertex_array.ptrw(); - const real_t *map_data_ptr = map_data.ptr(); - int vertex_index = 0; - - for (int d = 0; d < heightmap_depth - 1; d++) { - for (int w = 0; w < heightmap_width - 1; w++) { - vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); - vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); - vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); - vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); - vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); - vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); - vertex_index += 6; - } - } - if (vertex_array.size() > 0) { - p_source_geometry_data->add_faces(vertex_array, shapes[i]); - } - } - } break; - default: { - WARN_PRINT("Unsupported collision shape type."); - } break; - } - } - } - } -} -#endif // MODULE_GRIDMAP_ENABLED - -void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { - NavigationObstacle3D *obstacle = Object::cast_to(p_node); - if (obstacle == nullptr) { - return; - } - - if (!obstacle->get_affect_navigation_mesh()) { - return; - } - - const float elevation = obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y; - // Prevent non-positive scaling. - const Vector3 safe_scale = obstacle->get_global_basis().get_scale().abs().maxf(0.001); - const float obstacle_radius = obstacle->get_radius(); - - if (obstacle_radius > 0.0) { - // Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis. - const float scaling_max_value = safe_scale[safe_scale.max_axis_index()]; - const Vector3 uniform_max_scale = Vector3(scaling_max_value, scaling_max_value, scaling_max_value); - const Transform3D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(uniform_max_scale), obstacle->get_global_position()); - - Vector obstruction_circle_vertices; - - // The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding. - // Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck. - // No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty. - static const int circle_points = 12; - - obstruction_circle_vertices.resize(circle_points); - Vector3 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw(); - const real_t circle_point_step = Math_TAU / circle_points; - - for (int i = 0; i < circle_points; i++) { - const float angle = i * circle_point_step; - circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius)); - } - - p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, elevation - obstacle_radius, scaling_max_value * obstacle_radius, obstacle->get_carve_navigation_mesh()); - } - - // Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account. - const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y), obstacle->get_global_position()); - - const Vector &obstacle_vertices = obstacle->get_vertices(); - - if (obstacle_vertices.is_empty()) { - return; - } - - Vector obstruction_shape_vertices; - obstruction_shape_vertices.resize(obstacle_vertices.size()); - - const Vector3 *obstacle_vertices_ptr = obstacle_vertices.ptr(); - Vector3 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw(); - - for (int i = 0; i < obstacle_vertices.size(); i++) { - obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]); - obstruction_shape_vertices_ptrw[i].y = 0.0; - } - p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, elevation, safe_scale.y * obstacle->get_height(), obstacle->get_carve_navigation_mesh()); +void NavMeshGenerator3D::set_generator_parsers(LocalVector p_parsers) { + RWLockWrite write_lock(generator_parsers_rwlock); + generator_parsers = p_parsers; } void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node) { @@ -965,45 +561,4 @@ bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) { return ce.error == Callable::CallError::CALL_OK; } -RID NavMeshGenerator3D::source_geometry_parser_create() { - RWLockWrite write_lock(generator_rid_rwlock); - - RID rid = generator_parser_owner.make_rid(); - - NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(rid); - parser->self = rid; - - generator_parsers.push_back(parser); - - return rid; -} - -void NavMeshGenerator3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { - RWLockWrite write_lock(generator_rid_rwlock); - - NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_parser); - ERR_FAIL_NULL(parser); - - parser->callback = p_callback; -} - -bool NavMeshGenerator3D::owns(RID p_object) { - RWLockRead read_lock(generator_rid_rwlock); - return generator_parser_owner.owns(p_object); -} - -void NavMeshGenerator3D::free(RID p_object) { - RWLockWrite write_lock(generator_rid_rwlock); - - if (generator_parser_owner.owns(p_object)) { - NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_object); - - generator_parsers.erase(parser); - - generator_parser_owner.free(p_object); - } else { - ERR_PRINT("Attempted to free a NavMeshGenerator3D RID that did not exist (or was already freed)."); - } -} - #endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_mesh_generator_3d.h b/modules/navigation/3d/nav_mesh_generator_3d.h index b46a1736e01b..65c10141431c 100644 --- a/modules/navigation/3d/nav_mesh_generator_3d.h +++ b/modules/navigation/3d/nav_mesh_generator_3d.h @@ -36,7 +36,7 @@ #include "core/object/class_db.h" #include "core/object/worker_thread_pool.h" #include "core/templates/rid_owner.h" -#include "modules/modules_enabled.gen.h" // For csg, gridmap. +#include "servers/navigation_server_3d.h" class Node; class NavigationMesh; @@ -48,12 +48,7 @@ class NavMeshGenerator3D : public Object { static Mutex baking_navmesh_mutex; static Mutex generator_task_mutex; - static RWLock generator_rid_rwlock; - struct NavMeshGeometryParser3D { - RID self; - Callable callback; - }; - static RID_Owner generator_parser_owner; + static RWLock generator_parsers_rwlock; static LocalVector generator_parsers; static bool use_threads; @@ -86,17 +81,6 @@ class NavMeshGenerator3D : public Object { static void generator_parse_source_geometry_data(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node); static void generator_bake_from_source_geometry_data(Ref p_navigation_mesh, const Ref &p_source_geometry_data); - static void generator_parse_meshinstance3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_multimeshinstance3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static void generator_parse_staticbody3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); -#ifdef MODULE_CSG_ENABLED - static void generator_parse_csgshape3d_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); -#endif // MODULE_CSG_ENABLED -#ifdef MODULE_GRIDMAP_ENABLED - static void generator_parse_gridmap_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); -#endif // MODULE_GRIDMAP_ENABLED - static void generator_parse_navigationobstacle_node(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); - static bool generator_emit_callback(const Callable &p_callback); public: @@ -106,17 +90,13 @@ class NavMeshGenerator3D : public Object { static void cleanup(); static void finish(); + static void set_generator_parsers(LocalVector p_parsers); + static void parse_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()); static void bake_from_source_geometry_data(Ref p_navigation_mesh, Ref p_source_geometry_data, const Callable &p_callback = Callable()); static void bake_from_source_geometry_data_async(Ref p_navigation_mesh, Ref p_source_geometry_data, const Callable &p_callback = Callable()); static bool is_baking(Ref p_navigation_mesh); - static RID source_geometry_parser_create(); - static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback); - - static bool owns(RID p_object); - static void free(RID p_object); - NavMeshGenerator3D(); ~NavMeshGenerator3D(); }; diff --git a/modules/navigation/register_types.cpp b/modules/navigation/register_types.cpp index dbc9e53035be..8646039705c2 100644 --- a/modules/navigation/register_types.cpp +++ b/modules/navigation/register_types.cpp @@ -66,6 +66,9 @@ void initialize_navigation_module(ModuleInitializationLevel p_level) { NavigationServer3DManager::set_default_server(new_navigation_server_3d); NavigationServer2DManager::set_default_server(new_navigation_server_2d); + NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it. + NavigationServer2DManager::initialize_server(); + #ifndef DISABLE_DEPRECATED #ifndef _3D_DISABLED _nav_mesh_generator = memnew(NavigationMeshGenerator); diff --git a/scene/2d/mesh_instance_2d.cpp b/scene/2d/mesh_instance_2d.cpp index f6a8e0de6545..53b0e4fa6e57 100644 --- a/scene/2d/mesh_instance_2d.cpp +++ b/scene/2d/mesh_instance_2d.cpp @@ -30,6 +30,17 @@ #include "mesh_instance_2d.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "scene/resources/2d/navigation_polygon.h" +#include "scene/scene_string_names.h" +#include "servers/navigation_server_2d.h" + +#include "thirdparty/clipper2/include/clipper2/clipper.h" +#include "thirdparty/misc/polypartition.h" + +Callable MeshInstance2D::_navmesh_source_geometry_parsing_callback; +RID MeshInstance2D::_navmesh_source_geometry_parser; + void MeshInstance2D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_DRAW: { @@ -106,5 +117,99 @@ bool MeshInstance2D::_edit_use_rect() const { } #endif // DEBUG_ENABLED +void MeshInstance2D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&MeshInstance2D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void MeshInstance2D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + MeshInstance2D *mesh_instance = Object::cast_to(p_node); + + if (mesh_instance == nullptr) { + return; + } + + NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + + if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { + return; + } + + Ref mesh = mesh_instance->get_mesh(); + if (!mesh.is_valid()) { + return; + } + + const Transform2D mesh_instance_xform = p_source_geometry_data->root_node_transform * mesh_instance->get_global_transform(); + + using namespace Clipper2Lib; + + PathsD subject_paths, dummy_clip_paths; + + for (int i = 0; i < mesh->get_surface_count(); i++) { + if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) { + continue; + } + + if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) { + continue; + } + + PathD subject_path; + + int index_count = 0; + if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + index_count = mesh->surface_get_array_index_len(i); + } else { + index_count = mesh->surface_get_array_len(i); + } + + ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0)); + + Array a = mesh->surface_get_arrays(i); + + Vector mesh_vertices = a[Mesh::ARRAY_VERTEX]; + + if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + Vector mesh_indices = a[Mesh::ARRAY_INDEX]; + for (int vertex_index : mesh_indices) { + const Vector2 &vertex = mesh_vertices[vertex_index]; + const PointD &point = PointD(vertex.x, vertex.y); + subject_path.push_back(point); + } + } else { + for (const Vector2 &vertex : mesh_vertices) { + const PointD &point = PointD(vertex.x, vertex.y); + subject_path.push_back(point); + } + } + subject_paths.push_back(subject_path); + } + + PathsD path_solution; + + path_solution = Union(subject_paths, dummy_clip_paths, FillRule::NonZero); + + //path_solution = RamerDouglasPeucker(path_solution, 0.025); + + Vector> polypaths; + + for (const PathD &scaled_path : path_solution) { + Vector shape_outline; + for (const PointD &scaled_point : scaled_path) { + shape_outline.push_back(Point2(static_cast(scaled_point.x), static_cast(scaled_point.y))); + } + + for (int i = 0; i < shape_outline.size(); i++) { + shape_outline.write[i] = mesh_instance_xform.xform(shape_outline[i]); + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } +} + MeshInstance2D::MeshInstance2D() { } diff --git a/scene/2d/mesh_instance_2d.h b/scene/2d/mesh_instance_2d.h index b23b2ca90724..2054d49145c5 100644 --- a/scene/2d/mesh_instance_2d.h +++ b/scene/2d/mesh_instance_2d.h @@ -33,6 +33,9 @@ #include "scene/2d/node_2d.h" +class NavigationPolygon; +class NavigationMeshSourceGeometryData2D; + class MeshInstance2D : public Node2D { GDCLASS(MeshInstance2D, Node2D); @@ -56,6 +59,14 @@ class MeshInstance2D : public Node2D { void set_texture(const Ref &p_texture); Ref get_texture() const; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + MeshInstance2D(); }; diff --git a/scene/2d/multimesh_instance_2d.cpp b/scene/2d/multimesh_instance_2d.cpp index 811dccab4aac..342a5744af5c 100644 --- a/scene/2d/multimesh_instance_2d.cpp +++ b/scene/2d/multimesh_instance_2d.cpp @@ -30,6 +30,17 @@ #include "multimesh_instance_2d.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "scene/resources/2d/navigation_polygon.h" +#include "scene/scene_string_names.h" +#include "servers/navigation_server_2d.h" + +#include "thirdparty/clipper2/include/clipper2/clipper.h" +#include "thirdparty/misc/polypartition.h" + +Callable MultiMeshInstance2D::_navmesh_source_geometry_parsing_callback; +RID MultiMeshInstance2D::_navmesh_source_geometry_parser; + void MultiMeshInstance2D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_DRAW: { @@ -95,6 +106,109 @@ Rect2 MultiMeshInstance2D::_edit_get_rect() const { } #endif // DEBUG_ENABLED +void MultiMeshInstance2D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&MultiMeshInstance2D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void MultiMeshInstance2D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + MultiMeshInstance2D *multimesh_instance = Object::cast_to(p_node); + + if (multimesh_instance == nullptr) { + return; + } + + NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { + return; + } + + Ref multimesh = multimesh_instance->get_multimesh(); + if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) { + return; + } + + Ref mesh = multimesh->get_mesh(); + if (!mesh.is_valid()) { + return; + } + + using namespace Clipper2Lib; + + PathsD mesh_subject_paths, dummy_clip_paths; + + for (int i = 0; i < mesh->get_surface_count(); i++) { + if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) { + continue; + } + + if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) { + continue; + } + + PathD subject_path; + + int index_count = 0; + if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + index_count = mesh->surface_get_array_index_len(i); + } else { + index_count = mesh->surface_get_array_len(i); + } + + ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0)); + + Array a = mesh->surface_get_arrays(i); + + Vector mesh_vertices = a[Mesh::ARRAY_VERTEX]; + + if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + Vector mesh_indices = a[Mesh::ARRAY_INDEX]; + for (int vertex_index : mesh_indices) { + const Vector2 &vertex = mesh_vertices[vertex_index]; + const PointD &point = PointD(vertex.x, vertex.y); + subject_path.push_back(point); + } + } else { + for (const Vector2 &vertex : mesh_vertices) { + const PointD &point = PointD(vertex.x, vertex.y); + subject_path.push_back(point); + } + } + mesh_subject_paths.push_back(subject_path); + } + + PathsD mesh_path_solution = Union(mesh_subject_paths, dummy_clip_paths, FillRule::NonZero); + + //path_solution = RamerDouglasPeucker(path_solution, 0.025); + + int multimesh_instance_count = multimesh->get_visible_instance_count(); + if (multimesh_instance_count == -1) { + multimesh_instance_count = multimesh->get_instance_count(); + } + + const Transform2D multimesh_instance_xform = p_source_geometry_data->root_node_transform * multimesh_instance->get_global_transform(); + + for (int i = 0; i < multimesh_instance_count; i++) { + const Transform2D multimesh_instance_mesh_instance_xform = multimesh_instance_xform * multimesh->get_instance_transform_2d(i); + + for (const PathD &mesh_path : mesh_path_solution) { + Vector shape_outline; + + for (const PointD &mesh_path_point : mesh_path) { + shape_outline.push_back(Point2(static_cast(mesh_path_point.x), static_cast(mesh_path_point.y))); + } + + for (int j = 0; j < shape_outline.size(); j++) { + shape_outline.write[j] = multimesh_instance_mesh_instance_xform.xform(shape_outline[j]); + } + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + } +} + MultiMeshInstance2D::MultiMeshInstance2D() { } diff --git a/scene/2d/multimesh_instance_2d.h b/scene/2d/multimesh_instance_2d.h index fec27a601a0e..9b39c7c2695f 100644 --- a/scene/2d/multimesh_instance_2d.h +++ b/scene/2d/multimesh_instance_2d.h @@ -34,6 +34,9 @@ #include "scene/2d/node_2d.h" #include "scene/resources/multimesh.h" +class NavigationPolygon; +class NavigationMeshSourceGeometryData2D; + class MultiMeshInstance2D : public Node2D { GDCLASS(MultiMeshInstance2D, Node2D); @@ -56,6 +59,14 @@ class MultiMeshInstance2D : public Node2D { void set_texture(const Ref &p_texture); Ref get_texture() const; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + MultiMeshInstance2D(); ~MultiMeshInstance2D(); }; diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp index 6e7009f3f735..46780728b060 100644 --- a/scene/2d/navigation_obstacle_2d.cpp +++ b/scene/2d/navigation_obstacle_2d.cpp @@ -31,9 +31,14 @@ #include "navigation_obstacle_2d.h" #include "core/math/geometry_2d.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "scene/resources/2d/navigation_polygon.h" #include "scene/resources/world_2d.h" #include "servers/navigation_server_2d.h" +Callable NavigationObstacle2D::_navmesh_source_geometry_parsing_callback; +RID NavigationObstacle2D::_navmesh_source_geometry_parser; + void NavigationObstacle2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle2D::get_rid); @@ -339,6 +344,67 @@ PackedStringArray NavigationObstacle2D::get_configuration_warnings() const { return warnings; } +void NavigationObstacle2D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&NavigationObstacle2D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void NavigationObstacle2D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + NavigationObstacle2D *obstacle = Object::cast_to(p_node); + + if (obstacle == nullptr) { + return; + } + + if (!obstacle->get_affect_navigation_mesh()) { + return; + } + + const Transform2D node_xform = p_source_geometry_data->root_node_transform * Transform2D(0.0, obstacle->get_global_position()); + + const float obstacle_radius = obstacle->get_radius(); + + if (obstacle_radius > 0.0) { + Vector obstruction_circle_vertices; + + // The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding. + // Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck. + // No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty. + static const int circle_points = 12; + + obstruction_circle_vertices.resize(circle_points); + Vector2 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw(); + const real_t circle_point_step = Math_TAU / circle_points; + + for (int i = 0; i < circle_points; i++) { + const float angle = i * circle_point_step; + circle_vertices_ptrw[i] = node_xform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius)); + } + + p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh()); + } + + const Vector &obstacle_vertices = obstacle->get_vertices(); + + if (obstacle_vertices.is_empty()) { + return; + } + + Vector obstruction_shape_vertices; + obstruction_shape_vertices.resize(obstacle_vertices.size()); + + const Vector2 *obstacle_vertices_ptr = obstacle_vertices.ptr(); + Vector2 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw(); + + for (int i = 0; i < obstacle_vertices.size(); i++) { + obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]); + } + p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_carve_navigation_mesh()); +} + void NavigationObstacle2D::_update_map(RID p_map) { map_current = p_map; NavigationServer2D::get_singleton()->obstacle_set_map(obstacle, p_map); diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h index 1acead6a64ac..80b138433597 100644 --- a/scene/2d/navigation_obstacle_2d.h +++ b/scene/2d/navigation_obstacle_2d.h @@ -33,6 +33,9 @@ #include "scene/2d/node_2d.h" +class NavigationPolygon; +class NavigationMeshSourceGeometryData2D; + class NavigationObstacle2D : public Node2D { GDCLASS(NavigationObstacle2D, Node2D); @@ -115,6 +118,14 @@ class NavigationObstacle2D : public Node2D { PackedStringArray get_configuration_warnings() const override; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + private: void _update_map(RID p_map); void _update_position(const Vector2 p_position); diff --git a/scene/2d/physics/static_body_2d.cpp b/scene/2d/physics/static_body_2d.cpp index 3c5af73c1e8e..19a50a1b175d 100644 --- a/scene/2d/physics/static_body_2d.cpp +++ b/scene/2d/physics/static_body_2d.cpp @@ -30,6 +30,18 @@ #include "static_body_2d.h" +#include "scene/resources/2d/capsule_shape_2d.h" +#include "scene/resources/2d/circle_shape_2d.h" +#include "scene/resources/2d/concave_polygon_shape_2d.h" +#include "scene/resources/2d/convex_polygon_shape_2d.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "scene/resources/2d/navigation_polygon.h" +#include "scene/resources/2d/rectangle_shape_2d.h" +#include "servers/navigation_server_2d.h" + +Callable StaticBody2D::_navmesh_source_geometry_parsing_callback; +RID StaticBody2D::_navmesh_source_geometry_parser; + void StaticBody2D::set_constant_linear_velocity(const Vector2 &p_vel) { constant_linear_velocity = p_vel; @@ -77,6 +89,130 @@ void StaticBody2D::_reload_physics_characteristics() { } } +void StaticBody2D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&StaticBody2D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void StaticBody2D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + StaticBody2D *static_body = Object::cast_to(p_node); + + if (static_body == nullptr) { + return; + } + + NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) { + return; + } + + uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask(); + if (!(static_body->get_collision_layer() & parsed_collision_mask)) { + return; + } + + List shape_owners; + static_body->get_shape_owners(&shape_owners); + + for (uint32_t shape_owner : shape_owners) { + if (static_body->is_shape_owner_disabled(shape_owner)) { + continue; + } + + const int shape_count = static_body->shape_owner_get_shape_count(shape_owner); + + for (int shape_index = 0; shape_index < shape_count; shape_index++) { + Ref s = static_body->shape_owner_get_shape(shape_owner, shape_index); + + if (s.is_null()) { + continue; + } + + const Transform2D static_body_xform = p_source_geometry_data->root_node_transform * static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner); + + RectangleShape2D *rectangle_shape = Object::cast_to(*s); + if (rectangle_shape) { + Vector shape_outline; + + const Vector2 &rectangle_size = rectangle_shape->get_size(); + + shape_outline.resize(5); + shape_outline.write[0] = static_body_xform.xform(-rectangle_size * 0.5); + shape_outline.write[1] = static_body_xform.xform(Vector2(rectangle_size.x, -rectangle_size.y) * 0.5); + shape_outline.write[2] = static_body_xform.xform(rectangle_size * 0.5); + shape_outline.write[3] = static_body_xform.xform(Vector2(-rectangle_size.x, rectangle_size.y) * 0.5); + shape_outline.write[4] = static_body_xform.xform(-rectangle_size * 0.5); + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + + CapsuleShape2D *capsule_shape = Object::cast_to(*s); + if (capsule_shape) { + const real_t capsule_height = capsule_shape->get_height(); + const real_t capsule_radius = capsule_shape->get_radius(); + + Vector shape_outline; + const real_t turn_step = Math_TAU / 12.0; + shape_outline.resize(14); + int shape_outline_inx = 0; + for (int i = 0; i < 12; i++) { + Vector2 ofs = Vector2(0, (i > 3 && i <= 9) ? -capsule_height * 0.5 + capsule_radius : capsule_height * 0.5 - capsule_radius); + + shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius + ofs); + shape_outline_inx += 1; + if (i == 3 || i == 9) { + shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius - ofs); + shape_outline_inx += 1; + } + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + + CircleShape2D *circle_shape = Object::cast_to(*s); + if (circle_shape) { + const real_t circle_radius = circle_shape->get_radius(); + + Vector shape_outline; + int circle_edge_count = 12; + shape_outline.resize(circle_edge_count); + + const real_t turn_step = Math_TAU / real_t(circle_edge_count); + for (int i = 0; i < circle_edge_count; i++) { + shape_outline.write[i] = static_body_xform.xform(Vector2(Math::cos(i * turn_step), Math::sin(i * turn_step)) * circle_radius); + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + + ConcavePolygonShape2D *concave_polygon_shape = Object::cast_to(*s); + if (concave_polygon_shape) { + Vector shape_outline = concave_polygon_shape->get_segments(); + + for (int i = 0; i < shape_outline.size(); i++) { + shape_outline.write[i] = static_body_xform.xform(shape_outline[i]); + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + + ConvexPolygonShape2D *convex_polygon_shape = Object::cast_to(*s); + if (convex_polygon_shape) { + Vector shape_outline = convex_polygon_shape->get_points(); + + for (int i = 0; i < shape_outline.size(); i++) { + shape_outline.write[i] = static_body_xform.xform(shape_outline[i]); + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } + } + } +} + void StaticBody2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody2D::set_constant_linear_velocity); ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody2D::set_constant_angular_velocity); diff --git a/scene/2d/physics/static_body_2d.h b/scene/2d/physics/static_body_2d.h index ce05596d9487..ccfc14891d4f 100644 --- a/scene/2d/physics/static_body_2d.h +++ b/scene/2d/physics/static_body_2d.h @@ -33,6 +33,9 @@ #include "scene/2d/physics/physics_body_2d.h" +class NavigationPolygon; +class NavigationMeshSourceGeometryData2D; + class StaticBody2D : public PhysicsBody2D { GDCLASS(StaticBody2D, PhysicsBody2D); @@ -57,6 +60,14 @@ class StaticBody2D : public PhysicsBody2D { StaticBody2D(PhysicsServer2D::BodyMode p_mode = PhysicsServer2D::BODY_MODE_STATIC); +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + private: void _reload_physics_characteristics(); }; diff --git a/scene/2d/polygon_2d.cpp b/scene/2d/polygon_2d.cpp index dc5a08142d19..ccdfebbff38a 100644 --- a/scene/2d/polygon_2d.cpp +++ b/scene/2d/polygon_2d.cpp @@ -31,8 +31,14 @@ #include "polygon_2d.h" #include "core/math/geometry_2d.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "scene/resources/2d/navigation_polygon.h" +#include "servers/navigation_server_2d.h" #include "skeleton_2d.h" +Callable Polygon2D::_navmesh_source_geometry_parsing_callback; +RID Polygon2D::_navmesh_source_geometry_parser; + #ifdef TOOLS_ENABLED Dictionary Polygon2D::_edit_get_state() const { Dictionary state = Node2D::_edit_get_state(); @@ -604,6 +610,35 @@ NodePath Polygon2D::get_skeleton() const { return skeleton; } +void Polygon2D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&Polygon2D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void Polygon2D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + Polygon2D *polygon_2d = Object::cast_to(p_node); + + if (polygon_2d == nullptr) { + return; + } + + NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + + if (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) { + const Transform2D polygon_2d_xform = p_source_geometry_data->root_node_transform * polygon_2d->get_global_transform(); + + Vector shape_outline = polygon_2d->get_polygon(); + for (int i = 0; i < shape_outline.size(); i++) { + shape_outline.write[i] = polygon_2d_xform.xform(shape_outline[i]); + } + + p_source_geometry_data->add_obstruction_outline(shape_outline); + } +} + void Polygon2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_polygon", "polygon"), &Polygon2D::set_polygon); ClassDB::bind_method(D_METHOD("get_polygon"), &Polygon2D::get_polygon); diff --git a/scene/2d/polygon_2d.h b/scene/2d/polygon_2d.h index 0928fa150c6e..18abf2821aae 100644 --- a/scene/2d/polygon_2d.h +++ b/scene/2d/polygon_2d.h @@ -33,6 +33,9 @@ #include "scene/2d/node_2d.h" +class NavigationPolygon; +class NavigationMeshSourceGeometryData2D; + class Polygon2D : public Node2D { GDCLASS(Polygon2D, Node2D); @@ -150,6 +153,14 @@ class Polygon2D : public Node2D { void set_skeleton(const NodePath &p_skeleton); NodePath get_skeleton() const; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + Polygon2D(); ~Polygon2D(); }; diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp index 1b385c0aa92c..069bbc09421f 100644 --- a/scene/2d/tile_map.cpp +++ b/scene/2d/tile_map.cpp @@ -32,6 +32,8 @@ #include "tile_map.compat.inc" #include "core/io/marshalls.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" +#include "servers/navigation_server_2d.h" #define TILEMAP_CALL_FOR_LAYER(layer, function, ...) \ if (layer < 0) { \ @@ -47,6 +49,9 @@ ERR_FAIL_INDEX_V(layer, (int)layers.size(), err_value); \ return layers[layer]->function(__VA_ARGS__); +Callable TileMap::_navmesh_source_geometry_parsing_callback; +RID TileMap::_navmesh_source_geometry_parser; + void TileMap::_tile_set_changed() { update_configuration_warnings(); } @@ -1022,5 +1027,32 @@ TileMap::TileMap() { property_helper.setup_for_instance(base_property_helper, this); } +void TileMap::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&TileMap::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void TileMap::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + TileMap *nb_tilemap = Object::cast_to(p_node); + + if (nb_tilemap == nullptr) { + return; + } + + // Special case for TileMap, so that internal layer get parsed even if p_recurse_children is false. + bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationPolygon::SOURCE_GEOMETRY_GROUPS_EXPLICIT; + if (!recurse_children) { + for (int i = 0; i < p_node->get_child_count(); i++) { + TileMapLayer *tile_map_layer = Object::cast_to(p_node->get_child(i)); + if (tile_map_layer && tile_map_layer->get_index_in_tile_map() >= 0) { + _navmesh_parse_source_geometry(p_navigation_mesh, p_source_geometry_data, tile_map_layer); + } + } + } +} + #undef TILEMAP_CALL_FOR_LAYER #undef TILEMAP_CALL_FOR_LAYER_V diff --git a/scene/2d/tile_map.h b/scene/2d/tile_map.h index 47f5c8d99619..cef55568cdec 100644 --- a/scene/2d/tile_map.h +++ b/scene/2d/tile_map.h @@ -36,6 +36,7 @@ #include "scene/resources/2d/tile_set.h" class Control; +class NavigationMeshSourceGeometryData2D; class TileMapLayer; class TerrainConstraint; @@ -238,6 +239,14 @@ class TileMap : public Node2D { // Configuration warnings. PackedStringArray get_configuration_warnings() const override; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + TileMap(); }; diff --git a/scene/2d/tile_map_layer.cpp b/scene/2d/tile_map_layer.cpp index 889f61b3967b..727135ce3f3c 100644 --- a/scene/2d/tile_map_layer.cpp +++ b/scene/2d/tile_map_layer.cpp @@ -33,9 +33,13 @@ #include "core/io/marshalls.h" #include "scene/2d/tile_map.h" #include "scene/gui/control.h" +#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h" #include "scene/resources/world_2d.h" #include "servers/navigation_server_2d.h" +Callable TileMapLayer::_navmesh_source_geometry_parsing_callback; +RID TileMapLayer::_navmesh_source_geometry_parser; + #ifdef DEBUG_ENABLED /////////////////////////////// Debug ////////////////////////////////////////// constexpr int TILE_MAP_DEBUG_QUADRANT_SIZE = 16; @@ -3065,6 +3069,113 @@ TileMapLayer::DebugVisibilityMode TileMapLayer::get_navigation_visibility_mode() return navigation_visibility_mode; } +void TileMapLayer::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&TileMapLayer::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create(); + NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void TileMapLayer::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + TileMapLayer *tile_map_layer = Object::cast_to(p_node); + + if (tile_map_layer == nullptr) { + return; + } + + Ref tile_set = tile_map_layer->get_tile_set(); + if (!tile_set.is_valid()) { + return; + } + + int physics_layers_count = tile_set->get_physics_layers_count(); + int navigation_layers_count = tile_set->get_navigation_layers_count(); + if (physics_layers_count <= 0 && navigation_layers_count <= 0) { + return; + } + + NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask(); + + const Transform2D tilemap_xform = p_source_geometry_data->root_node_transform * tile_map_layer->get_global_transform(); + + TypedArray used_cells = tile_map_layer->get_used_cells(); + for (int used_cell_index = 0; used_cell_index < used_cells.size(); used_cell_index++) { + const Vector2i &cell = used_cells[used_cell_index]; + + const TileData *tile_data = tile_map_layer->get_cell_tile_data(cell); + if (tile_data == nullptr) { + continue; + } + + // Transform flags. + const int alternative_id = tile_map_layer->get_cell_alternative_tile(cell); + bool flip_h = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_H); + bool flip_v = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_V); + bool transpose = (alternative_id & TileSetAtlasSource::TRANSFORM_TRANSPOSE); + + Transform2D tile_transform; + tile_transform.set_origin(tile_map_layer->map_to_local(cell)); + + const Transform2D tile_transform_offset = tilemap_xform * tile_transform; + + // Parse traversable polygons. + for (int navigation_layer = 0; navigation_layer < navigation_layers_count; navigation_layer++) { + Ref navigation_polygon = tile_data->get_navigation_polygon(navigation_layer, flip_h, flip_v, transpose); + if (navigation_polygon.is_valid()) { + for (int outline_index = 0; outline_index < navigation_polygon->get_outline_count(); outline_index++) { + const Vector &navigation_polygon_outline = navigation_polygon->get_outline(outline_index); + if (navigation_polygon_outline.is_empty()) { + continue; + } + + Vector traversable_outline; + traversable_outline.resize(navigation_polygon_outline.size()); + + const Vector2 *navigation_polygon_outline_ptr = navigation_polygon_outline.ptr(); + Vector2 *traversable_outline_ptrw = traversable_outline.ptrw(); + + for (int traversable_outline_index = 0; traversable_outline_index < traversable_outline.size(); traversable_outline_index++) { + traversable_outline_ptrw[traversable_outline_index] = tile_transform_offset.xform(navigation_polygon_outline_ptr[traversable_outline_index]); + } + + p_source_geometry_data->_add_traversable_outline(traversable_outline); + } + } + } + + // Parse obstacles. + for (int physics_layer = 0; physics_layer < physics_layers_count; physics_layer++) { + if ((parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) && + (tile_set->get_physics_layer_collision_layer(physics_layer) & parsed_collision_mask)) { + for (int collision_polygon_index = 0; collision_polygon_index < tile_data->get_collision_polygons_count(physics_layer); collision_polygon_index++) { + PackedVector2Array collision_polygon_points = tile_data->get_collision_polygon_points(physics_layer, collision_polygon_index); + if (collision_polygon_points.is_empty()) { + continue; + } + + if (flip_h || flip_v || transpose) { + collision_polygon_points = TileData::get_transformed_vertices(collision_polygon_points, flip_h, flip_v, transpose); + } + + Vector obstruction_outline; + obstruction_outline.resize(collision_polygon_points.size()); + + const Vector2 *collision_polygon_points_ptr = collision_polygon_points.ptr(); + Vector2 *obstruction_outline_ptrw = obstruction_outline.ptrw(); + + for (int obstruction_outline_index = 0; obstruction_outline_index < obstruction_outline.size(); obstruction_outline_index++) { + obstruction_outline_ptrw[obstruction_outline_index] = tile_transform_offset.xform(collision_polygon_points_ptr[obstruction_outline_index]); + } + + p_source_geometry_data->_add_obstruction_outline(obstruction_outline); + } + } + } + } +} + TileMapLayer::TileMapLayer() { set_notify_transform(true); } diff --git a/scene/2d/tile_map_layer.h b/scene/2d/tile_map_layer.h index 912fa2be155c..f65140a77546 100644 --- a/scene/2d/tile_map_layer.h +++ b/scene/2d/tile_map_layer.h @@ -33,6 +33,7 @@ #include "scene/resources/2d/tile_set.h" +class NavigationMeshSourceGeometryData2D; class TileSetAtlasSource; class TileMap; @@ -512,6 +513,14 @@ class TileMapLayer : public Node2D { void set_navigation_visibility_mode(DebugVisibilityMode p_show_navigation); DebugVisibilityMode get_navigation_visibility_mode() const; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + TileMapLayer(); ~TileMapLayer(); }; diff --git a/scene/3d/mesh_instance_3d.cpp b/scene/3d/mesh_instance_3d.cpp index 15bd8a18b9fc..3fe6af09983a 100644 --- a/scene/3d/mesh_instance_3d.cpp +++ b/scene/3d/mesh_instance_3d.cpp @@ -36,6 +36,13 @@ #include "scene/resources/3d/concave_polygon_shape_3d.h" #include "scene/resources/3d/convex_polygon_shape_3d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/navigation_mesh.h" +#include "servers/navigation_server_3d.h" + +Callable MeshInstance3D::_navmesh_source_geometry_parsing_callback; +RID MeshInstance3D::_navmesh_source_geometry_parser; + bool MeshInstance3D::_set(const StringName &p_name, const Variant &p_value) { //this is not _too_ bad performance wise, really. it only arrives here if the property was not set anywhere else. //add to it that it's probably found on first call to _set anyway. @@ -835,6 +842,31 @@ Ref MeshInstance3D::bake_mesh_from_current_skeleton_pose(Refsource_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void MeshInstance3D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + MeshInstance3D *mesh_instance = Object::cast_to(p_node); + + if (mesh_instance == nullptr) { + return; + } + + NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + + if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { + Ref mesh = mesh_instance->get_mesh(); + if (mesh.is_valid()) { + p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform()); + } + } +} + void MeshInstance3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_mesh", "mesh"), &MeshInstance3D::set_mesh); ClassDB::bind_method(D_METHOD("get_mesh"), &MeshInstance3D::get_mesh); diff --git a/scene/3d/mesh_instance_3d.h b/scene/3d/mesh_instance_3d.h index 0eff12762dcb..a5fec3d16ad4 100644 --- a/scene/3d/mesh_instance_3d.h +++ b/scene/3d/mesh_instance_3d.h @@ -33,6 +33,9 @@ #include "core/templates/local_vector.h" #include "scene/3d/visual_instance_3d.h" + +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; class Skin; class SkinReference; @@ -104,6 +107,14 @@ class MeshInstance3D : public GeometryInstance3D { Ref bake_mesh_from_current_blend_shape_mix(Ref p_existing = Ref()); Ref bake_mesh_from_current_skeleton_pose(Ref p_existing = Ref()); +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + MeshInstance3D(); ~MeshInstance3D(); }; diff --git a/scene/3d/multimesh_instance_3d.cpp b/scene/3d/multimesh_instance_3d.cpp index 2eef1dbbf4dc..efb86656bc9b 100644 --- a/scene/3d/multimesh_instance_3d.cpp +++ b/scene/3d/multimesh_instance_3d.cpp @@ -30,6 +30,13 @@ #include "multimesh_instance_3d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/navigation_mesh.h" +#include "servers/navigation_server_3d.h" + +Callable MultiMeshInstance3D::_navmesh_source_geometry_parsing_callback; +RID MultiMeshInstance3D::_navmesh_source_geometry_parser; + void MultiMeshInstance3D::_refresh_interpolated() { if (is_inside_tree() && multimesh.is_valid()) { bool interpolated = is_physics_interpolated_and_enabled(); @@ -96,6 +103,40 @@ AABB MultiMeshInstance3D::get_aabb() const { } } +void MultiMeshInstance3D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&MultiMeshInstance3D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void MultiMeshInstance3D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + MultiMeshInstance3D *multimesh_instance = Object::cast_to(p_node); + + if (multimesh_instance == nullptr) { + return; + } + + NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + + if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) { + Ref multimesh = multimesh_instance->get_multimesh(); + if (multimesh.is_valid()) { + Ref mesh = multimesh->get_mesh(); + if (mesh.is_valid()) { + int n = multimesh->get_visible_instance_count(); + if (n == -1) { + n = multimesh->get_instance_count(); + } + for (int i = 0; i < n; i++) { + p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i)); + } + } + } + } +} + MultiMeshInstance3D::MultiMeshInstance3D() { } diff --git a/scene/3d/multimesh_instance_3d.h b/scene/3d/multimesh_instance_3d.h index c9507b1047e3..8f583c4f69fd 100644 --- a/scene/3d/multimesh_instance_3d.h +++ b/scene/3d/multimesh_instance_3d.h @@ -34,6 +34,9 @@ #include "scene/3d/visual_instance_3d.h" #include "scene/resources/multimesh.h" +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; + class MultiMeshInstance3D : public GeometryInstance3D { GDCLASS(MultiMeshInstance3D, GeometryInstance3D); @@ -54,6 +57,14 @@ class MultiMeshInstance3D : public GeometryInstance3D { virtual AABB get_aabb() const override; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + MultiMeshInstance3D(); ~MultiMeshInstance3D(); }; diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp index 47e27c397724..694dcfd2cfff 100644 --- a/scene/3d/navigation_obstacle_3d.cpp +++ b/scene/3d/navigation_obstacle_3d.cpp @@ -31,8 +31,13 @@ #include "navigation_obstacle_3d.h" #include "core/math/geometry_2d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/navigation_mesh.h" #include "servers/navigation_server_3d.h" +Callable NavigationObstacle3D::_navmesh_source_geometry_parsing_callback; +RID NavigationObstacle3D::_navmesh_source_geometry_parser; + void NavigationObstacle3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle3D::get_rid); @@ -419,6 +424,68 @@ PackedStringArray NavigationObstacle3D::get_configuration_warnings() const { return warnings; } +void NavigationObstacle3D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&NavigationObstacle3D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void NavigationObstacle3D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + NavigationObstacle3D *obstacle = Object::cast_to(p_node); + + if (obstacle == nullptr) { + return; + } + + if (!obstacle->get_affect_navigation_mesh()) { + return; + } + + const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis(), obstacle->get_global_position()); + + const float obstacle_radius = obstacle->get_radius(); + + if (obstacle_radius > 0.0) { + Vector obstruction_circle_vertices; + + // The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding. + // Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck. + // No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty. + static const int circle_points = 12; + + obstruction_circle_vertices.resize(circle_points); + Vector3 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw(); + const real_t circle_point_step = Math_TAU / circle_points; + + for (int i = 0; i < circle_points; i++) { + const float angle = i * circle_point_step; + circle_vertices_ptrw[i] = node_xform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius)); + } + + p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y - obstacle_radius, obstacle_radius, obstacle->get_carve_navigation_mesh()); + } + + const Vector &obstacle_vertices = obstacle->get_vertices(); + + if (obstacle_vertices.is_empty()) { + return; + } + + Vector obstruction_shape_vertices; + obstruction_shape_vertices.resize(obstacle_vertices.size()); + + const Vector3 *obstacle_vertices_ptr = obstacle_vertices.ptr(); + Vector3 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw(); + + for (int i = 0; i < obstacle_vertices.size(); i++) { + obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]); + obstruction_shape_vertices_ptrw[i].y = 0.0; + } + p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y, obstacle->get_height(), obstacle->get_carve_navigation_mesh()); +} + void NavigationObstacle3D::_update_map(RID p_map) { NavigationServer3D::get_singleton()->obstacle_set_map(obstacle, p_map); map_current = p_map; diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h index f18f93560041..69e78d55878e 100644 --- a/scene/3d/navigation_obstacle_3d.h +++ b/scene/3d/navigation_obstacle_3d.h @@ -33,6 +33,9 @@ #include "scene/3d/node_3d.h" +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; + class NavigationObstacle3D : public Node3D { GDCLASS(NavigationObstacle3D, Node3D); @@ -123,6 +126,14 @@ class NavigationObstacle3D : public Node3D { PackedStringArray get_configuration_warnings() const override; +private: + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); + private: void _update_map(RID p_map); void _update_position(const Vector3 p_position); diff --git a/scene/3d/physics/static_body_3d.cpp b/scene/3d/physics/static_body_3d.cpp index 30e796f92e37..6540fb1c36a7 100644 --- a/scene/3d/physics/static_body_3d.cpp +++ b/scene/3d/physics/static_body_3d.cpp @@ -30,6 +30,26 @@ #include "static_body_3d.h" +#include "core/math/convex_hull.h" +#include "scene/resources/3d/box_shape_3d.h" +#include "scene/resources/3d/capsule_shape_3d.h" +#include "scene/resources/3d/concave_polygon_shape_3d.h" +#include "scene/resources/3d/convex_polygon_shape_3d.h" +#include "scene/resources/3d/cylinder_shape_3d.h" +#include "scene/resources/3d/height_map_shape_3d.h" +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/3d/primitive_meshes.h" +#include "scene/resources/3d/shape_3d.h" +#include "scene/resources/3d/sphere_shape_3d.h" +#include "scene/resources/3d/world_boundary_shape_3d.h" + +#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h" +#include "scene/resources/navigation_mesh.h" +#include "servers/navigation_server_3d.h" + +Callable StaticBody3D::_navmesh_source_geometry_parsing_callback; +RID StaticBody3D::_navmesh_source_geometry_parser; + void StaticBody3D::set_physics_material_override(const Ref &p_physics_material_override) { if (physics_material_override.is_valid()) { physics_material_override->disconnect_changed(callable_mp(this, &StaticBody3D::_reload_physics_characteristics)); @@ -77,6 +97,137 @@ void StaticBody3D::_reload_physics_characteristics() { } } +void StaticBody3D::_navmesh_parse_init() { + if (!_navmesh_source_geometry_parser.is_valid()) { + _navmesh_source_geometry_parsing_callback = callable_mp_static(&StaticBody3D::_navmesh_parse_source_geometry); + _navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create(); + NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback); + } +} + +void StaticBody3D::_navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node) { + StaticBody3D *static_body = Object::cast_to(p_node); + + if (static_body == nullptr) { + return; + } + + NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type(); + uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask(); + + if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) { + List shape_owners; + static_body->get_shape_owners(&shape_owners); + for (uint32_t shape_owner : shape_owners) { + if (static_body->is_shape_owner_disabled(shape_owner)) { + continue; + } + const int shape_count = static_body->shape_owner_get_shape_count(shape_owner); + for (int shape_index = 0; shape_index < shape_count; shape_index++) { + Ref s = static_body->shape_owner_get_shape(shape_owner, shape_index); + if (s.is_null()) { + continue; + } + + const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner); + + BoxShape3D *box = Object::cast_to(*s); + if (box) { + Array arr; + arr.resize(RS::ARRAY_MAX); + BoxMesh::create_mesh_array(arr, box->get_size()); + p_source_geometry_data->add_mesh_array(arr, transform); + } + + CapsuleShape3D *capsule = Object::cast_to(*s); + if (capsule) { + Array arr; + arr.resize(RS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height()); + p_source_geometry_data->add_mesh_array(arr, transform); + } + + CylinderShape3D *cylinder = Object::cast_to(*s); + if (cylinder) { + Array arr; + arr.resize(RS::ARRAY_MAX); + CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height()); + p_source_geometry_data->add_mesh_array(arr, transform); + } + + SphereShape3D *sphere = Object::cast_to(*s); + if (sphere) { + Array arr; + arr.resize(RS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0); + p_source_geometry_data->add_mesh_array(arr, transform); + } + + ConcavePolygonShape3D *concave_polygon = Object::cast_to(*s); + if (concave_polygon) { + p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform); + } + + ConvexPolygonShape3D *convex_polygon = Object::cast_to(*s); + if (convex_polygon) { + Vector varr = Variant(convex_polygon->get_points()); + Geometry3D::MeshData md; + + Error err = ConvexHullComputer::convex_hull(varr, md); + + if (err == OK) { + PackedVector3Array faces; + + for (const Geometry3D::MeshData::Face &face : md.faces) { + for (uint32_t k = 2; k < face.indices.size(); ++k) { + faces.push_back(md.vertices[face.indices[0]]); + faces.push_back(md.vertices[face.indices[k - 1]]); + faces.push_back(md.vertices[face.indices[k]]); + } + } + + p_source_geometry_data->add_faces(faces, transform); + } + } + + HeightMapShape3D *heightmap_shape = Object::cast_to(*s); + if (heightmap_shape) { + int heightmap_depth = heightmap_shape->get_map_depth(); + int heightmap_width = heightmap_shape->get_map_width(); + + if (heightmap_depth >= 2 && heightmap_width >= 2) { + const Vector &map_data = heightmap_shape->get_map_data(); + + Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); + Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; + + Vector vertex_array; + vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); + Vector3 *vertex_array_ptrw = vertex_array.ptrw(); + const real_t *map_data_ptr = map_data.ptr(); + int vertex_index = 0; + + for (int d = 0; d < heightmap_depth - 1; d++) { + for (int w = 0; w < heightmap_width - 1; w++) { + vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); + vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); + vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_index += 6; + } + } + if (vertex_array.size() > 0) { + p_source_geometry_data->add_faces(vertex_array, transform); + } + } + } + } + } + } +} + void StaticBody3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody3D::set_constant_linear_velocity); ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody3D::set_constant_angular_velocity); diff --git a/scene/3d/physics/static_body_3d.h b/scene/3d/physics/static_body_3d.h index 7d7f7a4acc1d..25e6cdeb8181 100644 --- a/scene/3d/physics/static_body_3d.h +++ b/scene/3d/physics/static_body_3d.h @@ -33,6 +33,9 @@ #include "scene/3d/physics/physics_body_3d.h" +class NavigationMesh; +class NavigationMeshSourceGeometryData3D; + class StaticBody3D : public PhysicsBody3D { GDCLASS(StaticBody3D, PhysicsBody3D); @@ -59,6 +62,13 @@ class StaticBody3D : public PhysicsBody3D { private: void _reload_physics_characteristics(); + + static Callable _navmesh_source_geometry_parsing_callback; + static RID _navmesh_source_geometry_parser; + +public: + static void _navmesh_parse_init(); + static void _navmesh_parse_source_geometry(const Ref &p_navigation_mesh, Ref p_source_geometry_data, Node *p_node); }; #endif // STATIC_BODY_3D_H diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index 64bc90f88434..bf76484a5f25 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -1027,6 +1027,26 @@ void register_scene_types() { GDREGISTER_CLASS(NavigationObstacle2D); GDREGISTER_CLASS(NavigationLink2D); +#ifdef MODULE_NAVIGATION_ENABLED + OS::get_singleton()->yield(); // may take time to init + + // 2D nodes that support navmesh baking need to server register their source geometry parsers. + MeshInstance2D::_navmesh_parse_init(); + MultiMeshInstance2D::_navmesh_parse_init(); + NavigationObstacle2D::_navmesh_parse_init(); + Polygon2D::_navmesh_parse_init(); + TileMap::_navmesh_parse_init(); + TileMapLayer::_navmesh_parse_init(); + StaticBody2D::_navmesh_parse_init(); +#ifndef _3D_DISABLED + // 3D nodes that support navmesh baking need to server register their source geometry parsers. + MeshInstance3D::_navmesh_parse_init(); + MultiMeshInstance3D::_navmesh_parse_init(); + NavigationObstacle3D::_navmesh_parse_init(); + StaticBody3D::_navmesh_parse_init(); +#endif +#endif + OS::get_singleton()->yield(); // may take time to init GDREGISTER_ABSTRACT_CLASS(SceneState); diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index a24a3312684c..934ebcc1fc16 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -31,10 +31,15 @@ #include "navigation_server_2d.h" #include "navigation_server_2d.compat.inc" +#include "servers/navigation_server_2d_dummy.h" #include "servers/navigation_server_3d.h" NavigationServer2D *NavigationServer2D::singleton = nullptr; +RWLock NavigationServer2D::geometry_parser_rwlock; +RID_Owner NavigationServer2D::geometry_parser_owner; +LocalVector NavigationServer2D::generator_parsers; + void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_maps"), &NavigationServer2D::get_maps); @@ -207,6 +212,47 @@ void NavigationServer2D::_emit_navigation_debug_changed_signal() { NavigationServer2D::~NavigationServer2D() { singleton = nullptr; + + RWLockWrite write_lock(geometry_parser_rwlock); + for (NavMeshGeometryParser2D *parser : generator_parsers) { + geometry_parser_owner.free(parser->self); + } + generator_parsers.clear(); +} + +RID NavigationServer2D::source_geometry_parser_create() { + RWLockWrite write_lock(geometry_parser_rwlock); + + RID rid = geometry_parser_owner.make_rid(); + + NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid); + parser->self = rid; + + generator_parsers.push_back(parser); + + return rid; +} + +void NavigationServer2D::free(RID p_object) { + if (!geometry_parser_owner.owns(p_object)) { + return; + } + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object); + ERR_FAIL_NULL(parser); + + generator_parsers.erase(parser); + geometry_parser_owner.free(parser->self); +} + +void NavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser); + ERR_FAIL_NULL(parser); + + parser->callback = p_callback; } void NavigationServer2D::_emit_map_changed(RID p_map) { @@ -417,6 +463,8 @@ bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_static( /////////////////////////////////////////////////////// +static NavigationServer2D *navigation_server_2d = nullptr; + NavigationServer2DCallback NavigationServer2DManager::create_callback = nullptr; void NavigationServer2DManager::set_default_server(NavigationServer2DCallback p_callback) { @@ -430,3 +478,26 @@ NavigationServer2D *NavigationServer2DManager::new_default_server() { return create_callback(); } + +void NavigationServer2DManager::initialize_server() { + // NavigationServer3D must be initialized before NavigationServer2D. + ERR_FAIL_NULL(NavigationServer3D::get_singleton()); + ERR_FAIL_COND(navigation_server_2d != nullptr); + + // Init 2D Navigation Server + navigation_server_2d = NavigationServer2DManager::new_default_server(); + if (!navigation_server_2d) { + WARN_PRINT("Failed to initialize NavigationServer2D. Fall back to dummy server."); + navigation_server_2d = memnew(NavigationServer2DDummy); + } + + ERR_FAIL_NULL_MSG(navigation_server_2d, "Failed to initialize NavigationServer2D."); + navigation_server_2d->init(); +} + +void NavigationServer2DManager::finalize_server() { + ERR_FAIL_NULL(navigation_server_2d); + navigation_server_2d->finish(); + memdelete(navigation_server_2d); + navigation_server_2d = nullptr; +} diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index 510cf3965255..e3ba377e4b1a 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -43,6 +43,11 @@ class NavMeshGenerator2D; #endif // CLIPPER2_ENABLED +struct NavMeshGeometryParser2D { + RID self; + Callable callback; +}; + // This server exposes the `NavigationServer3D` features in the 2D world. class NavigationServer2D : public Object { GDCLASS(NavigationServer2D, Object); @@ -310,6 +315,12 @@ class NavigationServer2D : public Object { virtual void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) = 0; virtual bool is_baking_navigation_polygon(Ref p_navigation_polygon) const = 0; +protected: + static RWLock geometry_parser_rwlock; + static RID_Owner geometry_parser_owner; + static LocalVector generator_parsers; + +public: virtual RID source_geometry_parser_create() = 0; virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) = 0; @@ -418,6 +429,9 @@ class NavigationServer2DManager { public: static void set_default_server(NavigationServer2DCallback p_callback); static NavigationServer2D *new_default_server(); + + static void initialize_server(); + static void finalize_server(); }; #endif // NAVIGATION_SERVER_2D_H diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index 05ce3d8d3564..ea383bf56e02 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -34,9 +34,14 @@ #include "core/config/project_settings.h" #include "scene/main/node.h" #include "servers/navigation/navigation_globals.h" +#include "servers/navigation_server_3d_dummy.h" NavigationServer3D *NavigationServer3D::singleton = nullptr; +RWLock NavigationServer3D::geometry_parser_rwlock; +RID_Owner NavigationServer3D::geometry_parser_owner; +LocalVector NavigationServer3D::generator_parsers; + void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_maps"), &NavigationServer3D::get_maps); @@ -302,6 +307,47 @@ NavigationServer3D::NavigationServer3D() { NavigationServer3D::~NavigationServer3D() { singleton = nullptr; + + RWLockWrite write_lock(geometry_parser_rwlock); + for (NavMeshGeometryParser3D *parser : generator_parsers) { + geometry_parser_owner.free(parser->self); + } + generator_parsers.clear(); +} + +RID NavigationServer3D::source_geometry_parser_create() { + RWLockWrite write_lock(geometry_parser_rwlock); + + RID rid = geometry_parser_owner.make_rid(); + + NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(rid); + parser->self = rid; + + generator_parsers.push_back(parser); + + return rid; +} + +void NavigationServer3D::free(RID p_object) { + if (!geometry_parser_owner.owns(p_object)) { + return; + } + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_object); + ERR_FAIL_NULL(parser); + + generator_parsers.erase(parser); + geometry_parser_owner.free(parser->self); +} + +void NavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { + RWLockWrite write_lock(geometry_parser_rwlock); + + NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_parser); + ERR_FAIL_NULL(parser); + + parser->callback = p_callback; } void NavigationServer3D::set_debug_enabled(bool p_enabled) { @@ -944,6 +990,8 @@ bool NavigationServer3D::get_debug_avoidance_enabled() const { /////////////////////////////////////////////////////// +static NavigationServer3D *navigation_server_3d = nullptr; + NavigationServer3DCallback NavigationServer3DManager::create_callback = nullptr; void NavigationServer3DManager::set_default_server(NavigationServer3DCallback p_callback) { @@ -957,3 +1005,27 @@ NavigationServer3D *NavigationServer3DManager::new_default_server() { return create_callback(); } + +void NavigationServer3DManager::initialize_server() { + ERR_FAIL_COND(navigation_server_3d != nullptr); + + // Init 3D Navigation Server + navigation_server_3d = NavigationServer3DManager::new_default_server(); + + // Fall back to dummy if no default server has been registered. + if (!navigation_server_3d) { + WARN_PRINT("Failed to initialize NavigationServer3D. Fall back to dummy server."); + navigation_server_3d = memnew(NavigationServer3DDummy); + } + + // Should be impossible, but make sure it's not null. + ERR_FAIL_NULL_MSG(navigation_server_3d, "Failed to initialize NavigationServer3D."); + navigation_server_3d->init(); +} + +void NavigationServer3DManager::finalize_server() { + ERR_FAIL_NULL(navigation_server_3d); + navigation_server_3d->finish(); + memdelete(navigation_server_3d); + navigation_server_3d = nullptr; +} diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index 5a9896a86a3b..fa4d085bc8b0 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -39,6 +39,11 @@ #include "servers/navigation/navigation_path_query_parameters_3d.h" #include "servers/navigation/navigation_path_query_result_3d.h" +struct NavMeshGeometryParser3D { + RID self; + Callable callback; +}; + /// This server uses the concept of internal mutability. /// All the constant functions can be called in multithread because internally /// the server takes care to schedule the functions access. @@ -355,6 +360,12 @@ class NavigationServer3D : public Object { virtual bool is_baking_navigation_mesh(Ref p_navigation_mesh) const = 0; #endif // _3D_DISABLED +protected: + static RWLock geometry_parser_rwlock; + static RID_Owner geometry_parser_owner; + static LocalVector generator_parsers; + +public: virtual RID source_geometry_parser_create() = 0; virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) = 0; @@ -571,6 +582,9 @@ class NavigationServer3DManager { public: static void set_default_server(NavigationServer3DCallback p_callback); static NavigationServer3D *new_default_server(); + + static void initialize_server(); + static void finalize_server(); }; VARIANT_ENUM_CAST(NavigationServer3D::ProcessInfo);