1 files changed, 223 insertions, 125 deletions

diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index 9a73e11562..ba18bac611 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -33,109 +33,150 @@
 
 #include "core/os/os.h"
 
-void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) {
+#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 Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
 	p_body->set_island_step(_step);
-	p_body->set_island_next(*p_island);
-	*p_island = p_body;
+
+	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 *c = (Constraint3DSW *)E->key();
-		if (c->get_island_step() == _step) {
-			continue; //already processed
+		Constraint3DSW *constraint = (Constraint3DSW *)E->key();
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
 		}
-		c->set_island_step(_step);
-		c->set_island_next(*p_constraint_island);
-		*p_constraint_island = c;
+		constraint->set_island_step(_step);
+		p_constraint_island.push_back(constraint);
 
-		for (int i = 0; i < c->get_body_count(); i++) {
+		all_constraints.push_back(constraint);
+
+		// 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(c->get_body_ptr()[i], p_island, p_constraint_island);
+			_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_soft_body(soft_body, p_body_island, p_constraint_island);
 		}
 	}
 }
 
-void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) {
-	Constraint3DSW *ci = p_island;
-	while (ci) {
-		ci->setup(p_delta);
-		//todo remove from island if process fails
-		ci = ci->get_island_next();
+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]->pre_solve(delta)) {
+			// Keep this constraint for solving.
+			p_constraint_island[valid_constraint_count++] = constraint;
+		}
 	}
+	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) {
-	int at_priority = 1;
+void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
+	LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index];
 
-	while (p_island) {
-		for (int i = 0; i < p_iterations; i++) {
-			Constraint3DSW *ci = p_island;
-			while (ci) {
-				ci->solve(p_delta);
-				ci = ci->get_island_next();
+	int current_priority = 1;
+
+	uint32_t constraint_count = constraint_island.size();
+	while (constraint_count > 0) {
+		for (int i = 0; i < iterations; i++) {
+			// Go through all iterations.
+			for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+				constraint_island[constraint_index]->solve(delta);
 			}
 		}
 
-		at_priority++;
-
-		{
-			Constraint3DSW *ci = p_island;
-			Constraint3DSW *prev = nullptr;
-			while (ci) {
-				if (ci->get_priority() < at_priority) {
-					if (prev) {
-						prev->set_island_next(ci->get_island_next()); //remove
-					} else {
-						p_island = ci->get_island_next();
-					}
-				} else {
-					prev = ci;
-				}
-
-				ci = ci->get_island_next();
+		// Check priority to keep only higher priority constraints.
+		uint32_t priority_constraint_count = 0;
+		++current_priority;
+		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+			Constraint3DSW *constraint = constraint_island[constraint_index];
+			if (constraint->get_priority() >= current_priority) {
+				// Keep this constraint for the next iteration.
+				constraint_island[priority_constraint_count++] = constraint;
 			}
 		}
+		constraint_count = priority_constraint_count;
 	}
 }
 
-void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) {
+void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const {
 	bool can_sleep = true;
 
-	Body3DSW *b = p_island;
-	while (b) {
-		if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			b = b->get_island_next();
-			continue; //ignore for static
-		}
+	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 (!b->sleep_test(p_delta)) {
+		if (!body->sleep_test(delta)) {
 			can_sleep = false;
 		}
-
-		b = b->get_island_next();
 	}
 
-	//put all to sleep or wake up everyoen
-
-	b = p_island;
-	while (b) {
-		if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			b = b->get_island_next();
-			continue; //ignore for static
-		}
+	// Put all to sleep or wake up everyone.
+	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
+		Body3DSW *body = p_body_island[body_index];
 
-		bool active = b->is_active();
+		bool active = body->is_active();
 
 		if (active == can_sleep) {
-			b->set_active(!can_sleep);
+			body->set_active(!can_sleep);
 		}
-
-		b = b->get_island_next();
 	}
 }
 
@@ -144,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();
@@ -179,81 +223,121 @@ 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 */
 
-	Body3DSW *island_list = nullptr;
-	Constraint3DSW *constraint_island_list = nullptr;
 	b = body_list->first();
 
-	int island_count = 0;
+	uint32_t body_island_count = 0;
 
 	while (b) {
 		Body3DSW *body = b->self();
 
 		if (body->get_island_step() != _step) {
-			Body3DSW *island = nullptr;
-			Constraint3DSW *constraint_island = nullptr;
-			_populate_island(body, &island, &constraint_island);
-
-			island->set_island_list_next(island_list);
-			island_list = island;
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
+			}
+			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			body_island.clear();
+			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
 
-			if (constraint_island) {
-				constraint_island->set_island_list_next(constraint_island_list);
-				constraint_island_list = constraint_island;
-				island_count++;
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
 			}
-		}
-		b = b->next();
-	}
+			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
-	p_space->set_island_count(island_count);
+			_populate_island(body, body_island, constraint_island);
 
-	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
 
-	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 (constraint_island.is_empty()) {
+				--island_count;
 			}
-			c->set_island_step(_step);
-			c->set_island_next(nullptr);
-			c->set_island_list_next(constraint_island_list);
-			constraint_island_list = c;
 		}
-		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
+		b = b->next();
 	}
 
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
+
 	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;
-			c->set_island_step(_step);
-			c->set_island_next(NULL);
-			c->set_island_list_next(constraint_island_list);
-			constraint_island_list = c;
+		SoftBody3DSW *soft_body = sb->self();
+
+		if (soft_body->get_island_step() != _step) {
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
+			}
+			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.reserve(ISLAND_SIZE_RESERVE);
+
+			_populate_island_soft_body(soft_body, body_island, constraint_island);
+
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
+
+			if (constraint_island.is_empty()) {
+				--island_count;
+			}
 		}
 		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 */
 
-	{
-		Constraint3DSW *ci = constraint_island_list;
-		while (ci) {
-			_setup_island(ci, p_delta);
-			ci = ci->get_island_list_next();
-		}
-	}
+	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();
@@ -261,15 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
+	/* 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) {
+		_pre_solve_island(constraint_islands[island_index]);
+	}
+
 	/* SOLVE CONSTRAINT ISLANDS */
 
-	{
-		Constraint3DSW *ci = constraint_island_list;
-		while (ci) {
-			//iterating each island separatedly improves cache efficiency
-			_solve_island(ci, p_iterations, p_delta);
-			ci = ci->get_island_list_next();
-		}
+	// 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
@@ -289,12 +379,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* SLEEP / WAKE UP ISLANDS */
 
-	{
-		Body3DSW *bi = island_list;
-		while (bi) {
-			_check_suspend(bi, p_delta);
-			bi = bi->get_island_list_next();
-		}
+	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
+		_check_suspend(body_islands[island_index]);
 	}
 
 	/* UPDATE SOFT BODY CONSTRAINTS */
@@ -311,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++;
@@ -318,4 +406,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 Step3DSW::Step3DSW() {
 	_step = 1;
+
+	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();
 }