Make nodes handle their respective navigation source geometry

Makes nodes handle their respective navigation source geometry.

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;
@@ -376,44 +374,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);
 }
@@ -770,6 +730,9 @@ Error Main::test_setup() {
 	// Default theme will be initialized later, after modules and ScriptServer are ready.
 	initialize_theme_db();
 
+	NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+	NavigationServer2DManager::initialize_server();
+
 	register_scene_types();
 	register_driver_types();
 
@@ -792,8 +755,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. */
@@ -854,7 +815,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);
@@ -3389,6 +3351,11 @@ Error Main::setup2(bool p_show_boot_logo) {
 	// Default theme will be initialized later, after modules and ScriptServer are ready.
 	initialize_theme_db();
 
+	MAIN_PRINT("Main: Load Navigation");
+
+	NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+	NavigationServer2DManager::initialize_server();
+
 	register_scene_types();
 	register_driver_types();
 
@@ -3459,10 +3426,6 @@ Error Main::setup2(bool p_show_boot_logo) {
 
 	initialize_physics();
 
-	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
@@ -4703,7 +4666,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);

modules/csg/csg_shape.cpp

@@ -34,9 +34,45 @@
 #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 <manifold/manifold.h>
 
+Callable CSGShape3D::_navmesh_source_geometry_parsing_callback;
+RID CSGShape3D::_navmesh_source_geometry_parser;
+
+void CSGShape3D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(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> 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;

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);
 
@@ -171,6 +174,14 @@ public:
 
 	virtual Ref<TriangleMesh> generate_triangle_mesh() 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
+
 	CSGShape3D();
 	~CSGShape3D();
 };

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) {

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,143 @@ GridMap::GridMap() {
 #endif // DEBUG_ENABLED
 }
 
+void GridMap::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	GridMap *gridmap = Object::cast_to<GridMap>(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> 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<real_t> &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<Vector3> 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) {

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 @@ public:
 	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
+
 	GridMap();
 	~GridMap();
 };

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) {

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 Ref<NavigationPathQueryParameters
 }
 
 RID GodotNavigationServer2D::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);
 #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;
 }

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<Ref<NavigationPolygon>> NavMeshGenerator2D::baking_navmeshes;
 HashMap<WorkerThreadPool::TaskID, NavMeshGenerator2D::NavMeshGeneratorTask2D *> NavMeshGenerator2D::generator_tasks;
