Implement Navigation layers

6 files changed, 44 insertions, 6 deletions

diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 4f61ad5040..af76d9a4cc 100644
--- a/modules/gdnavigation/gd_navigation_server.cpp
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -200,11 +200,11 @@ real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const {
 	return map->get_edge_connection_margin();
 }
 
-Vector<Vector3> GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
+Vector<Vector3> GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
 	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == nullptr, Vector<Vector3>());
 
-	return map->get_path(p_origin, p_destination, p_optimize);
+	return map->get_path(p_origin, p_destination, p_optimize, p_layers);
 }
 
 Vector3 GdNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -273,6 +273,20 @@ COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform) {
 	region->set_transform(p_transform);
 }
 
+COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers) {
+	NavRegion *region = region_owner.getornull(p_region);
+	ERR_FAIL_COND(region == nullptr);
+
+	region->set_layers(p_layers);
+}
+
+uint32_t GdNavigationServer::region_get_layers(RID p_region) const {
+	NavRegion *region = region_owner.getornull(p_region);
+	ERR_FAIL_COND_V(region == nullptr, 0);
+
+	return region->get_layers();
+}
+
 COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh) {
 	NavRegion *region = region_owner.getornull(p_region);
 	ERR_FAIL_COND(region == nullptr);
diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h
index 92f4ccfdd5..8bc65eccab 100644
--- a/modules/gdnavigation/gd_navigation_server.h
+++ b/modules/gdnavigation/gd_navigation_server.h
@@ -100,7 +100,7 @@ public:
 	COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
 	virtual real_t map_get_edge_connection_margin(RID p_map) const;
 
-	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
+	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers = 1) const;
 
 	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const;
 	virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const;
@@ -109,6 +109,8 @@ public:
 
 	virtual RID region_create() const;
 	COMMAND_2(region_set_map, RID, p_region, RID, p_map);
+	COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers);
+	virtual uint32_t region_get_layers(RID p_region) const;
 	COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform);
 	COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh);
 	virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const;
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
index 2646a4cc0c..5289975e4b 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -70,7 +70,7 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 	return p;
 }
 
-Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
+Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
 	const gd::Polygon *begin_poly = nullptr;
 	const gd::Polygon *end_poly = nullptr;
 	Vector3 begin_point;
@@ -82,6 +82,11 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 	for (size_t i(0); i < polygons.size(); i++) {
 		const gd::Polygon &p = polygons[i];
 
+		// Only consider the polygon if it in a region with compatible layers.
+		if ((p_layers & p.owner->get_layers()) == 0) {
+			continue;
+		}
+
 		// For each point cast a face and check the distance between the origin/destination
 		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
 			Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
@@ -144,6 +149,11 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
 				gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
 
+				// Only consider the polygon if it in a region with compatible layers.
+				if ((p_layers & least_cost_poly->poly->owner->get_layers()) == 0) {
+					continue;
+				}
+
 				const gd::Edge &edge = least_cost_poly->poly->edges[i];
 				if (!edge.other_polygon) {
 					continue;
@@ -629,7 +639,7 @@ void NavMap::sync() {
 					connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge;
 				} else {
 					// The edge is already connected with another edge, skip.
-					ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the Navigation3D's `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
+					ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
 				}
 			}
 		}
diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h
index bffc1fbc1a..6b7cfae324 100644
--- a/modules/gdnavigation/nav_map.h
+++ b/modules/gdnavigation/nav_map.h
@@ -102,7 +102,7 @@ public:
 
 	gd::PointKey get_point_key(const Vector3 &p_pos) const;
 
-	Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
+	Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers = 1) const;
 	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
 	Vector3 get_closest_point(const Vector3 &p_point) const;
 	Vector3 get_closest_point_normal(const Vector3 &p_point) const;
diff --git a/modules/gdnavigation/nav_region.cpp b/modules/gdnavigation/nav_region.cpp
index 383b0f15a6..a07995bc11 100644
--- a/modules/gdnavigation/nav_region.cpp
+++ b/modules/gdnavigation/nav_region.cpp
@@ -41,6 +41,14 @@ void NavRegion::set_map(NavMap *p_map) {
 	polygons_dirty = true;
 }
 
+void NavRegion::set_layers(uint32_t p_layers) {
+	layers = p_layers;
+}
+
+uint32_t NavRegion::get_layers() const {
+	return layers;
+}
+
 void NavRegion::set_transform(Transform p_transform) {
 	transform = p_transform;
 	polygons_dirty = true;
diff --git a/modules/gdnavigation/nav_region.h b/modules/gdnavigation/nav_region.h
index 3095435359..fff7843fde 100644
--- a/modules/gdnavigation/nav_region.h
+++ b/modules/gdnavigation/nav_region.h
@@ -48,6 +48,7 @@ class NavRegion : public NavRid {
 	NavMap *map = nullptr;
 	Transform transform;
 	Ref<NavigationMesh> mesh;
+	uint32_t layers = 1;
 
 	bool polygons_dirty = true;
 
@@ -66,6 +67,9 @@ public:
 		return map;
 	}
 
+	void set_layers(uint32_t p_layers);
+	uint32_t get_layers() const;
+
 	void set_transform(Transform transform);
 	const Transform &get_transform() const {
 		return transform;