1 files changed, 19 insertions, 25 deletions

diff --git a/servers/physics_server_3d_wrap_mt.cpp b/servers/physics_server_3d_wrap_mt.cpp
index f8f60281a7..95b71217c4 100644
--- a/servers/physics_server_3d_wrap_mt.cpp
+++ b/servers/physics_server_3d_wrap_mt.cpp
@@ -32,45 +32,37 @@
 
 #include "core/os/os.h"
 
-void PhysicsServer3DWrapMT::thread_exit() {
-	exit = true;
+void PhysicsServer3DWrapMT::_assign_mt_ids(WorkerThreadPool::TaskID p_pump_task_id) {
+	server_thread = Thread::get_caller_id();
+	server_task_id = p_pump_task_id;
 }
 
-void PhysicsServer3DWrapMT::thread_step(real_t p_delta) {
-	physics_server_3d->step(p_delta);
-	step_sem.post();
+void PhysicsServer3DWrapMT::_thread_exit() {
+	exit = true;
 }
 
-void PhysicsServer3DWrapMT::thread_loop() {
-	server_thread = Thread::get_caller_id();
-
-	physics_server_3d->init();
-
-	command_queue.set_pump_task_id(server_task_id);
+void PhysicsServer3DWrapMT::_thread_loop() {
 	while (!exit) {
 		WorkerThreadPool::get_singleton()->yield();
 		command_queue.flush_all();
 	}
-
-	command_queue.flush_all(); // flush all
-
-	physics_server_3d->finish();
 }
 
 /* EVENT QUEUING */
 
 void PhysicsServer3DWrapMT::step(real_t p_step) {
 	if (create_thread) {
-		command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step);
+		command_queue.push(physics_server_3d, &PhysicsServer3D::step, p_step);
 	} else {
-		command_queue.flush_all(); // Flush all pending from other threads.
 		physics_server_3d->step(p_step);
 	}
 }
 
 void PhysicsServer3DWrapMT::sync() {
 	if (create_thread) {
-		step_sem.wait();
+		command_queue.sync();
+	} else {
+		command_queue.flush_all(); // Flush all pending from other threads.
 	}
 	physics_server_3d->sync();
 }
@@ -85,21 +77,26 @@ void PhysicsServer3DWrapMT::end_sync() {
 
 void PhysicsServer3DWrapMT::init() {
 	if (create_thread) {
-		exit = false;
-		server_task_id = WorkerThreadPool::get_singleton()->add_task(callable_mp(this, &PhysicsServer3DWrapMT::thread_loop), true);
-		step_sem.post();
+		WorkerThreadPool::TaskID tid = WorkerThreadPool::get_singleton()->add_task(callable_mp(this, &PhysicsServer3DWrapMT::_thread_loop), true);
+		command_queue.set_pump_task_id(tid);
+		command_queue.push(this, &PhysicsServer3DWrapMT::_assign_mt_ids, tid);
+		command_queue.push_and_sync(physics_server_3d, &PhysicsServer3D::init);
+		DEV_ASSERT(server_task_id == tid);
 	} else {
+		server_thread = Thread::MAIN_ID;
 		physics_server_3d->init();
 	}
 }
 
 void PhysicsServer3DWrapMT::finish() {
 	if (create_thread) {
-		command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit);
+		command_queue.push(physics_server_3d, &PhysicsServer3D::finish);
+		command_queue.push(this, &PhysicsServer3DWrapMT::_thread_exit);
 		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;
 		}
+		server_thread = Thread::MAIN_ID;
 	} else {
 		physics_server_3d->finish();
 	}
@@ -108,9 +105,6 @@ void PhysicsServer3DWrapMT::finish() {
 PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) {
 	physics_server_3d = p_contained;
 	create_thread = p_create_thread;
-	if (!create_thread) {
-		server_thread = Thread::MAIN_ID;
-	}
 }
 
 PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() {