Fix `cell_height` for navigation meshes

Fixes `cell_height` for navigation meshes.

15 files changed, 74 insertions, 14 deletions

diff --git a/doc/classes/NavigationMesh.xml b/doc/classes/NavigationMesh.xml
index 5999dc6222..f481fd7099 100644
--- a/doc/classes/NavigationMesh.xml
+++ b/doc/classes/NavigationMesh.xml
@@ -91,10 +91,10 @@
 			[b]Note:[/b] While baking, this value will be rounded up to the nearest multiple of [member cell_size].
 		</member>
 		<member name="cell_height" type="float" setter="set_cell_height" getter="get_cell_height" default="0.25">
-			The Y axis cell size to use for fields.
+			The cell height used to rasterize the navigation mesh vertices on the Y axis. Must match with the cell height on the navigation map.
 		</member>
 		<member name="cell_size" type="float" setter="set_cell_size" getter="get_cell_size" default="0.25">
-			The XZ plane cell size to use for fields.
+			The cell size used to rasterize the navigation mesh vertices on the XZ plane. Must match with the cell size on the navigation map.
 		</member>
 		<member name="detail_sample_distance" type="float" setter="set_detail_sample_distance" getter="get_detail_sample_distance" default="6.0">
 			The sampling distance to use when generating the detail mesh, in cell unit.
diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml
index fb23d22127..f46f2912b3 100644
--- a/doc/classes/NavigationServer2D.xml
+++ b/doc/classes/NavigationServer2D.xml
@@ -339,7 +339,7 @@
 			<return type="float" />
 			<param index="0" name="map" type="RID" />
 			<description>
-				Returns the map cell size.
+				Returns the map cell size used to rasterize the navigation mesh vertices.
 			</description>
 		</method>
 		<method name="map_get_closest_point" qualifiers="const">
@@ -431,7 +431,7 @@
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="cell_size" type="float" />
 			<description>
-				Set the map cell size used to weld the navigation mesh polygons.
+				Sets the map cell size used to rasterize the navigation mesh vertices. Must match with the cell size of the used navigation meshes.
 			</description>
 		</method>
 		<method name="map_set_edge_connection_margin">
diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml
index 6f406aa629..ac9646da4a 100644
--- a/doc/classes/NavigationServer3D.xml
+++ b/doc/classes/NavigationServer3D.xml
@@ -367,11 +367,18 @@
 				Returns all navigation agents [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
+		<method name="map_get_cell_height" qualifiers="const">
+			<return type="float" />
+			<param index="0" name="map" type="RID" />
+			<description>
+				Returns the map cell height used to rasterize the navigation mesh vertices on the Y axis.
+			</description>
+		</method>
 		<method name="map_get_cell_size" qualifiers="const">
 			<return type="float" />
 			<param index="0" name="map" type="RID" />
 			<description>
-				Returns the map cell size.
+				Returns the map cell size used to rasterize the navigation mesh vertices on the XZ plane.
 			</description>
 		</method>
 		<method name="map_get_closest_point" qualifiers="const">
@@ -483,12 +490,20 @@
 				Sets the map active.
 			</description>
 		</method>
+		<method name="map_set_cell_height">
+			<return type="void" />
+			<param index="0" name="map" type="RID" />
+			<param index="1" name="cell_height" type="float" />
+			<description>
+				Sets the map cell height used to rasterize the navigation mesh vertices on the Y axis. Must match with the cell height of the used navigation meshes.
+			</description>
+		</method>
 		<method name="map_set_cell_size">
 			<return type="void" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="cell_size" type="float" />
 			<description>
-				Set the map cell size used to weld the navigation mesh polygons.
+				Sets the map cell size used to rasterize the navigation mesh vertices on the XZ plane. Must match with the cell size of the used navigation meshes.
 			</description>
 		</method>
 		<method name="map_set_edge_connection_margin">
diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index ffb61ab20d..9baa288ace 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -1952,6 +1952,9 @@
 		<member name="navigation/2d/use_edge_connections" type="bool" setter="" getter="" default="true">
 			If enabled 2D navigation regions will use edge connections to connect with other navigation regions within proximity of the navigation map edge connection margin. This setting only affects World2D default navigation maps.
 		</member>
+		<member name="navigation/3d/default_cell_height" type="float" setter="" getter="" default="0.25">
+			Default cell height for 3D navigation maps. See [method NavigationServer3D.map_set_cell_height].
+		</member>
 		<member name="navigation/3d/default_cell_size" type="float" setter="" getter="" default="0.25">
 			Default cell size for 3D navigation maps. See [method NavigationServer3D.map_set_cell_size].
 		</member>
diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index c72b61014b..5baae478c2 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -166,6 +166,20 @@ real_t GodotNavigationServer::map_get_cell_size(RID p_map) const {
 	return map->get_cell_size();
 }
 
+COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height) {
+	NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND(map == nullptr);
+
+	map->set_cell_height(p_cell_height);
+}
+
+real_t GodotNavigationServer::map_get_cell_height(RID p_map) const {
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND_V(map == nullptr, 0);
+
+	return map->get_cell_height();
+}
+
 COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled) {
 	NavMap *map = map_owner.get_or_null(p_map);
 	ERR_FAIL_COND(map == nullptr);
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index 8c9b1eb1b1..1f2096b61a 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -107,6 +107,9 @@ public:
 	COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size);
 	virtual real_t map_get_cell_size(RID p_map) const override;
 
+	COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height);
+	virtual real_t map_get_cell_height(RID p_map) const override;
+
 	COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
 	virtual bool map_get_use_edge_connections(RID p_map) const override;
 
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 57c33a49b5..3f6465d94e 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -69,6 +69,14 @@ void NavMap::set_cell_size(real_t p_cell_size) {
 	regenerate_polygons = true;
 }
 
+void NavMap::set_cell_height(real_t p_cell_height) {
+	if (cell_height == p_cell_height) {
+		return;
+	}
+	cell_height = p_cell_height;
+	regenerate_polygons = true;
+}
+
 void NavMap::set_use_edge_connections(bool p_enabled) {
 	if (use_edge_connections == p_enabled) {
 		return;
@@ -94,9 +102,9 @@ void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
 }
 
 gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
-	const int x = int(Math::floor(p_pos.x / cell_size));
-	const int y = int(Math::floor(p_pos.y / cell_size));
-	const int z = int(Math::floor(p_pos.z / cell_size));
+	const int x = static_cast<int>(Math::floor(p_pos.x / cell_size));
+	const int y = static_cast<int>(Math::floor(p_pos.y / cell_height));
+	const int z = static_cast<int>(Math::floor(p_pos.z / cell_size));
 
 	gd::PointKey p;
 	p.key = 0;
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index 2bd9459626..8d529f464e 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -54,8 +54,9 @@ class NavMap : public NavRid {
 	Vector3 up = Vector3(0, 1, 0);
 
 	/// To find the polygons edges the vertices are displaced in a grid where
-	/// each cell has the following cell_size.
-	real_t cell_size = 0.25;
+	/// each cell has the following cell_size and cell_height.
+	real_t cell_size = 0.25; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size.
+	real_t cell_height = 0.25; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height.
 
 	bool use_edge_connections = true;
 	/// This value is used to detect the near edges to connect.
@@ -131,6 +132,9 @@ public:
 		return cell_size;
 	}
 
+	void set_cell_height(real_t p_cell_height);
+	real_t get_cell_height() const { return cell_height; }
+
 	void set_use_edge_connections(bool p_enabled);
 	bool get_use_edge_connections() const {
 		return use_edge_connections;
diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
index 302b881eb5..bf4fec1ac8 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -110,6 +110,10 @@ void NavRegion::update_polygons() {
 		ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a different `cell_size` than the `cell_size` set on the navigation map.");
 	}
 
+	if (!Math::is_equal_approx(double(map->get_cell_height()), double(mesh->get_cell_height()))) {
+		ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a different `cell_height` than the `cell_height` set on the navigation map.");
+	}
+
 	if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
 		ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
 	}
diff --git a/scene/resources/navigation_mesh.h b/scene/resources/navigation_mesh.h
index e6a432518c..c3b8c13c05 100644
--- a/scene/resources/navigation_mesh.h
+++ b/scene/resources/navigation_mesh.h
@@ -91,8 +91,8 @@ public:
 	};
 
 protected:
-	float cell_size = 0.25f;
-	float cell_height = 0.25f;
+	float cell_size = 0.25f; // Must match ProjectSettings default 3D cell_size and NavigationServer NavMap cell_size.
+	float cell_height = 0.25f; // Must match ProjectSettings default 3D cell_height and NavigationServer NavMap cell_height.
 	float agent_height = 1.5f;
 	float agent_radius = 0.5f;
 	float agent_max_climb = 0.25f;
