Extend `NavigationMeshSourceGeometryData[23]D` to allow data merging

7 files changed, 83 insertions, 2 deletions

diff --git a/doc/classes/NavigationMeshSourceGeometryData2D.xml b/doc/classes/NavigationMeshSourceGeometryData2D.xml
index 3f6fcc733a..9c05248eff 100644
--- a/doc/classes/NavigationMeshSourceGeometryData2D.xml
+++ b/doc/classes/NavigationMeshSourceGeometryData2D.xml
@@ -47,6 +47,13 @@
 				Returns [code]true[/code] when parsed source geometry data exists.
 			</description>
 		</method>
+		<method name="merge">
+			<return type="void" />
+			<param index="0" name="other_geometry" type="NavigationMeshSourceGeometryData2D" />
+			<description>
+				Adds the geometry data of another [NavigationMeshSourceGeometryData2D] to the navigation mesh baking data.
+			</description>
+		</method>
 		<method name="set_obstruction_outlines">
 			<return type="void" />
 			<param index="0" name="obstruction_outlines" type="PackedVector2Array[]" />
diff --git a/doc/classes/NavigationMeshSourceGeometryData3D.xml b/doc/classes/NavigationMeshSourceGeometryData3D.xml
index ffa8163eaa..a3dcd4d209 100644
--- a/doc/classes/NavigationMeshSourceGeometryData3D.xml
+++ b/doc/classes/NavigationMeshSourceGeometryData3D.xml
@@ -57,6 +57,13 @@
 				Returns [code]true[/code] when parsed source geometry data exists.
 			</description>
 		</method>
+		<method name="merge">
+			<return type="void" />
+			<param index="0" name="other_geometry" type="NavigationMeshSourceGeometryData3D" />
+			<description>
+				Adds the geometry data of another [NavigationMeshSourceGeometryData3D] to the navigation mesh baking data.
+			</description>
+		</method>
 		<method name="set_indices">
 			<return type="void" />
 			<param index="0" name="indices" type="PackedInt32Array" />
diff --git a/scene/resources/navigation_mesh_source_geometry_data_2d.cpp b/scene/resources/navigation_mesh_source_geometry_data_2d.cpp
index fabe1659c6..7c33aa9e38 100644
--- a/scene/resources/navigation_mesh_source_geometry_data_2d.cpp
+++ b/scene/resources/navigation_mesh_source_geometry_data_2d.cpp
@@ -113,6 +113,12 @@ void NavigationMeshSourceGeometryData2D::add_obstruction_outline(const PackedVec
 	}
 }
 
+void NavigationMeshSourceGeometryData2D::merge(const Ref<NavigationMeshSourceGeometryData2D> &p_other_geometry) {
+	// No need to worry about `root_node_transform` here as the data is already xformed.
+	traversable_outlines.append_array(p_other_geometry->traversable_outlines);
+	obstruction_outlines.append_array(p_other_geometry->obstruction_outlines);
+}
+
 void NavigationMeshSourceGeometryData2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("clear"), &NavigationMeshSourceGeometryData2D::clear);
 	ClassDB::bind_method(D_METHOD("has_data"), &NavigationMeshSourceGeometryData2D::has_data);
@@ -126,6 +132,8 @@ void NavigationMeshSourceGeometryData2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("add_traversable_outline", "shape_outline"), &NavigationMeshSourceGeometryData2D::add_traversable_outline);
 	ClassDB::bind_method(D_METHOD("add_obstruction_outline", "shape_outline"), &NavigationMeshSourceGeometryData2D::add_obstruction_outline);
 
+	ClassDB::bind_method(D_METHOD("merge", "other_geometry"), &NavigationMeshSourceGeometryData2D::merge);
+
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "traversable_outlines", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_traversable_outlines", "get_traversable_outlines");
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "obstruction_outlines", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_obstruction_outlines", "get_obstruction_outlines");
 }
diff --git a/scene/resources/navigation_mesh_source_geometry_data_2d.h b/scene/resources/navigation_mesh_source_geometry_data_2d.h
index 985f90fb9e..4accdbc1f4 100644
--- a/scene/resources/navigation_mesh_source_geometry_data_2d.h
+++ b/scene/resources/navigation_mesh_source_geometry_data_2d.h
@@ -71,6 +71,8 @@ public:
 	bool has_data() { return traversable_outlines.size(); };
 	void clear();
 
+	void merge(const Ref<NavigationMeshSourceGeometryData2D> &p_other_geometry);
+
 	NavigationMeshSourceGeometryData2D() {}
 	~NavigationMeshSourceGeometryData2D() { clear(); }
 };
diff --git a/scene/resources/navigation_mesh_source_geometry_data_3d.cpp b/scene/resources/navigation_mesh_source_geometry_data_3d.cpp
index e39ffab47a..43fb592bba 100644
--- a/scene/resources/navigation_mesh_source_geometry_data_3d.cpp
+++ b/scene/resources/navigation_mesh_source_geometry_data_3d.cpp
@@ -165,6 +165,17 @@ void NavigationMeshSourceGeometryData3D::add_faces(const PackedVector3Array &p_f
 	_add_faces(p_faces, root_node_transform * p_xform);
 }
 
