Godot Physics collisions and solver processed on threads

Use ThreadWorkPool to process physics step tasks in multiple threads. Collisions are all processed in parallel and solving impulses is
processed in parallel for rigid body islands.

Additional changes:
- Proper islands for soft bodies linked to active bodies
- All moving areas are on separate islands (can be parallelized)
- Fix inconsistencies with body islands (Kinematic bodies could link
bodies together or not depending on the processing order)
- Completely prevent static bodies to be active (it could cause islands
to be wrongly created and cause dangerous multi-threading operations as
well as inconsistencies in created islands)
- Apply impulses only on dynamic bodies to avoid unsafe multi-threaded
operations (static bodies can be on multiple islands)
- Removed inverted iterations when populating body islands, it's now
faster in regular order (maybe after fixing inconsistencies)

1 files changed, 159 insertions, 68 deletions

diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index 06f3227eab..ba18bac611 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -37,39 +37,90 @@
 #define BODY_ISLAND_SIZE_RESERVE 512
 #define ISLAND_COUNT_RESERVE 128
 #define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
 
 void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
 	p_body->set_island_step(_step);
-	p_body_island.push_back(p_body);
 
-	// Faster with reversed iterations.
-	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) {
-		Constraint3DSW *c = (Constraint3DSW *)E->key();
-		if (c->get_island_step() == _step) {
-			continue; //already processed
+	if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		// Only dynamic bodies are tested for activation.
+		p_body_island.push_back(p_body);
+	}
+
+	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
+		Constraint3DSW *constraint = (Constraint3DSW *)E->key();
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
 		}
-		c->set_island_step(_step);
-		p_constraint_island.push_back(c);
+		constraint->set_island_step(_step);
+		p_constraint_island.push_back(constraint);
+
+		all_constraints.push_back(constraint);
 
-		for (int i = 0; i < c->get_body_count(); i++) {
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
 			if (i == E->get()) {
 				continue;
 			}
-			Body3DSW *b = c->get_body_ptr()[i];
-			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-				continue; //no go
+			Body3DSW *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(other_body, p_body_island, p_constraint_island);
+		}
+
+		// Find connected soft bodies.
+		for (int i = 0; i < constraint->get_soft_body_count(); i++) {
+			SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i);
+			if (soft_body->get_island_step() == _step) {
+				continue; // Already processed.
 			}
-			_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
+			_populate_island_soft_body(soft_body, p_body_island, p_constraint_island);
 		}
 	}
 }
 
-void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) {
+void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
+	p_soft_body->set_island_step(_step);
+
+	for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
+		Constraint3DSW *constraint = (Constraint3DSW *)E->get();
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
+		}
+		constraint->set_island_step(_step);
+		p_constraint_island.push_back(constraint);
+
+		all_constraints.push_back(constraint);
+
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
+			Body3DSW *body = constraint->get_body_ptr()[i];
+			if (body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(body, p_body_island, p_constraint_island);
+		}
+	}
+}
+
+void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+	Constraint3DSW *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const {
 	uint32_t constraint_count = p_constraint_island.size();
 	uint32_t valid_constraint_count = 0;
 	for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
 		Constraint3DSW *constraint = p_constraint_island[constraint_index];
-		if (p_constraint_island[constraint_index]->setup(p_delta)) {
+		if (p_constraint_island[constraint_index]->pre_solve(delta)) {
 			// Keep this constraint for solving.
 			p_constraint_island[valid_constraint_count++] = constraint;
 		}
@@ -77,15 +128,17 @@ void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island,
 	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
+void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
+	LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index];
+
 	int current_priority = 1;
 
-	uint32_t constraint_count = p_constraint_island.size();
+	uint32_t constraint_count = constraint_island.size();
 	while (constraint_count > 0) {
-		for (int i = 0; i < p_iterations; i++) {
+		for (int i = 0; i < iterations; i++) {
 			// Go through all iterations.
 			for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-				p_constraint_island[constraint_index]->solve(p_delta);
+				constraint_island[constraint_index]->solve(delta);
 			}
 		}
 
@@ -93,28 +146,24 @@ void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island,
 		uint32_t priority_constraint_count = 0;
 		++current_priority;
 		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-			Constraint3DSW *constraint = p_constraint_island[constraint_index];
+			Constraint3DSW *constraint = constraint_island[constraint_index];
 			if (constraint->get_priority() >= current_priority) {
 				// Keep this constraint for the next iteration.
-				p_constraint_island[priority_constraint_count++] = constraint;
+				constraint_island[priority_constraint_count++] = constraint;
 			}
 		}
 		constraint_count = priority_constraint_count;
 	}
 }
 