diff --git a/scene/resources/navigation_polygon.h b/scene/resources/navigation_polygon.h
index 01780c187c..3ab666eb16 100644
--- a/scene/resources/navigation_polygon.h
+++ b/scene/resources/navigation_polygon.h
@@ -51,7 +51,7 @@ class NavigationPolygon : public Resource {
 	// Navigation mesh
 	Ref<NavigationMesh> navigation_mesh;
 
-	real_t cell_size = 1.0f;
+	real_t cell_size = 1.0f; // Must match ProjectSettings default 2D cell_size.
 
 protected:
 	static void _bind_methods();
diff --git a/scene/resources/world_3d.cpp b/scene/resources/world_3d.cpp
index cc961941cf..297d219caf 100644
--- a/scene/resources/world_3d.cpp
+++ b/scene/resources/world_3d.cpp
@@ -67,6 +67,7 @@ RID World3D::get_navigation_map() const {
 		navigation_map = NavigationServer3D::get_singleton()->map_create();
 		NavigationServer3D::get_singleton()->map_set_active(navigation_map, true);
 		NavigationServer3D::get_singleton()->map_set_cell_size(navigation_map, GLOBAL_GET("navigation/3d/default_cell_size"));
+		NavigationServer3D::get_singleton()->map_set_cell_height(navigation_map, GLOBAL_GET("navigation/3d/default_cell_height"));
 		NavigationServer3D::get_singleton()->map_set_use_edge_connections(navigation_map, GLOBAL_GET("navigation/3d/use_edge_connections"));
 		NavigationServer3D::get_singleton()->map_set_edge_connection_margin(navigation_map, GLOBAL_GET("navigation/3d/default_edge_connection_margin"));
 		NavigationServer3D::get_singleton()->map_set_link_connection_radius(navigation_map, GLOBAL_GET("navigation/3d/default_link_connection_radius"));
diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp
index f9f8797b76..45b85a616e 100644
--- a/servers/navigation_server_3d.cpp
+++ b/servers/navigation_server_3d.cpp
@@ -43,6 +43,8 @@ void NavigationServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("map_get_up", "map"), &NavigationServer3D::map_get_up);
 	ClassDB::bind_method(D_METHOD("map_set_cell_size", "map", "cell_size"), &NavigationServer3D::map_set_cell_size);
 	ClassDB::bind_method(D_METHOD("map_get_cell_size", "map"), &NavigationServer3D::map_get_cell_size);
+	ClassDB::bind_method(D_METHOD("map_set_cell_height", "map", "cell_height"), &NavigationServer3D::map_set_cell_height);
+	ClassDB::bind_method(D_METHOD("map_get_cell_height", "map"), &NavigationServer3D::map_get_cell_height);
 	ClassDB::bind_method(D_METHOD("map_set_use_edge_connections", "map", "enabled"), &NavigationServer3D::map_set_use_edge_connections);
 	ClassDB::bind_method(D_METHOD("map_get_use_edge_connections", "map"), &NavigationServer3D::map_get_use_edge_connections);
 	ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &NavigationServer3D::map_set_edge_connection_margin);
@@ -180,6 +182,7 @@ NavigationServer3D::NavigationServer3D() {
 	GLOBAL_DEF_BASIC("navigation/2d/default_link_connection_radius", 4);
 
 	GLOBAL_DEF_BASIC("navigation/3d/default_cell_size", 0.25);
+	GLOBAL_DEF_BASIC("navigation/3d/default_cell_height", 0.25);
 	GLOBAL_DEF("navigation/3d/use_edge_connections", true);
 	GLOBAL_DEF_BASIC("navigation/3d/default_edge_connection_margin", 0.25);
 	GLOBAL_DEF_BASIC("navigation/3d/default_link_connection_radius", 1.0);
diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h
index 23c38532e8..da3741ed8b 100644
--- a/servers/navigation_server_3d.h
+++ b/servers/navigation_server_3d.h
@@ -80,6 +80,9 @@ public:
 	/// Returns the map cell size.
 	virtual real_t map_get_cell_size(RID p_map) const = 0;
 
+	virtual void map_set_cell_height(RID p_map, real_t p_height) = 0;
+	virtual real_t map_get_cell_height(RID p_map) const = 0;
+
 	virtual void map_set_use_edge_connections(RID p_map, bool p_enabled) = 0;
 	virtual bool map_get_use_edge_connections(RID p_map) const = 0;
 
diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h
index 23fd8291db..d2f11ccfa0 100644
--- a/servers/navigation_server_3d_dummy.h
+++ b/servers/navigation_server_3d_dummy.h
@@ -45,6 +45,8 @@ public:
 	Vector3 map_get_up(RID p_map) const override { return Vector3(); }
 	void map_set_cell_size(RID p_map, real_t p_cell_size) override {}
 	real_t map_get_cell_size(RID p_map) const override { return 0; }
+	void map_set_cell_height(RID p_map, real_t p_cell_height) override {}
+	real_t map_get_cell_height(RID p_map) const override { return 0; }
 	void map_set_use_edge_connections(RID p_map, bool p_enabled) override {}
 	bool map_get_use_edge_connections(RID p_map) const override { return false; }
 	void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) override {}