Merge commit godotengine/godot@cb411fa960f0b7fdbd97dcdb4c90f9346360ee0e

2 files changed, 94 insertions, 131 deletions

diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation/2d/nav_mesh_generator_2d.cpp
index 44f45529ca..99ecb416ed 100644
--- a/modules/navigation/2d/nav_mesh_generator_2d.cpp
+++ b/modules/navigation/2d/nav_mesh_generator_2d.cpp
@@ -693,11 +693,15 @@ void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref<Navig
 		return;
 	}
 
-	const Transform2D node_xform = p_source_geometry_data->root_node_transform * Transform2D(0.0, obstacle->get_global_position());
-
+	const Vector2 safe_scale = obstacle->get_global_scale().abs().maxf(0.001);
 	const float obstacle_radius = obstacle->get_radius();
 
 	if (obstacle_radius > 0.0) {
+		// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
+		const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+		const Vector2 uniform_max_scale = Vector2(scaling_max_value, scaling_max_value);
+		const Transform2D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform2D(obstacle->get_global_rotation(), uniform_max_scale, 0.0, obstacle->get_global_position());
+
 		Vector<Vector2> obstruction_circle_vertices;
 
 		// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
@@ -711,12 +715,15 @@ void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref<Navig
 
 		for (int i = 0; i < circle_points; i++) {
 			const float angle = i * circle_point_step;
-			circle_vertices_ptrw[i] = node_xform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
+			circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
 		}
 
 		p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh());
 	}
 
+	// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
+	const Transform2D node_xform = p_source_geometry_data->root_node_transform * obstacle->get_global_transform();
+
 	const Vector<Vector2> &obstacle_vertices = obstacle->get_vertices();
 
 	if (obstacle_vertices.is_empty()) {
@@ -762,16 +769,14 @@ void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPoly
 static void generator_recursive_process_polytree_items(List<TPPLPoly> &p_tppl_in_polygon, const Clipper2Lib::PolyPathD *p_polypath_item) {
 	using namespace Clipper2Lib;
 
-	Vector<Vector2> polygon_vertices;
+	TPPLPoly tp;
+	int size = p_polypath_item->Polygon().size();
+	tp.Init(size);
 
+	int j = 0;
 	for (const PointD &polypath_point : p_polypath_item->Polygon()) {
-		polygon_vertices.push_back(Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y)));
-	}
-
-	TPPLPoly tp;
-	tp.Init(polygon_vertices.size());
-	for (int j = 0; j < polygon_vertices.size(); j++) {
-		tp[j] = polygon_vertices[j];
+		tp[j] = Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y));
+		++j;
 	}
 
 	if (p_polypath_item->IsHole()) {
@@ -844,87 +849,79 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 		return;
 	}
 
-	if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
-		return;
-	}
-
-	int outline_count = p_navigation_mesh->get_outline_count();
-
-	Vector<Vector<Vector2>> traversable_outlines;
-	Vector<Vector<Vector2>> obstruction_outlines;
-	Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> projected_obstructions;
-
-	p_source_geometry_data->get_data(
-			traversable_outlines,
-			obstruction_outlines,
-			projected_obstructions);
-
-	if (outline_count == 0 && traversable_outlines.size() == 0) {
-		return;
-	}
-
 	using namespace Clipper2Lib;
-
 	PathsD traversable_polygon_paths;
 	PathsD obstruction_polygon_paths;
+	int obstruction_polygon_path_size = 0;
+	{
+		RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
 
-	traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
-	obstruction_polygon_paths.reserve(obstruction_outlines.size());
+		const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->traversable_outlines;
+		int outline_count = p_navigation_mesh->get_outline_count();
 
-	for (int i = 0; i < outline_count; i++) {
-		const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
-		PathD subject_path;
-		subject_path.reserve(traversable_outline.size());
-		for (const Vector2 &traversable_point : traversable_outline) {
-			const PointD &point = PointD(traversable_point.x, traversable_point.y);
-			subject_path.push_back(point);
+		if (outline_count == 0 && (!p_source_geometry_data->has_data() || (traversable_outlines.is_empty()))) {
+			return;
 		}
-		traversable_polygon_paths.push_back(subject_path);
-	}
 
-	for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
-		PathD subject_path;
-		subject_path.reserve(traversable_outline.size());
-		for (const Vector2 &traversable_point : traversable_outline) {
-			const PointD &point = PointD(traversable_point.x, traversable_point.y);
-			subject_path.push_back(point);
-		}
-		traversable_polygon_paths.push_back(subject_path);
-	}
+		const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->obstruction_outlines;
+		const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
+
+		traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
+		obstruction_polygon_paths.reserve(obstruction_outlines.size());
 
-	for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
-		PathD clip_path;
-		clip_path.reserve(obstruction_outline.size());
-		for (const Vector2 &obstruction_point : obstruction_outline) {
-			const PointD &point = PointD(obstruction_point.x, obstruction_point.y);
-			clip_path.push_back(point);
+		for (int i = 0; i < outline_count; i++) {
+			const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
+			PathD subject_path;
+			subject_path.reserve(traversable_outline.size());
+			for (const Vector2 &traversable_point : traversable_outline) {
+				subject_path.emplace_back(traversable_point.x, traversable_point.y);
+			}
+			traversable_polygon_paths.push_back(std::move(subject_path));
 		}
-		obstruction_polygon_paths.push_back(clip_path);
-	}
 
-	if (!projected_obstructions.is_empty()) {
-		for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
-			if (projected_obstruction.carve) {
-				continue;
+		for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
+			PathD subject_path;
+			subject_path.reserve(traversable_outline.size());
+			for (const Vector2 &traversable_point : traversable_outline) {
+				subject_path.emplace_back(traversable_point.x, traversable_point.y);
 			}
-			if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
-				continue;
+			traversable_polygon_paths.push_back(std::move(subject_path));
+		}
+
+		if (!projected_obstructions.is_empty()) {
+			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));
 			}
+		}
 
+		obstruction_polygon_path_size = obstruction_polygon_paths.size();
+		for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
 			PathD clip_path;
-			clip_path.reserve(projected_obstruction.vertices.size() / 2);
-			for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
-				const PointD &point = PointD(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
-				clip_path.push_back(point);
-			}
-			if (!IsPositive(clip_path)) {
-				std::reverse(clip_path.begin(), clip_path.end());
+			clip_path.reserve(obstruction_outline.size());
+			for (const Vector2 &obstruction_point : obstruction_outline) {
+				clip_path.emplace_back(obstruction_point.x, obstruction_point.y);
 			}
-			obstruction_polygon_paths.push_back(clip_path);
+			obstruction_polygon_paths.push_back(std::move(clip_path));
 		}
 	}
 
 	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();
 
@@ -936,48 +933,27 @@ 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);
-		obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
+		area_obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
+	} else {
+		area_obstruction_polygon_paths = obstruction_polygon_paths;
 	}
 
-	PathsD path_solution;
-
 	// 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
-	obstruction_polygon_paths = Union(obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
+	area_obstruction_polygon_paths = Union(area_obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
 
-	path_solution = Difference(traversable_polygon_paths, obstruction_polygon_paths, FillRule::NonZero);
+	PathsD path_solution = Difference(traversable_polygon_paths, area_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 (!projected_obstructions.is_empty()) {
-		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++) {
-				const PointD &point = PointD(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
-				clip_path.push_back(point);
-			}
-			if (!IsPositive(clip_path)) {
-				std::reverse(clip_path.begin(), clip_path.end());
-			}
-			obstruction_polygon_paths.push_back(clip_path);
-		}
-		if (obstruction_polygon_paths.size() > 0) {
-			path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
-		}
+	if (obstruction_polygon_path_size > 0) {
+		obstruction_polygon_paths.resize(obstruction_polygon_path_size);
+		path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
 	}
 
 	//path_solution = RamerDouglasPeucker(path_solution, 0.025); //
@@ -996,33 +972,11 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 		path_solution = RectClip(clipper_rect, path_solution);
 	}
 
-	Vector<Vector<Vector2>> new_baked_outlines;
-
-	for (const PathD &scaled_path : path_solution) {
-		Vector<Vector2> polypath;
-		for (const PointD &scaled_point : scaled_path) {
-			polypath.push_back(Vector2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
-		}
-		new_baked_outlines.push_back(polypath);
-	}
-
-	if (new_baked_outlines.size() == 0) {
+	if (path_solution.size() == 0) {
 		p_navigation_mesh->clear();
 		return;
 	}
 
-	PathsD polygon_paths;
-	polygon_paths.reserve(new_baked_outlines.size());
-
-	for (const Vector<Vector2> &baked_outline : new_baked_outlines) {
-		PathD polygon_path;
-		for (const Vector2 &baked_outline_point : baked_outline) {
-			const PointD &point = PointD(baked_outline_point.x, baked_outline_point.y);
-			polygon_path.push_back(point);
-		}
-		polygon_paths.push_back(polygon_path);
-	}
-
 	ClipType clipper_cliptype = ClipType::Union;
 
 	List<TPPLPoly> tppl_in_polygon, tppl_out_polygon;
@@ -1030,7 +984,7 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
 	PolyTreeD polytree;
 	ClipperD clipper_D;
 
-	clipper_D.AddSubject(polygon_paths);
+	clipper_D.AddSubject(path_solution);
 	clipper_D.Execute(clipper_cliptype, FillRule::NonZero, polytree);
 
 	for (size_t i = 0; i < polytree.Count(); i++) {
diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp
index bfbd8d539f..43a7a5f6bb 100644
--- a/modules/navigation/3d/nav_mesh_generator_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp
@@ -597,11 +597,17 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
 		return;
 	}
 
-	const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis(), obstacle->get_global_position());
-
+	const float elevation = obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y;
+	// Prevent non-positive scaling.
+	const Vector3 safe_scale = obstacle->get_global_basis().get_scale().abs().maxf(0.001);
 	const float obstacle_radius = obstacle->get_radius();
 
 	if (obstacle_radius > 0.0) {
+		// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
+		const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+		const Vector3 uniform_max_scale = Vector3(scaling_max_value, scaling_max_value, scaling_max_value);
+		const Transform3D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(uniform_max_scale), obstacle->get_global_position());
+
 		Vector<Vector3> obstruction_circle_vertices;
 
 		// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
@@ -615,12 +621,15 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
 
 		for (int i = 0; i < circle_points; i++) {
 			const float angle = i * circle_point_step;
-			circle_vertices_ptrw[i] = node_xform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
+			circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
 		}
 
-		p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y - obstacle_radius, obstacle_radius, obstacle->get_carve_navigation_mesh());
+		p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, elevation - obstacle_radius, scaling_max_value * obstacle_radius, obstacle->get_carve_navigation_mesh());
 	}
 
+	// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
+	const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y), obstacle->get_global_position());
+
 	const Vector<Vector3> &obstacle_vertices = obstacle->get_vertices();
 
 	if (obstacle_vertices.is_empty()) {
@@ -637,7 +646,7 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
 		obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
 		obstruction_shape_vertices_ptrw[i].y = 0.0;
 	}
-	p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y, obstacle->get_height(), obstacle->get_carve_navigation_mesh());
+	p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, elevation, safe_scale.y * obstacle->get_height(), obstacle->get_carve_navigation_mesh());
 }
 
 void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {