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;
@@ -375,44 +373,6 @@ void finalize_display() {
 	memdelete(display_server);
 }
 
-void initialize_navigation_server() {
-	ERR_FAIL_COND(navigation_server_3d != nullptr);
-	ERR_FAIL_COND(navigation_server_2d != nullptr);
-
-	// Init 3D Navigation Server
-	navigation_server_3d = NavigationServer3DManager::new_default_server();
-
-	// Fall back to dummy if no default server has been registered.
-	if (!navigation_server_3d) {
-		navigation_server_3d = memnew(NavigationServer3DDummy);
-	}
-
-	// Should be impossible, but make sure it's not null.
-	ERR_FAIL_NULL_MSG(navigation_server_3d, "Failed to initialize NavigationServer3D.");
-	navigation_server_3d->init();
-
-	// Init 2D Navigation Server
-	navigation_server_2d = NavigationServer2DManager::new_default_server();
-	if (!navigation_server_2d) {
-		navigation_server_2d = memnew(NavigationServer2DDummy);
-	}
-
-	ERR_FAIL_NULL_MSG(navigation_server_2d, "Failed to initialize NavigationServer2D.");
-	navigation_server_2d->init();
-}
-
-void finalize_navigation_server() {
-	ERR_FAIL_NULL(navigation_server_3d);
-	navigation_server_3d->finish();
-	memdelete(navigation_server_3d);
-	navigation_server_3d = nullptr;
-
-	ERR_FAIL_NULL(navigation_server_2d);
-	navigation_server_2d->finish();
-	memdelete(navigation_server_2d);
-	navigation_server_2d = nullptr;
-}
-
 void initialize_theme_db() {
 	theme_db = memnew(ThemeDB);
 }
@@ -790,8 +750,6 @@ Error Main::test_setup() {
 	// Theme needs modules to be initialized so that sub-resources can be loaded.
 	theme_db->initialize_theme_noproject();
 
-	initialize_navigation_server();
-
 	ERR_FAIL_COND_V(TextServerManager::get_singleton()->get_interface_count() == 0, ERR_CANT_CREATE);
 
 	/* Use one with the most features available. */
@@ -852,7 +810,8 @@ void Main::test_cleanup() {
 
 	finalize_theme_db();
 
-	finalize_navigation_server();
+	NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+	NavigationServer3DManager::finalize_server();
 
 	GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);
 	uninitialize_modules(MODULE_INITIALIZATION_LEVEL_SERVERS);
@@ -3441,8 +3400,6 @@ Error Main::setup2(bool p_show_boot_logo) {
 
 	MAIN_PRINT("Main: Load Navigation");
 
-	initialize_navigation_server();
-
 	register_server_singletons();
 
 	// This loads global classes, so it must happen before custom loaders and savers are registered
@@ -4677,7 +4634,8 @@ void Main::cleanup(bool p_force) {
 	finalize_theme_db();
 
 	// Before deinitializing server extensions, finalize servers which may be loaded as extensions.
-	finalize_navigation_server();
+	NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+	NavigationServer3DManager::finalize_server();
 	finalize_physics();
 
 	GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);

modules/csg/csg_shape.cpp

@@ -34,9 +34,44 @@
 #include "core/io/json.h"
 #endif // DEV_ENABLED
 #include "core/math/geometry_2d.h"
+#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
+#include "scene/resources/navigation_mesh.h"
+#include "servers/navigation_server_3d.h"
 
 #include <manifold/manifold.h>
 
+Callable CSGShape3D::_navmesh_source_geometry_parsing_callback;
+RID CSGShape3D::_navmesh_source_geometry_parser;
+
+void CSGShape3D::_navmesh_parse_init() {
+	if (!_navmesh_source_geometry_parser.is_valid()) {
+		_navmesh_source_geometry_parsing_callback = callable_mp_static(&CSGShape3D::_navmesh_parse_source_geometry);
+		_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
+		NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
+	}
+}
+
+void CSGShape3D::_navmesh_parse_source_geometry(const Ref<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);
 
@@ -169,6 +172,14 @@ class CSGShape3D : public GeometryInstance3D {
 	Ref<ArrayMesh> bake_static_mesh();
 	Ref<ConcavePolygonShape3D> bake_collision_shape();
 
+private:
+	static Callable _navmesh_source_geometry_parsing_callback;
+	static RID _navmesh_source_geometry_parser;
+
+public:
+	static void _navmesh_parse_init();
+	static void _navmesh_parse_source_geometry(const Ref<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,142 @@ GridMap::GridMap() {
 #endif // DEBUG_ENABLED
 }
 
+void GridMap::_navmesh_parse_init() {
+	if (!_navmesh_source_geometry_parser.is_valid()) {
+		_navmesh_source_geometry_parsing_callback = callable_mp_static(&GridMap::_navmesh_parse_source_geometry);
+		_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
+		NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
+	}
+}
+
+void GridMap::_navmesh_parse_source_geometry(const Ref<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 @@ class GridMap : public Node3D {
 	Array get_bake_meshes();
 	RID get_bake_mesh_instance(int p_idx);
 
+private:
+	static Callable _navmesh_source_geometry_parsing_callback;
+	static RID _navmesh_source_geometry_parser;
+
+public:
+	static void _navmesh_parse_init();
+	static void _navmesh_parse_source_geometry(const Ref<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) {