-void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) {
+void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const {
 	bool can_sleep = true;
 
 	uint32_t body_count = p_body_island.size();
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
 		Body3DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
-		if (!body->sleep_test(p_delta)) {
+		if (!body->sleep_test(delta)) {
 			can_sleep = false;
 		}
 	}
@@ -123,10 +172,6 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
 		Body3DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
 		bool active = body->is_active();
 
 		if (active == can_sleep) {
@@ -140,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->setup(); //update inertias, etc
 
+	iterations = p_iterations;
+	delta = p_delta;
+
 	const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
 
 	const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
@@ -175,12 +223,39 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
-	/* GENERATE CONSTRAINT ISLANDS */
+	/* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
+
+	uint32_t island_count = 0;
+
+	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
+
+	while (aml.first()) {
+		for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+			Constraint3DSW *constraint = E->get();
+			if (constraint->get_island_step() == _step) {
+				continue;
+			}
+			constraint->set_island_step(_step);
+
+			// Each constraint can be on a separate island for areas as there's no solving phase.
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+
+			all_constraints.push_back(constraint);
+			constraint_island.push_back(constraint);
+		}
+		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
 
 	b = body_list->first();
 
 	uint32_t body_island_count = 0;
-	uint32_t island_count = 0;
 
 	while (b) {
 		Body3DSW *body = b->self();
@@ -204,7 +279,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 			_populate_island(body, body_island, constraint_island);
 
-			body_islands.push_back(body_island);
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
 
 			if (constraint_island.is_empty()) {
 				--island_count;
@@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		b = b->next();
 	}
 
-	p_space->set_island_count((int)island_count);
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
 
-	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
+	sb = soft_body_list->first();
+	while (sb) {
+		SoftBody3DSW *soft_body = sb->self();
 
-	while (aml.first()) {
-		for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint3DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
+		if (soft_body->get_island_step() != _step) {
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
 			}
-			c->set_island_step(_step);
+			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			body_island.clear();
+			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
+
 			++island_count;
 			if (constraint_islands.size() < island_count) {
 				constraint_islands.resize(island_count);
 			}
 			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
 			constraint_island.clear();
-			constraint_island.push_back(c);
-		}
-		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
-	}
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
-	sb = soft_body_list->first();
-	while (sb) {
-		for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint3DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
+			_populate_island_soft_body(soft_body, body_island, constraint_island);
+
+			if (body_island.is_empty()) {
+				--body_island_count;
 			}
-			c->set_island_step(_step);
-			++island_count;
-			if (constraint_islands.size() < island_count) {
-				constraint_islands.resize(island_count);
+
+			if (constraint_island.is_empty()) {
+				--island_count;
 			}
-			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
-			constraint_island.clear();
-			constraint_island.push_back(c);
 		}
 		sb = sb->next();
 	}
 
+	p_space->set_island_count((int)island_count);
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
-	/* SETUP CONSTRAINT ISLANDS */
+	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
-	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
-		_setup_island(constraint_islands[island_index], p_delta);
-	}
+	uint32_t total_contraint_count = all_constraints.size();
+	work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -272,12 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
-	/* SOLVE CONSTRAINT ISLANDS */
+	/* PRE-SOLVE CONSTRAINT ISLANDS */
 
+	// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
 	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
-		// Warning: _solve_island modifies the constraint islands for optimization purpose,
-		// their content is not reliable after these calls and shouldn't be used anymore.
-		_solve_island(constraint_islands[island_index], p_iterations, p_delta);
+		_pre_solve_island(constraint_islands[island_index]);
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	// Warning: _solve_island modifies the constraint islands for optimization purpose,
+	// their content is not reliable after these calls and shouldn't be used anymore.
+	if (island_count > 1) {
+		work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr);
+	} else if (island_count > 0) {
+		_solve_island(0);
 	}
 
 	{ //profile
@@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 	/* SLEEP / WAKE UP ISLANDS */
 
 	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
-		_check_suspend(body_islands[island_index], p_delta);
+		_check_suspend(body_islands[island_index]);
 	}
 
 	/* UPDATE SOFT BODY CONSTRAINTS */
@@ -315,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
+	all_constraints.clear();
+
 	p_space->update();
 	p_space->unlock();
 	_step++;
@@ -325,4 +409,11 @@ Step3DSW::Step3DSW() {
 
 	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
 	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+
+	work_pool.init();
+}
+
+Step3DSW::~Step3DSW() {
+	work_pool.finish();
 }