summaryrefslogtreecommitdiff
path: root/servers/physics_2d/step_2d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/step_2d_sw.cpp')
-rw-r--r--servers/physics_2d/step_2d_sw.cpp288
1 files changed, 147 insertions, 141 deletions
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 6613d19729..0306ec5050 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -29,102 +29,98 @@
/*************************************************************************/
#include "step_2d_sw.h"
+
#include "core/os/os.h"
-void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **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 Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) {
p_body->set_island_step(_step);
- p_body->set_island_next(*p_island);
- *p_island = p_body;
- for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) {
- 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 Pair<Constraint2DSW *, int> &E : p_body->get_constraint_list()) {
+ Constraint2DSW *constraint = (Constraint2DSW *)E.first;
+ 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);
+ all_constraints.push_back(constraint);
- for (int i = 0; i < c->get_body_count(); i++) {
- if (i == E->get().second) {
+ for (int i = 0; i < constraint->get_body_count(); i++) {
+ if (i == E.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_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);
}
}
}
-bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) {
- Constraint2DSW *ci = p_island;
- Constraint2DSW *prev_ci = nullptr;
- bool removed_root = false;
- while (ci) {
- bool process = ci->setup(p_delta);
-
- if (!process) {
- //remove from island if process fails
- if (prev_ci) {
- prev_ci->set_island_next(ci->get_island_next());
- } else {
- removed_root = true;
- prev_ci = ci;
- }
- } else {
- prev_ci = ci;
+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]->pre_solve(delta)) {
+ // Keep this constraint for solving.
+ p_constraint_island[valid_constraint_count++] = constraint;
}
- ci = ci->get_island_next();
}
-
- return removed_root;
+ p_constraint_island.resize(valid_constraint_count);
}
-void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta) {
- for (int i = 0; i < p_iterations; i++) {
- Constraint2DSW *ci = p_island;
- while (ci) {
- ci->solve(p_delta);
- ci = ci->get_island_next();
+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) {
+ constraint_island[constraint_index]->solve(delta);
}
}
}
-void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) {
+void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
bool can_sleep = true;
- Body2DSW *b = p_island;
- while (b) {
- if (b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::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) {
+ Body2DSW *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() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::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) {
+ Body2DSW *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();
}
}
@@ -133,6 +129,11 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc
+ p_space->set_last_step(p_delta);
+
+ iterations = p_iterations;
+ delta = p_delta;
+
const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */
@@ -157,94 +158,85 @@ 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 */
- Body2DSW *island_list = nullptr;
- Constraint2DSW *constraint_island_list = nullptr;
b = body_list->first();
- int island_count = 0;
+ uint32_t body_island_count = 0;
while (b) {
Body2DSW *body = b->self();
if (body->get_island_step() != _step) {
- Body2DSW *island = nullptr;
- Constraint2DSW *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<Body2DSW *> &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<Constraint2DSW *> &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<Area2DSW>::List &aml = p_space->get_moved_area_list();
+ if (body_island.is_empty()) {
+ --body_island_count;
+ }
- 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;
+ 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<Area2DSW> *)aml.first()); //faster to remove here
+ b = b->next();
}
+ p_space->set_island_count((int)island_count);
+
{ //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 */
-
- {
- Constraint2DSW *ci = constraint_island_list;
- Constraint2DSW *prev_ci = nullptr;
- while (ci) {
- if (_setup_island(ci, p_delta)) {
- //removed the root from the island graph because it is not to be processed
-
- Constraint2DSW *next = ci->get_island_next();
-
- if (next) {
- //root from list being deleted no longer exists, replace by next
- next->set_island_list_next(ci->get_island_list_next());
- if (prev_ci) {
- prev_ci->set_island_list_next(next);
- } else {
- constraint_island_list = next;
- }
- prev_ci = next;
- } else {
- //list is empty, just skip
- if (prev_ci) {
- prev_ci->set_island_list_next(ci->get_island_list_next());
-
- } else {
- constraint_island_list = ci->get_island_list_next();
- }
- }
- } else {
- prev_ci = ci;
- }
+ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
- ci = ci->get_island_list_next();
- }
- }
+ 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();
@@ -252,15 +244,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 */
- {
- Constraint2DSW *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, &Step2DSW::_solve_island, nullptr);
+ } else if (island_count > 0) {
+ _solve_island(0);
}
{ //profile
@@ -280,12 +278,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */
- {
- Body2DSW *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]);
}
{ //profile
@@ -294,6 +288,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++;
@@ -301,4 +297,14 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
Step2DSW::Step2DSW() {
_step = 1;
+
+ 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();
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-16 03:47:01 +0000