diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp
index b60d8d073dc..619288305cb 100644
--- a/modules/navigation/3d/nav_mesh_queries_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp
@@ -920,40 +920,16 @@ LocalVector<uint32_t> NavMeshQueries3D::get_simplified_path_indices(const LocalV
 	p_epsilon = MAX(0.0, p_epsilon);
 	real_t squared_epsilon = p_epsilon * p_epsilon;
 
-	LocalVector<bool> valid_points;
-	valid_points.resize(p_path.size());
-	for (uint32_t i = 0; i < valid_points.size(); i++) {
-		valid_points[i] = false;
-	}
-
-	simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points);
-
-	int valid_point_index = 0;
-
-	for (bool valid : valid_points) {
-		if (valid) {
-			valid_point_index += 1;
-		}
-	}
-
 	LocalVector<uint32_t> simplified_path_indices;
-	simplified_path_indices.resize(valid_point_index);
-	valid_point_index = 0;
-
-	for (uint32_t i = 0; i < valid_points.size(); i++) {
-		if (valid_points[i]) {
-			simplified_path_indices[valid_point_index] = i;
-			valid_point_index += 1;
-		}
-	}
+	simplified_path_indices.reserve(p_path.size());
+	simplified_path_indices.push_back(0);
+	simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, simplified_path_indices);
+	simplified_path_indices.push_back(p_path.size() - 1);
 
 	return simplified_path_indices;
 }
 
-void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) {
-	r_valid_points[p_start_inx] = true;
-	r_valid_points[p_end_inx] = true;
-
+void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices) {
 	Vector3 path_segment[2] = { p_points[p_start_inx], p_points[p_end_inx] };
 
 	real_t point_max_distance = 0.0;
@@ -972,8 +948,9 @@ void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, con
 	}
 
 	if (point_max_distance > p_epsilon) {
-		simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points);
-		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points);
+		simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_simplified_path_indices);
+		r_simplified_path_indices.push_back(point_max_index);
+		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_simplified_path_indices);
 	}
 }
 
diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h
index a92520f9444..2fd433e07d4 100644
--- a/modules/navigation/3d/nav_mesh_queries_3d.h
+++ b/modules/navigation/3d/nav_mesh_queries_3d.h
@@ -116,7 +116,7 @@ public:
 	static void clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up);
 	static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
 
-	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
+	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
 	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
 };