2 files changed, 20 insertions, 6 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 217e503d82..cbc0adc574 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -30,7 +30,6 @@
 
 #include "nav_map.h"
 
-#include "core/os/threaded_array_processor.h"
 #include "nav_region.h"
 #include "rvo_agent.h"
 
@@ -142,10 +141,10 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 	bool is_reachable = true;
 
 	while (true) {
-		gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
-
 		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
-		for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) {
+		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];
+
 			const gd::Edge &edge = least_cost_poly->poly->edges[i];
 
 			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
@@ -674,7 +673,10 @@ void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) {
 void NavMap::step(real_t p_deltatime) {
 	deltatime = p_deltatime;
 	if (controlled_agents.size() > 0) {
-		thread_process_array(
+		if (step_work_pool.get_thread_count() == 0) {
+			step_work_pool.init();
+		}
+		step_work_pool.do_work(
 				controlled_agents.size(),
 				this,
 				&NavMap::compute_single_step,
@@ -719,3 +721,10 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
 		}
 	}
 }
+
+NavMap::NavMap() {
+}
+
+NavMap::~NavMap() {
+	step_work_pool.finish();
+}
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index f46297a7ce..5232e42bed 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -35,6 +35,7 @@
 
 #include "core/math/math_defs.h"
 #include "core/templates/map.h"
+#include "core/templates/thread_work_pool.h"
 #include "nav_utils.h"
 
 #include <KdTree.h>
@@ -80,8 +81,12 @@ class NavMap : public NavRid {
 	/// Change the id each time the map is updated.
 	uint32_t map_update_id = 0;
 
+	/// Pooled threads for computing steps
+	ThreadWorkPool step_work_pool;
+
 public:
-	NavMap() {}
+	NavMap();
+	~NavMap();
 
 	void set_up(Vector3 p_up);
 	Vector3 get_up() const {