1 files changed, 98 insertions, 59 deletions

diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 406d750776..b8cb4cddc5 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -29,45 +29,59 @@
 /*************************************************************************/
 
 #include "step_2d_sw.h"
+
 #include "core/os/os.h"
 
 #define BODY_ISLAND_COUNT_RESERVE 128
 #define BODY_ISLAND_SIZE_RESERVE 512
 #define ISLAND_COUNT_RESERVE 128
 #define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
 
 void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) {
 	p_body->set_island_step(_step);
-	p_body_island.push_back(p_body);
 
-	// Faster with reversed iterations.
-	for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) {
-		Constraint2DSW *c = (Constraint2DSW *)E->get().first;
-		if (c->get_island_step() == _step) {
-			continue; //already processed
+	if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		// Only dynamic bodies are tested for activation.
+		p_body_island.push_back(p_body);
+	}
+
+	for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) {
+		Constraint2DSW *constraint = (Constraint2DSW *)E->get().first;
+		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++) {
+		for (int i = 0; i < constraint->get_body_count(); i++) {
 			if (i == E->get().second) {
 				continue;
 			}
-			Body2DSW *b = c->get_body_ptr()[i];
-			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
-				continue; //no go
+			Body2DSW *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
 			}
-			_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
+			_populate_island(other_body, p_body_island, p_constraint_island);
 		}
 	}
 }
 
-void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta) {
+void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+	Constraint2DSW *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &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) {
 		Constraint2DSW *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;
 		}
@@ -75,27 +89,25 @@ void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island,
 	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
-	for (int i = 0; i < p_iterations; i++) {
-		uint32_t constraint_count = p_constraint_island.size();
+void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
+	const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index];
+
+	for (int i = 0; i < iterations; i++) {
+		uint32_t constraint_count = constraint_island.size();
 		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);
 		}
 	}
 }
 
-void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta) {
+void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &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) {
 		Body2DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
-		if (!body->sleep_test(p_delta)) {
+		if (!body->sleep_test(delta)) {
 			can_sleep = false;
 		}
 	}
@@ -104,10 +116,6 @@ void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
 		Body2DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
 		bool active = body->is_active();
 
 		if (active == can_sleep) {
@@ -121,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->setup(); //update inertias, etc
 
+	iterations = p_iterations;
+	delta = p_delta;
+
 	const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
 
 	/* INTEGRATE FORCES */
@@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *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<Area2DSW>::List &aml = p_space->get_moved_area_list();
+
+	while (aml.first()) {
+		for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+			Constraint2DSW *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<Constraint2DSW *> &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<Area2DSW> *)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) {
 		Body2DSW *body = b->self();
@@ -174,7 +212,9 @@ void Step2DSW::step(Space2DSW *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;
@@ -185,37 +225,16 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->set_island_count((int)island_count);
 
-	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
-
-	while (aml.first()) {
-		for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint2DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
-			}
-			c->set_island_step(_step);
-			++island_count;
-			if (constraint_islands.size() < island_count) {
-				constraint_islands.resize(island_count);
-			}
-			LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
-			constraint_island.clear();
-			constraint_island.push_back(c);
-		}
-		p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
-	}
-
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space2DSW::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, &Step2DSW::_setup_contraint, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -223,10 +242,21 @@ void Step2DSW::step(Space2DSW *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) {
-		_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, &Step2DSW::_solve_island, nullptr);
+	} else if (island_count > 0) {
+		_solve_island(0);
 	}
 
 	{ //profile
@@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *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]);
 	}
 
 	{ //profile
@@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 		//profile_begtime=profile_endtime;
 	}
 
+	all_constraints.clear();
+
 	p_space->update();
 	p_space->unlock();
 	_step++;
@@ -266,4 +298,11 @@ Step2DSW::Step2DSW() {
 
 	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
 	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+
+	work_pool.init();
+}
+
+Step2DSW::~Step2DSW() {
+	work_pool.finish();
 }