+void NavigationMeshSourceGeometryData3D::merge(const Ref<NavigationMeshSourceGeometryData3D> &p_other_geometry) {
+	// No need to worry about `root_node_transform` here as the vertices are already xformed.
+	const int64_t number_of_vertices_before_merge = vertices.size();
+	const int64_t number_of_indices_before_merge = indices.size();
+	vertices.append_array(p_other_geometry->vertices);
+	indices.append_array(p_other_geometry->indices);
+	for (int64_t i = number_of_indices_before_merge; i < indices.size(); i++) {
+		indices.set(i, indices[i] + number_of_vertices_before_merge / 3);
+	}
+}
+
 void NavigationMeshSourceGeometryData3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationMeshSourceGeometryData3D::set_vertices);
 	ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationMeshSourceGeometryData3D::get_vertices);
@@ -178,6 +189,7 @@ void NavigationMeshSourceGeometryData3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("add_mesh", "mesh", "xform"), &NavigationMeshSourceGeometryData3D::add_mesh);
 	ClassDB::bind_method(D_METHOD("add_mesh_array", "mesh_array", "xform"), &NavigationMeshSourceGeometryData3D::add_mesh_array);
 	ClassDB::bind_method(D_METHOD("add_faces", "faces", "xform"), &NavigationMeshSourceGeometryData3D::add_faces);
+	ClassDB::bind_method(D_METHOD("merge", "other_geometry"), &NavigationMeshSourceGeometryData3D::merge);
 
 	ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "vertices", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_vertices", "get_vertices");
 	ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT32_ARRAY, "indices", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_indices", "get_indices");
diff --git a/scene/resources/navigation_mesh_source_geometry_data_3d.h b/scene/resources/navigation_mesh_source_geometry_data_3d.h
index 10048773fe..5f68692971 100644
--- a/scene/resources/navigation_mesh_source_geometry_data_3d.h
+++ b/scene/resources/navigation_mesh_source_geometry_data_3d.h
@@ -68,6 +68,8 @@ public:
 	void add_mesh_array(const Array &p_mesh_array, const Transform3D &p_xform);
 	void add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform);
 
+	void merge(const Ref<NavigationMeshSourceGeometryData3D> &p_other_geometry);
+
 	NavigationMeshSourceGeometryData3D() {}
 	~NavigationMeshSourceGeometryData3D() { clear(); }
 };
diff --git a/tests/servers/test_navigation_server_3d.h b/tests/servers/test_navigation_server_3d.h
index b5547f2c94..827a1bed17 100644
--- a/tests/servers/test_navigation_server_3d.h
+++ b/tests/servers/test_navigation_server_3d.h
@@ -35,8 +35,6 @@
 #include "scene/resources/3d/primitive_meshes.h"
 #include "servers/navigation_server_3d.h"
 
-#include "tests/test_macros.h"
-
 namespace TestNavigationServer3D {
 
 // TODO: Find a more generic way to create `Callable` mocks.
@@ -580,6 +578,51 @@ TEST_SUITE("[Navigation]") {
 	}
 #endif // DISABLE_DEPRECATED
 
+	TEST_CASE("[NavigationServer3D][SceneTree] Server should be able to parse geometry") {
+		NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();
+
+		// Prepare scene tree with simple mesh to serve as an input geometry.
+		Node3D *node_3d = memnew(Node3D);
+		SceneTree::get_singleton()->get_root()->add_child(node_3d);
+		Ref<PlaneMesh> plane_mesh = memnew(PlaneMesh);
+		plane_mesh->set_size(Size2(10.0, 10.0));
+		MeshInstance3D *mesh_instance = memnew(MeshInstance3D);
+		mesh_instance->set_mesh(plane_mesh);
+		node_3d->add_child(mesh_instance);
+
+		Ref<NavigationMesh> navigation_mesh = memnew(NavigationMesh);
+		Ref<NavigationMeshSourceGeometryData3D> source_geometry = memnew(NavigationMeshSourceGeometryData3D);
+		CHECK_EQ(source_geometry->get_vertices().size(), 0);
+		CHECK_EQ(source_geometry->get_indices().size(), 0);
+
+		navigation_server->parse_source_geometry_data(navigation_mesh, source_geometry, mesh_instance);
+		CHECK_EQ(source_geometry->get_vertices().size(), 12);
+		CHECK_EQ(source_geometry->get_indices().size(), 6);
+
+		SUBCASE("By default, parsing should remove any data that was parsed before") {
+			navigation_server->parse_source_geometry_data(navigation_mesh, source_geometry, mesh_instance);
+			CHECK_EQ(source_geometry->get_vertices().size(), 12);
+			CHECK_EQ(source_geometry->get_indices().size(), 6);
+		}
+
+		SUBCASE("Parsed geometry should be extendible with other geometry") {
+			source_geometry->merge(source_geometry); // Merging with itself.
+			const Vector<float> vertices = source_geometry->get_vertices();
+			const Vector<int> indices = source_geometry->get_indices();
+			REQUIRE_EQ(vertices.size(), 24);
+			REQUIRE_EQ(indices.size(), 12);
+			// Check if first newly added vertex is the same as first vertex.
+			CHECK_EQ(vertices[0], vertices[12]);
+			CHECK_EQ(vertices[1], vertices[13]);
+			CHECK_EQ(vertices[2], vertices[14]);
+			// Check if first newly added index is the same as first index.
+			CHECK_EQ(indices[0] + 4, indices[6]);
+		}
+
+		memdelete(mesh_instance);
+		memdelete(node_3d);
+	}
+
 	// This test case uses only public APIs on purpose - other test cases use simplified baking.
 	TEST_CASE("[NavigationServer3D][SceneTree] Server should be able to bake map correctly") {
 		NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();