1 files changed, 19 insertions, 35 deletions

diff --git a/servers/physics_server_2d_wrap_mt.cpp b/servers/physics_server_2d_wrap_mt.cpp
index f0d31ddb7a..4548bb91cb 100644
--- a/servers/physics_server_2d_wrap_mt.cpp
+++ b/servers/physics_server_2d_wrap_mt.cpp
@@ -33,7 +33,7 @@
 #include "core/os/os.h"
 
 void PhysicsServer2DWrapMT::thread_exit() {
-	exit.set();
+	exit = true;
 }
 
 void PhysicsServer2DWrapMT::thread_step(real_t p_delta) {
@@ -41,25 +41,18 @@ void PhysicsServer2DWrapMT::thread_step(real_t p_delta) {
 	step_sem.post();
 }
 
-void PhysicsServer2DWrapMT::_thread_callback(void *_instance) {
-	PhysicsServer2DWrapMT *vsmt = reinterpret_cast<PhysicsServer2DWrapMT *>(_instance);
-
-	vsmt->thread_loop();
-}
-
 void PhysicsServer2DWrapMT::thread_loop() {
 	server_thread = Thread::get_caller_id();
 
 	physics_server_2d->init();
 
-	exit.clear();
-	step_thread_up.set();
-	while (!exit.is_set()) {
-		// flush commands one by one, until exit is requested
-		command_queue.wait_and_flush();
+	command_queue.set_pump_task_id(server_task_id);
+	while (!exit) {
+		WorkerThreadPool::get_singleton()->yield();
+		command_queue.flush_all();
 	}
 
-	command_queue.flush_all(); // flush all
+	command_queue.flush_all();
 
 	physics_server_2d->finish();
 }
@@ -70,18 +63,14 @@ void PhysicsServer2DWrapMT::step(real_t p_step) {
 	if (create_thread) {
 		command_queue.push(this, &PhysicsServer2DWrapMT::thread_step, p_step);
 	} else {
-		command_queue.flush_all(); //flush all pending from other threads
+		command_queue.flush_all(); // Flush all pending from other threads.
 		physics_server_2d->step(p_step);
 	}
 }
 
 void PhysicsServer2DWrapMT::sync() {
 	if (create_thread) {
-		if (first_frame) {
-			first_frame = false;
-		} else {
-			step_sem.wait(); //must not wait if a step was not issued
-		}
+		step_sem.wait();
 	}
 	physics_server_2d->sync();
 }
@@ -96,39 +85,34 @@ void PhysicsServer2DWrapMT::end_sync() {
 
 void PhysicsServer2DWrapMT::init() {
 	if (create_thread) {
-		thread.start(_thread_callback, this);
-		while (!step_thread_up.is_set()) {
-			OS::get_singleton()->delay_usec(1000);
-		}
+		exit = false;
+		server_task_id = WorkerThreadPool::get_singleton()->add_task(callable_mp(this, &PhysicsServer2DWrapMT::thread_loop), true);
+		step_sem.post();
 	} else {
 		physics_server_2d->init();
 	}
 }
 
 void PhysicsServer2DWrapMT::finish() {
-	if (thread.is_started()) {
+	if (create_thread) {
 		command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit);
-		thread.wait_to_finish();
+		if (server_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(server_task_id);
+			server_task_id = WorkerThreadPool::INVALID_TASK_ID;
+		}
 	} else {
 		physics_server_2d->finish();
 	}
 }
 
-PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) :
-		command_queue(p_create_thread) {
+PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) {
 	physics_server_2d = p_contained;
 	create_thread = p_create_thread;
-
-	if (!p_create_thread) {
-		server_thread = Thread::get_caller_id();
-	} else {
-		server_thread = 0;
+	if (!create_thread) {
+		server_thread = Thread::MAIN_ID;
 	}
-
-	main_thread = Thread::get_caller_id();
 }
 
 PhysicsServer2DWrapMT::~PhysicsServer2DWrapMT() {
 	memdelete(physics_server_2d);
-	//finish();
 }