1 files changed, 8 insertions, 9 deletions

diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp
index 866c415440..46718c8819 100644
--- a/servers/physics_2d/godot_step_2d.cpp
+++ b/servers/physics_2d/godot_step_2d.cpp
@@ -42,12 +42,12 @@ void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D
 	p_body->set_island_step(_step);
 
 	if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
-		// Only dynamic bodies are tested for activation.
+		// Only rigid bodies are tested for activation.
 		p_body_island.push_back(p_body);
 	}
 
 	for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
-		GodotConstraint2D *constraint = (GodotConstraint2D *)E.first;
+		GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first);
 		if (constraint->get_island_step() == _step) {
 			continue; // Already processed.
 		}
@@ -168,8 +168,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
 	const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
 
 	while (aml.first()) {
-		for (const Set<GodotConstraint2D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			GodotConstraint2D *constraint = E->get();
+		for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) {
+			GodotConstraint2D *constraint = E;
 			if (constraint->get_island_step() == _step) {
 				continue;
 			}
@@ -239,7 +239,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
 	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
 	uint32_t total_contraint_count = all_constraints.size();
-	work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr);
+	WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -258,7 +259,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
 
 	// Warning: _solve_island modifies the constraint islands for optimization purpose,
 	// their content is not reliable after these calls and shouldn't be used anymore.
-	work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr);
+	group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -297,10 +299,7 @@ GodotStep2D::GodotStep2D() {
 	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
 	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
 	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
-
-	work_pool.init();
 }
 
 GodotStep2D::~GodotStep2D() {
-	work_pool.finish();
 }