From 3a946aaa73138a23e52471fa7a65a8c5eb1fe221 Mon Sep 17 00:00:00 2001
From: Kiro <mouton.guillaume88@gmail.com>
Date: Fri, 15 Nov 2024 11:12:14 +0100
Subject: [PATCH] fixed navigation obstacle carving broken during 07b7f76

---
 .../navigation/2d/nav_mesh_generator_2d.cpp   | 44 ++++++++++++++-----
 1 file changed, 32 insertions(+), 12 deletions(-)

diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation/2d/nav_mesh_generator_2d.cpp
index c996c9e9351..a8eb07147cc 100644
--- a/modules/navigation/2d/nav_mesh_generator_2d.cpp
+++ b/modules/navigation/2d/nav_mesh_generator_2d.cpp
@@ -850,7 +850,7 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 	using namespace Clipper2Lib;
 	PathsD traversable_polygon_paths;
 	PathsD obstruction_polygon_paths;
-	int obstruction_polygon_path_size = 0;
+	bool empty_projected_obstructions = true;
 	{
 		RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
 
@@ -886,7 +886,8 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 			traversable_polygon_paths.push_back(std::move(subject_path));
 		}
 
-		if (!projected_obstructions.is_empty()) {
+		empty_projected_obstructions = projected_obstructions.is_empty();
+		if (!empty_projected_obstructions) {
 			for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
 				if (projected_obstruction.carve) {
 					continue;
@@ -907,7 +908,6 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 			}
 		}
 
-		obstruction_polygon_path_size = obstruction_polygon_paths.size();
 		for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
 			PathD clip_path;
 			clip_path.reserve(obstruction_outline.size());
@@ -919,7 +919,6 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 	}
 
 	Rect2 baking_rect = p_navigation_mesh->get_baking_rect();
-	PathsD area_obstruction_polygon_paths;
 	if (baking_rect.has_area()) {
 		Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
 
@@ -931,27 +930,48 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 		RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
 
 		traversable_polygon_paths = RectClip(clipper_rect, traversable_polygon_paths);
-		area_obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
-	} else {
-		area_obstruction_polygon_paths = obstruction_polygon_paths;
+		obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
 	}
 
 	// first merge all traversable polygons according to user specified fill rule
 	PathsD dummy_clip_path;
 	traversable_polygon_paths = Union(traversable_polygon_paths, dummy_clip_path, FillRule::NonZero);
 	// merge all obstruction polygons, don't allow holes for what is considered "solid" 2D geometry
-	area_obstruction_polygon_paths = Union(area_obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
+	obstruction_polygon_paths = Union(obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
 
-	PathsD path_solution = Difference(traversable_polygon_paths, area_obstruction_polygon_paths, FillRule::NonZero);
+	PathsD path_solution = Difference(traversable_polygon_paths, obstruction_polygon_paths, FillRule::NonZero);
 
 	real_t agent_radius_offset = p_navigation_mesh->get_agent_radius();
 	if (agent_radius_offset > 0.0) {
 		path_solution = InflatePaths(path_solution, -agent_radius_offset, JoinType::Miter, EndType::Polygon);
 	}
 
-	if (obstruction_polygon_path_size > 0) {
-		obstruction_polygon_paths.resize(obstruction_polygon_path_size);
-		path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
+	// Apply obstructions that are not affected by agent radius, the ones with carve enabled.
+	if (!empty_projected_obstructions) {
+		RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
+		const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
+		obstruction_polygon_paths.resize(0);
+		for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+			if (!projected_obstruction.carve) {
+				continue;
+			}
+			if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
+				continue;
+			}
+
+			PathD clip_path;
+			clip_path.reserve(projected_obstruction.vertices.size() / 2);
+			for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
+				clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
+			}
+			if (!IsPositive(clip_path)) {
+				std::reverse(clip_path.begin(), clip_path.end());
+			}
+			obstruction_polygon_paths.push_back(std::move(clip_path));
+		}
+		if (obstruction_polygon_paths.size() > 0) {
+			path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
+		}
 	}
 
 	//path_solution = RamerDouglasPeucker(path_solution, 0.025); //