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)
This commit is contained in:
PouleyKetchoupp
parent
639b02f454
commit
448c41a3e4
31 changed files with 894 additions and 466 deletions
|
|
@ -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.
|
||||
}
|
||||
_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
|
||||
if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
|
||||
continue; // Static bodies don't connect islands.
|
||||
}
|
||||
_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;
|
||||
}
|
||||
|
||||
/* 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 */
|
||||
|
||||
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
|
||||
_solve_island(constraint_islands[island_index], p_iterations, 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.
|
||||
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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue