From b65d6b56fb83fdd745350b6b415bda44dea7398b Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 12 Apr 2021 16:22:06 -0700 Subject: [PATCH] Godot Physics solver optimization Several optimizations in the way solver islands are processed in both 2D and 3D physics: - Use LocalVector instead of linked list to avoid cache misses (with persistent storage based on worst case scenario) - Remove pairs when setup fails (no valid contact) to avoid unnecessary solving of non-colliding rigid bodies just to return immediately --- servers/physics_2d/body_2d_sw.cpp | 2 - servers/physics_2d/body_2d_sw.h | 8 - servers/physics_2d/constraint_2d_sw.h | 8 - servers/physics_2d/step_2d_sw.cpp | 191 ++++++++++-------------- servers/physics_2d/step_2d_sw.h | 13 +- servers/physics_3d/body_3d_sw.cpp | 2 - servers/physics_3d/body_3d_sw.h | 8 - servers/physics_3d/body_pair_3d_sw.cpp | 10 +- servers/physics_3d/constraint_3d_sw.h | 8 - servers/physics_3d/step_3d_sw.cpp | 194 +++++++++++++------------ servers/physics_3d/step_3d_sw.h | 13 +- 11 files changed, 204 insertions(+), 253 deletions(-) diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index d0636047b7b1..0a91931354fe 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -658,8 +658,6 @@ Body2DSW::Body2DSW() : omit_force_integration = false; applied_torque = 0; island_step = 0; - island_next = nullptr; - island_list_next = nullptr; _set_static(false); first_time_kinematic = false; linear_damp = -1; diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 60d55ab8bdaf..7ea4ac697ce8 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -125,8 +125,6 @@ class Body2DSW : public CollisionObject2DSW { ForceIntegrationCallback *fi_callback; uint64_t island_step; - Body2DSW *island_next; - Body2DSW *island_list_next; _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area); @@ -175,12 +173,6 @@ class Body2DSW : public CollisionObject2DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Body2DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body2DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Body2DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body2DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } const List> &get_constraint_list() const { return constraint_list; } diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index 49ae4dd848d6..b724deb48e8f 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -37,8 +37,6 @@ class Constraint2DSW { Body2DSW **_body_ptr; int _body_count; uint64_t island_step; - Constraint2DSW *island_next; - Constraint2DSW *island_list_next; bool disabled_collisions_between_bodies; RID self; @@ -58,12 +56,6 @@ class Constraint2DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Constraint2DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Constraint2DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Constraint2DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Constraint2DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 6613d1972987..406d75077631 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -31,19 +31,23 @@ #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 + +void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; + p_body_island.push_back(p_body); - for (const List>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) { + // Faster with reversed iterations. + for (const List>::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 } c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; + p_constraint_island.push_back(c); for (int i = 0; i < c->get_body_count(); i++) { if (i == E->get().second) { @@ -53,78 +57,62 @@ void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constrain if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { continue; //no go } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + _populate_island(c->get_body_ptr()[i], 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_island(LocalVector &p_constraint_island, real_t p_delta) { + 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)) { + // 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) { +void Step2DSW::_solve_island(LocalVector &p_constraint_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(); + uint32_t constraint_count = p_constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + p_constraint_island[constraint_index]->solve(p_delta); } } } -void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { +void Step2DSW::_check_suspend(const LocalVector &p_body_island, real_t p_delta) { 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 (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { + continue; // Ignore for static. } - if (!b->sleep_test(p_delta)) { + if (!body->sleep_test(p_delta)) { can_sleep = false; } - - b = b->get_island_next(); } - //put all to sleep or wake up everyoen + // 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]; - 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 + if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { + continue; // Ignore for static. } - 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(); } } @@ -159,33 +147,43 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* GENERATE CONSTRAINT ISLANDS */ - Body2DSW *island_list = nullptr; - Constraint2DSW *constraint_island_list = nullptr; b = body_list->first(); - int island_count = 0; + uint32_t body_island_count = 0; + uint32_t 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); + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - island->set_island_list_next(island_list); - island_list = island; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; + _populate_island(body, body_island, constraint_island); + + body_islands.push_back(body_island); + + if (constraint_island.is_empty()) { + --island_count; } } b = b->next(); } - p_space->set_island_count(island_count); + p_space->set_island_count((int)island_count); const SelfList::List &aml = p_space->get_moved_area_list(); @@ -196,9 +194,13 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { continue; } c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.push_back(c); } p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here } @@ -211,39 +213,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* 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; - } - - ci = ci->get_island_list_next(); - } + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _setup_island(constraint_islands[island_index], p_delta); } { //profile @@ -254,13 +225,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* 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(); - } + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _solve_island(constraint_islands[island_index], p_iterations, p_delta); } { //profile @@ -280,12 +246,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], p_delta); } { //profile @@ -301,4 +263,7 @@ 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); } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 83b9130608e3..5af4a36f52b8 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -33,13 +33,18 @@ #include "space_2d_sw.h" +#include "core/templates/local_vector.h" + class Step2DSW { uint64_t _step; - void _populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island); - bool _setup_island(Constraint2DSW *p_island, real_t p_delta); - void _solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(Body2DSW *p_island, real_t p_delta); + LocalVector> body_islands; + LocalVector> constraint_islands; + + void _populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _setup_island(LocalVector &p_constraint_island, real_t p_delta); + void _solve_island(LocalVector &p_constraint_island, int p_iterations, real_t p_delta); + void _check_suspend(const LocalVector &p_body_island, real_t p_delta); public: void step(Space2DSW *p_space, real_t p_delta, int p_iterations); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 64ba0cb09d7a..cc414b7f303c 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -761,8 +761,6 @@ Body3DSW::Body3DSW() : omit_force_integration = false; //applied_torque=0; island_step = 0; - island_next = nullptr; - island_list_next = nullptr; first_time_kinematic = false; first_integration = false; _set_static(false); diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index e87ff2364bd3..5790f43019d0 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -135,8 +135,6 @@ class Body3DSW : public CollisionObject3DSW { ForceIntegrationCallback *fi_callback; uint64_t island_step; - Body3DSW *island_next; - Body3DSW *island_list_next; _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); @@ -189,12 +187,6 @@ class Body3DSW : public CollisionObject3DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Body3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Body3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } const Map &get_constraint_map() const { return constraint_map; } diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 36114c0c915c..30dc83bb41f6 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -281,6 +281,8 @@ bool BodyPair3DSW::setup(real_t p_step) { real_t inv_dt = 1.0 / p_step; + bool do_process = false; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; @@ -323,6 +325,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } c.active = true; + do_process = true; // Precompute normal mass, tangent mass, and bias. Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); @@ -350,7 +353,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodyPair3DSW::solve(real_t p_step) { @@ -594,6 +597,8 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { real_t inv_dt = 1.0 / p_step; + bool do_process = false; + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -614,6 +619,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } c.active = true; + do_process = true; #ifdef DEBUG_ENABLED @@ -661,7 +667,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodySoftBodyPair3DSW::solve(real_t p_step) { diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 2571335c432b..16a31e167dd7 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -37,8 +37,6 @@ class Constraint3DSW { Body3DSW **_body_ptr; int _body_count; uint64_t island_step; - Constraint3DSW *island_next; - Constraint3DSW *island_list_next; int priority; bool disabled_collisions_between_bodies; @@ -60,12 +58,6 @@ class Constraint3DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Constraint3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Constraint3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Constraint3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Constraint3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 2133a38670f3..06f3227eab60 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -33,19 +33,23 @@ #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 + +void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; + p_body_island.push_back(p_body); - for (Map::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { + // Faster with reversed iterations. + for (Map::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 } c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; + p_constraint_island.push_back(c); for (int i = 0; i < c->get_body_count(); i++) { if (i == E->get()) { @@ -55,87 +59,79 @@ void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constrain if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { continue; //no go } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + _populate_island(c->get_body_ptr()[i], 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::_setup_island(LocalVector &p_constraint_island, real_t p_delta) { + 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)) { + // 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(LocalVector &p_constraint_island, int p_iterations, real_t p_delta) { + int current_priority = 1; - while (p_island) { + uint32_t constraint_count = p_constraint_island.size(); + while (constraint_count > 0) { for (int i = 0; i < p_iterations; i++) { - Constraint3DSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); + // Go through all iterations. + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + p_constraint_island[constraint_index]->solve(p_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 = p_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_count = priority_constraint_count; } } -void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { +void Step3DSW::_check_suspend(const LocalVector &p_body_island, real_t p_delta) { 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 (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { + continue; // Ignore for static. } - if (!b->sleep_test(p_delta)) { + if (!body->sleep_test(p_delta)) { can_sleep = false; } - - b = b->get_island_next(); } - //put all to sleep or wake up everyoen + // 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]; - 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 + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { + continue; // Ignore for static. } - 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(); } } @@ -181,33 +177,43 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* GENERATE CONSTRAINT ISLANDS */ - Body3DSW *island_list = nullptr; - Constraint3DSW *constraint_island_list = nullptr; b = body_list->first(); - int island_count = 0; + uint32_t body_island_count = 0; + uint32_t 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); + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - island->set_island_list_next(island_list); - island_list = island; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; + _populate_island(body, body_island, constraint_island); + + body_islands.push_back(body_island); + + if (constraint_island.is_empty()) { + --island_count; } } b = b->next(); } - p_space->set_island_count(island_count); + p_space->set_island_count((int)island_count); const SelfList::List &aml = p_space->get_moved_area_list(); @@ -218,9 +224,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { continue; } c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.push_back(c); } p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here } @@ -233,9 +243,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { continue; } c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.push_back(c); } sb = sb->next(); } @@ -248,12 +262,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* SETUP CONSTRAINT ISLANDS */ - { - Constraint3DSW *ci = constraint_island_list; - while (ci) { - _setup_island(ci, p_delta); - ci = ci->get_island_list_next(); - } + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _setup_island(constraint_islands[island_index], p_delta); } { //profile @@ -264,13 +274,10 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* 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(); - } + 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); } { //profile @@ -290,12 +297,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], p_delta); } /* UPDATE SOFT BODY CONSTRAINTS */ @@ -319,4 +322,7 @@ 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); } diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 55c48ec0eb99..f406c35c3ad3 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -33,13 +33,18 @@ #include "space_3d_sw.h" +#include "core/templates/local_vector.h" + class Step3DSW { uint64_t _step; - void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island); - void _setup_island(Constraint3DSW *p_island, real_t p_delta); - void _solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(Body3DSW *p_island, real_t p_delta); + LocalVector> body_islands; + LocalVector> constraint_islands; + + void _populate_island(Body3DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _setup_island(LocalVector &p_constraint_island, real_t p_delta); + void _solve_island(LocalVector &p_constraint_island, int p_iterations, real_t p_delta); + void _check_suspend(const LocalVector &p_body_island, real_t p_delta); public: void step(Space3DSW *p_space, real_t p_delta, int p_iterations);