-RID_Owner<NavMeshGenerator2D::NavMeshGeometryParser2D> NavMeshGenerator2D::generator_parser_owner;
-LocalVector<NavMeshGenerator2D::NavMeshGeometryParser2D *> NavMeshGenerator2D::generator_parsers;
+LocalVector<NavMeshGeometryParser2D *> 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<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> 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<TileMap>(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<TileMapLayer>(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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	MeshInstance2D *mesh_instance = Object::cast_to<MeshInstance2D>(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 = mesh_instance->get_mesh();
-	if (mesh.is_null()) {
-		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<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
-
-		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
-			Vector<int> 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<Vector<Vector2>> polypaths;
-
-	for (const PathD &scaled_path : path_solution) {
-		Vector<Vector2> shape_outline;
-		for (const PointD &scaled_point : scaled_path) {
-			shape_outline.push_back(Point2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	MultiMeshInstance2D *multimesh_instance = Object::cast_to<MultiMeshInstance2D>(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 = multimesh_instance->get_multimesh();
-	if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) {
-		return;
-	}
-
-	Ref<Mesh> mesh = multimesh->get_mesh();
-	if (mesh.is_null()) {
-		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<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
-
-		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
-			Vector<int> 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<Vector2> shape_outline;
-
-			for (const PointD &mesh_path_point : mesh_path) {
-				shape_outline.push_back(Point2(static_cast<real_t>(mesh_path_point.x), static_cast<real_t>(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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	Polygon2D *polygon_2d = Object::cast_to<Polygon2D>(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<Vector2> 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	StaticBody2D *static_body = Object::cast_to<StaticBody2D>(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<uint32_t> 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<Shape2D> 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<RectangleShape2D>(*s);
-			if (rectangle_shape) {
-				Vector<Vector2> 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<CapsuleShape2D>(*s);
-			if (capsule_shape) {
-				const real_t capsule_height = capsule_shape->get_height();
-				const real_t capsule_radius = capsule_shape->get_radius();
-
-				Vector<Vector2> 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<CircleShape2D>(*s);
-			if (circle_shape) {
-				const real_t circle_radius = circle_shape->get_radius();
-
-				Vector<Vector2> 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<ConcavePolygonShape2D>(*s);
-			if (concave_polygon_shape) {
-				Vector<Vector2> 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<ConvexPolygonShape2D>(*s);
-			if (convex_polygon_shape) {
-				Vector<Vector2> 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node);
-	if (tile_map_layer == nullptr) {
-		return;
-	}
-
-	Ref<TileSet> tile_set = tile_map_layer->get_tile_set();
-	if (tile_set.is_null()) {
-		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<Vector2i> 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<NavigationPolygon> 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<Vector2> &navigation_polygon_outline = navigation_polygon->get_outline(outline_index);
-					if (navigation_polygon_outline.is_empty()) {
-						continue;
-					}
-
-					Vector<Vector2> 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<Vector2> 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
-	NavigationObstacle2D *obstacle = Object::cast_to<NavigationObstacle2D>(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<Vector2> 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<Vector2> &obstacle_vertices = obstacle->get_vertices();
-
-	if (obstacle_vertices.is_empty()) {
-		return;
-	}
-
-	Vector<Vector2> 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<NavMeshGeometryParser2D *> p_parsers) {
+	RWLockWrite write_lock(generator_parsers_rwlock);
+	generator_parsers = p_parsers;
 }
 
 void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> 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<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data) {
 	if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
 		return;

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<NavMeshGeometryParser2D> generator_parser_owner;
+	static RWLock generator_parsers_rwlock;
 	static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
 
 	static bool use_threads;
@@ -85,13 +81,6 @@ class NavMeshGenerator2D : public Object {
 	static void generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node);
 	static void generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data);
 
-	static void generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_tile_map_layer_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_navigationobstacle_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
-
 	static bool generator_emit_callback(const Callable &p_callback);
 
 public:
@@ -101,17 +90,13 @@ public:
 	static void cleanup();
 	static void finish();
 
+	static void set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers);
+
 	static void parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
 	static void bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
 	static void bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
 	static bool is_baking(Ref<NavigationPolygon> 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();
 };

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 Ref<NavigationPathQueryParameters
 }
 
 RID GodotNavigationServer3D::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);
 #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<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) {

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 <Recast.h>
 
 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<Ref<NavigationMesh>> NavMeshGenerator3D::baking_navmeshes;
 HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
-RID_Owner<NavMeshGenerator3D::NavMeshGeometryParser3D> NavMeshGenerator3D::generator_parser_owner;
-LocalVector<NavMeshGenerator3D::NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
+LocalVector<NavMeshGeometryParser3D *> 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> 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<NavigationMesh>
 	}
 }
 
-void NavMeshGenerator3D::generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(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 = 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(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 = multimesh_instance->get_multimesh();
-			if (multimesh.is_valid()) {
-				Ref<Mesh> 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	StaticBody3D *static_body = Object::cast_to<StaticBody3D>(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<uint32_t> 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<Shape3D> 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<BoxShape3D>(*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<CapsuleShape3D>(*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<CylinderShape3D>(*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<SphereShape3D>(*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<ConcavePolygonShape3D>(*s);
-					if (concave_polygon) {
-						p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
-					}
-
-					ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
-					if (convex_polygon) {
-						Vector<Vector3> 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<HeightMapShape3D>(*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<real_t> &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<Vector3> 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(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<CSGShape3D>(p_node);
-			Array meshes = csg_shape->get_meshes();
-			if (!meshes.is_empty()) {
-				Ref<Mesh> 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	GridMap *gridmap = Object::cast_to<GridMap>(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> 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<real_t> &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<Vector3> 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
-	NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(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<Vector3> 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<Vector3> &obstacle_vertices = obstacle->get_vertices();
-
-	if (obstacle_vertices.is_empty()) {
-		return;
-	}
-
-	Vector<Vector3> 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<NavMeshGeometryParser3D *> p_parsers) {
+	RWLockWrite write_lock(generator_parsers_rwlock);
+	generator_parsers = p_parsers;
 }
 
 void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> 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

modules/navigation/3d/nav_mesh_generator_3d.h

@@ -36,8 +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;
@@ -49,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<NavMeshGeometryParser3D> generator_parser_owner;
+	static RWLock generator_parsers_rwlock;
 	static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
 
 	static bool use_threads;
@@ -87,17 +81,6 @@ class NavMeshGenerator3D : public Object {
 	static void generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node);
 	static void generator_bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data);
 
-	static void generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-	static void generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-#ifdef MODULE_CSG_ENABLED
-	static void generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-#endif // MODULE_CSG_ENABLED
-#ifdef MODULE_GRIDMAP_ENABLED
-	static void generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-#endif // MODULE_GRIDMAP_ENABLED
-	static void generator_parse_navigationobstacle_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
-
 	static bool generator_emit_callback(const Callable &p_callback);
 
 public:
@@ -107,17 +90,13 @@ public:
 	static void cleanup();
 	static void finish();
 
+	static void set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers);
+
 	static void parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
 	static void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
 	static void bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
 	static bool is_baking(Ref<NavigationMesh> p_navigation_mesh);
 
-	static RID source_geometry_parser_create();
-	static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback);
-
-	static bool owns(RID p_object);
-	static void free(RID p_object);
-
 	NavMeshGenerator3D();
 	~NavMeshGenerator3D();
 };

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,100 @@ bool MeshInstance2D::_edit_use_rect() const {
 }
 #endif // DEBUG_ENABLED
 
+void MeshInstance2D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	MeshInstance2D *mesh_instance = Object::cast_to<MeshInstance2D>(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 = mesh_instance->get_mesh();
+	if (mesh.is_null()) {
+		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<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
+
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			Vector<int> 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<Vector<Vector2>> polypaths;
+
+	for (const PathD &scaled_path : path_solution) {
+		Vector<Vector2> shape_outline;
+		for (const PointD &scaled_point : scaled_path) {
+			shape_outline.push_back(Point2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(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() {
 }

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 @@ public:
 	void set_texture(const Ref<Texture2D> &p_texture);
 	Ref<Texture2D> 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 	MeshInstance2D();
 };
 

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,110 @@ Rect2 MultiMeshInstance2D::_edit_get_rect() const {
 }
 #endif // DEBUG_ENABLED
 
+void MultiMeshInstance2D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	MultiMeshInstance2D *multimesh_instance = Object::cast_to<MultiMeshInstance2D>(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 = multimesh_instance->get_multimesh();
+	if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) {
+		return;
+	}
+
+	Ref<Mesh> mesh = multimesh->get_mesh();
+	if (mesh.is_null()) {
+		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<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
+
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			Vector<int> 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<Vector2> shape_outline;
+
+			for (const PointD &mesh_path_point : mesh_path) {
+				shape_outline.push_back(Point2(static_cast<real_t>(mesh_path_point.x), static_cast<real_t>(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() {
 }
 

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 @@ public:
 	void set_texture(const Ref<Texture2D> &p_texture);
 	Ref<Texture2D> 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 	MultiMeshInstance2D();
 	~MultiMeshInstance2D();
 };

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,75 @@ PackedStringArray NavigationObstacle2D::get_configuration_warnings() const {
 	return warnings;
 }
 
+void NavigationObstacle2D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	NavigationObstacle2D *obstacle = Object::cast_to<NavigationObstacle2D>(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<Vector2> 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<Vector2> &obstacle_vertices = obstacle->get_vertices();
+
+	if (obstacle_vertices.is_empty()) {
+		return;
+	}
+
+	Vector<Vector2> 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);

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 @@ public:
 
 	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 private:
 	void _update_map(RID p_map);
 	void _update_position(const Vector2 p_position);

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,131 @@ void StaticBody2D::_reload_physics_characteristics() {
 	}
 }
 
+void StaticBody2D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	StaticBody2D *static_body = Object::cast_to<StaticBody2D>(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<uint32_t> 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<Shape2D> 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<RectangleShape2D>(*s);
+			if (rectangle_shape) {
+				Vector<Vector2> 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<CapsuleShape2D>(*s);
+			if (capsule_shape) {
+				const real_t capsule_height = capsule_shape->get_height();
+				const real_t capsule_radius = capsule_shape->get_radius();
+
+				Vector<Vector2> 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<CircleShape2D>(*s);
+			if (circle_shape) {
+				const real_t circle_radius = circle_shape->get_radius();
+
+				Vector<Vector2> 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<ConcavePolygonShape2D>(*s);
+			if (concave_polygon_shape) {
+				Vector<Vector2> 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<ConvexPolygonShape2D>(*s);
+			if (convex_polygon_shape) {
+				Vector<Vector2> 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);

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 @@ public:
 
 	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 private:
 	void _reload_physics_characteristics();
 };

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,36 @@ NodePath Polygon2D::get_skeleton() const {
 	return skeleton;
 }
 
+void Polygon2D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	Polygon2D *polygon_2d = Object::cast_to<Polygon2D>(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<Vector2> 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);

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 @@ public:
 	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 	Polygon2D();
 	~Polygon2D();
 };

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,33 @@ TileMap::TileMap() {
 	property_helper.setup_for_instance(base_property_helper, this);
 }
 
+void TileMap::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	TileMap *nb_tilemap = Object::cast_to<TileMap>(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<TileMapLayer>(p_node->get_child(i));
+			if (tile_map_layer && tile_map_layer->get_index_in_tile_map() >= 0) {
+				tile_map_layer->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

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 @@ public:
 	// 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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 	TileMap();
 };
 

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,114 @@ TileMapLayer::DebugVisibilityMode TileMapLayer::get_navigation_visibility_mode()
 	return navigation_visibility_mode;
 }
 
+void TileMapLayer::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer2D::get_singleton());
+	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node);
+
+	if (tile_map_layer == nullptr) {
+		return;
+	}
+
+	Ref<TileSet> tile_set = tile_map_layer->get_tile_set();
+	if (tile_set.is_null()) {
+		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<Vector2i> 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<NavigationPolygon> 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<Vector2> &navigation_polygon_outline = navigation_polygon->get_outline(outline_index);
+					if (navigation_polygon_outline.is_empty()) {
+						continue;
+					}
+
+					Vector<Vector2> 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<Vector2> 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);
 }

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 @@ public:
 	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<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
 	TileMapLayer();
 	~TileMapLayer();
 };

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.
@@ -842,6 +849,32 @@ Ref<TriangleMesh> MeshInstance3D::generate_triangle_mesh() const {
 	return Ref<TriangleMesh>();
 }
 
+void MeshInstance3D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	if (!_navmesh_source_geometry_parser.is_valid()) {
+		_navmesh_source_geometry_parsing_callback = callable_mp_static(&MeshInstance3D::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 MeshInstance3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(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 = 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);

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;
 
@@ -106,6 +109,14 @@ public:
 
 	virtual Ref<TriangleMesh> generate_triangle_mesh() 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
+
 	MeshInstance3D();
 	~MeshInstance3D();
 };

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,41 @@ AABB MultiMeshInstance3D::get_aabb() const {
 	}
 }
 
+void MultiMeshInstance3D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(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 = multimesh_instance->get_multimesh();
+		if (multimesh.is_valid()) {
+			Ref<Mesh> 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() {
 }
 

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 @@ public:
 
 	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
+
 	MultiMeshInstance3D();
 	~MultiMeshInstance3D();
 };

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,78 @@ PackedStringArray NavigationObstacle3D::get_configuration_warnings() const {
 	return warnings;
 }
 
+void NavigationObstacle3D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(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<Vector3> 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<Vector3> &obstacle_vertices = obstacle->get_vertices();
+
+	if (obstacle_vertices.is_empty()) {
+		return;
+	}
+
+	Vector<Vector3> 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 NavigationObstacle3D::_update_map(RID p_map) {
 	NavigationServer3D::get_singleton()->obstacle_set_map(obstacle, p_map);
 	map_current = p_map;

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 @@ public:
 
 	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
+
 private:
 	void _update_map(RID p_map);
 	void _update_position(const Vector3 p_position);

scene/3d/physics/static_body_3d.cpp

@@ -30,6 +30,24 @@
 
 #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/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<PhysicsMaterial> &p_physics_material_override) {
 	if (physics_material_override.is_valid()) {
 		physics_material_override->disconnect_changed(callable_mp(this, &StaticBody3D::_reload_physics_characteristics));
@@ -77,6 +95,138 @@ void StaticBody3D::_reload_physics_characteristics() {
 	}
 }
 
+void StaticBody3D::navmesh_parse_init() {
+	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
+	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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
+	StaticBody3D *static_body = Object::cast_to<StaticBody3D>(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<uint32_t> 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<Shape3D> 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<BoxShape3D>(*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<CapsuleShape3D>(*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<CylinderShape3D>(*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<SphereShape3D>(*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<ConcavePolygonShape3D>(*s);
+				if (concave_polygon) {
+					p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
+				}
+
+				ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
+				if (convex_polygon) {
+					Vector<Vector3> 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<HeightMapShape3D>(*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<real_t> &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<Vector3> 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);

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 @@ public:
 
 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<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
 };
 
 #endif // STATIC_BODY_3D_H

scene/register_scene_types.cpp

@@ -1030,6 +1030,24 @@ void register_scene_types() {
 
 	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
+
+	OS::get_singleton()->yield(); // may take time to init
+
 	GDREGISTER_ABSTRACT_CLASS(SceneState);
 	GDREGISTER_CLASS(PackedScene);
 

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<NavMeshGeometryParser2D> NavigationServer2D::geometry_parser_owner;
+LocalVector<NavMeshGeometryParser2D *> 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_VERBOSE("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;
+}

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 @@ public:
 	virtual void bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) = 0;
 	virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const = 0;
 
+protected:
+	static RWLock geometry_parser_rwlock;
+	static RID_Owner<NavMeshGeometryParser2D> geometry_parser_owner;
+	static LocalVector<NavMeshGeometryParser2D *> 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

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<NavMeshGeometryParser3D> NavigationServer3D::geometry_parser_owner;
+LocalVector<NavMeshGeometryParser3D *> 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_VERBOSE("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;
+}

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 @@ public:
 	virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const = 0;
 #endif // _3D_DISABLED
 
+protected:
+	static RWLock geometry_parser_rwlock;
+	static RID_Owner<NavMeshGeometryParser3D> geometry_parser_owner;
+	static LocalVector<NavMeshGeometryParser3D *> 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);