summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--servers/physics_2d/body_2d_sw.cpp2
-rw-r--r--servers/physics_2d/body_2d_sw.h8
-rw-r--r--servers/physics_2d/constraint_2d_sw.h8
-rw-r--r--servers/physics_2d/step_2d_sw.cpp191
-rw-r--r--servers/physics_2d/step_2d_sw.h13
-rw-r--r--servers/physics_3d/body_3d_sw.cpp2
-rw-r--r--servers/physics_3d/body_3d_sw.h8
-rw-r--r--servers/physics_3d/body_pair_3d_sw.cpp10
-rw-r--r--servers/physics_3d/constraint_3d_sw.h8
-rw-r--r--servers/physics_3d/step_3d_sw.cpp194
-rw-r--r--servers/physics_3d/step_3d_sw.h13
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 d0636047b7..0a91931354 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 60d55ab8bd..7ea4ac697c 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 @@ public:
_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<Pair<Constraint2DSW *, int>> &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 49ae4dd848..b724deb48e 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 @@ public:
_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 6613d19729..406d750776 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<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;
+ 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()) {
+ // 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
}
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<Constraint2DSW *> &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<Constraint2DSW *> &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<Body2DSW *> &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<Body2DSW *> &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<Constraint2DSW *> &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<Area2DSW>::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<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
}
@@ -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 83b9130608..5af4a36f52 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<LocalVector<Body2DSW *>> body_islands;
+ LocalVector<LocalVector<Constraint2DSW *>> constraint_islands;
+
+ void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island);
+ void _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta);
+ void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
+ void _check_suspend(const LocalVector<Body2DSW *> &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 64ba0cb09d..cc414b7f30 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 e87ff2364b..5790f43019 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 @@ public:
_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<Constraint3DSW *, int> &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 ef4dca7dcf..28c854466f 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 2571335c43..16a31e167d 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 @@ public:
_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 2133a38670..06f3227eab 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<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;
+ p_body_island.push_back(p_body);
- for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
+ // Faster with reversed iterations.
+ for (Map<Constraint3DSW *, int>::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<Constraint3DSW *> &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<Constraint3DSW *> &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<Body3DSW *> &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<Body3DSW *> &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<Constraint3DSW *> &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<Area3DSW>::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<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+ constraint_island.clear();
+ constraint_island.push_back(c);
}
p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)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<Constraint3DSW *> &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 55c48ec0eb..f406c35c3a 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<LocalVector<Body3DSW *>> body_islands;
+ LocalVector<LocalVector<Constraint3DSW *>> constraint_islands;
+
+ void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
+ void _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta);
+ void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
+ void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta);
public:
void step(Space3DSW *p_space, real_t p_delta, int p_iterations);
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 03:28:19 +0000