summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_2d_sw.cpp18
-rw-r--r--servers/physics_2d/body_2d_sw.h13
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.cpp79
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.h11
-rw-r--r--servers/physics_2d/constraint_2d_sw.h8
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp13
-rw-r--r--servers/physics_2d/physics_server_2d_sw.cpp4
-rw-r--r--servers/physics_2d/physics_server_2d_sw.h2
-rw-r--r--servers/physics_2d/physics_server_2d_wrap_mt.h2
-rw-r--r--servers/physics_2d/step_2d_sw.cpp191
-rw-r--r--servers/physics_2d/step_2d_sw.h13
11 files changed, 164 insertions, 190 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index d0636047b7..9306fea70c 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -591,16 +591,17 @@ void Body2DSW::call_queries() {
Variant v = dbs;
const Variant *vp[2] = { &v, &fi_callback->callback_udata };
- Object *obj = ObjectDB::get_instance(fi_callback->id);
+ Object *obj = fi_callback->callable.get_object();
if (!obj) {
- set_force_integration_callback(ObjectID(), StringName());
+ set_force_integration_callback(Callable());
} else {
Callable::CallError ce;
+ Variant rv;
if (fi_callback->callback_udata.get_type() != Variant::NIL) {
- obj->call(fi_callback->method, vp, 2, ce);
+ fi_callback->callable.call(vp, 2, rv, ce);
} else {
- obj->call(fi_callback->method, vp, 1, ce);
+ fi_callback->callable.call(vp, 1, rv, ce);
}
}
}
@@ -625,16 +626,15 @@ bool Body2DSW::sleep_test(real_t p_step) {
}
}
-void Body2DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
+void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (fi_callback) {
memdelete(fi_callback);
fi_callback = nullptr;
}
- if (p_id.is_valid()) {
+ if (p_callable.get_object()) {
fi_callback = memnew(ForceIntegrationCallback);
- fi_callback->id = p_id;
- fi_callback->method = p_method;
+ fi_callback->callable = p_callable;
fi_callback->callback_udata = p_udata;
}
}
@@ -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..b4a95651cb 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -117,23 +117,20 @@ class Body2DSW : public CollisionObject2DSW {
int contact_count;
struct ForceIntegrationCallback {
- ObjectID id;
- StringName method;
+ Callable callable;
Variant callback_udata;
};
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);
friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose
public:
- void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
+ void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
_FORCE_INLINE_ void add_area(Area2DSW *p_area) {
int index = areas.find(AreaCMP(p_area));
@@ -175,12 +172,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/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 6cfe6908d1..35447c5389 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -35,6 +35,12 @@
#define LARGE_ELEMENT_FI 1.01239812
void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {
+ if (p_elem->owner == p_with->owner) {
+ return;
+ }
+ if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
+ return;
+ }
Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
ERR_FAIL_COND(p_elem->_static && p_with->_static);
@@ -49,6 +55,12 @@ void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {
}
void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {
+ if (p_elem->owner == p_with->owner) {
+ return;
+ }
+ if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
+ return;
+ }
Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
ERR_FAIL_COND(!E); //this should really be paired..
@@ -74,24 +86,22 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
bool physical_collision = p_elem->aabb.intersects(E->key()->aabb);
bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner);
- if (physical_collision) {
- if (!E->get()->colliding || (logical_collision && !E->get()->ud && pair_callback)) {
+ if (physical_collision && logical_collision) {
+ if (!E->get()->colliding && pair_callback) {
E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
- } else if (E->get()->colliding && !logical_collision && E->get()->ud && unpair_callback) {
- unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
- E->get()->ud = nullptr;
}
E->get()->colliding = true;
- } else { // No physcial_collision
+ } else { // No collision
if (E->get()->colliding && unpair_callback) {
unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
+ E->get()->ud = nullptr;
}
E->get()->colliding = false;
}
}
}
-void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
+void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter) {
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
if (sz.width * sz.height > large_object_min_surface) {
//large object, do not use grid, must check against all elements
@@ -99,9 +109,6 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo
if (E->key() == p_elem->self) {
continue; // do not pair against itself
}
- if (E->get().owner == p_elem->owner) {
- continue;
- }
if (E->get()._static && p_static) {
continue;
}
@@ -133,7 +140,7 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo
pb = pb->next;
}
- bool entered = false;
+ bool entered = p_force_enter;
if (!pb) {
//does not exist, create!
@@ -155,17 +162,11 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo
if (entered) {
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
_pair_attempt(p_elem, E->key());
}
if (!p_static) {
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
_pair_attempt(p_elem, E->key());
}
}
@@ -179,18 +180,14 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo
if (E->key() == p_elem) {
continue; // do not pair against itself
}
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
if (E->key()->_static && p_static) {
continue;
}
-
_pair_attempt(E->key(), p_elem);
}
}
-void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
+void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit) {
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
if (sz.width * sz.height > large_object_min_surface) {
//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
@@ -229,7 +226,7 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool
ERR_CONTINUE(!pb); //should exist!!
- bool exited = false;
+ bool exited = p_force_exit;
if (p_static) {
if (pb->static_object_set[p_elem].dec() == 0) {
@@ -245,17 +242,11 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool
if (exited) {
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
_unpair_attempt(p_elem, E->key());
}
if (!p_static) {
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
_unpair_attempt(p_elem, E->key());
}
}
@@ -288,9 +279,6 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool
if (E->key() == p_elem) {
continue; // do not pair against itself
}
- if (E->key()->owner == p_elem->owner) {
- continue;
- }
if (E->key()->_static && p_static) {
continue;
}
@@ -306,6 +294,8 @@ BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_obj
Element e;
e.owner = p_object;
e._static = false;
+ e.collision_mask = p_object->get_collision_mask();
+ e.collision_layer = p_object->get_collision_layer();
e.subindex = p_subindex;
e.self = current;
e.pass = 0;
@@ -319,13 +309,26 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
ERR_FAIL_COND(!E);
Element &e = E->get();
+ bool layer_changed = e.collision_mask != e.owner->get_collision_mask() || e.collision_layer != e.owner->get_collision_layer();
- if (p_aabb != e.aabb) {
+ if (p_aabb != e.aabb || layer_changed) {
+ uint32_t old_mask = e.collision_mask;
+ uint32_t old_layer = e.collision_layer;
if (p_aabb != Rect2()) {
- _enter_grid(&e, p_aabb, e._static);
+ e.collision_mask = e.owner->get_collision_mask();
+ e.collision_layer = e.owner->get_collision_layer();
+
+ _enter_grid(&e, p_aabb, e._static, layer_changed);
}
if (e.aabb != Rect2()) {
- _exit_grid(&e, e.aabb, e._static);
+ // Need _exit_grid to remove from cells based on the old layer values.
+ e.collision_mask = old_mask;
+ e.collision_layer = old_layer;
+
+ _exit_grid(&e, e.aabb, e._static, layer_changed);
+
+ e.collision_mask = e.owner->get_collision_mask();
+ e.collision_layer = e.owner->get_collision_layer();
}
e.aabb = p_aabb;
}
@@ -344,13 +347,13 @@ void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
}
if (e.aabb != Rect2()) {
- _exit_grid(&e, e.aabb, e._static);
+ _exit_grid(&e, e.aabb, e._static, false);
}
e._static = p_static;
if (e.aabb != Rect2()) {
- _enter_grid(&e, e.aabb, e._static);
+ _enter_grid(&e, e.aabb, e._static, false);
_check_motion(&e);
}
}
@@ -362,7 +365,7 @@ void BroadPhase2DHashGrid::remove(ID p_id) {
Element &e = E->get();
if (e.aabb != Rect2()) {
- _exit_grid(&e, e.aabb, e._static);
+ _exit_grid(&e, e.aabb, e._static, false);
}
element_map.erase(p_id);
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
index eb7c8879ac..bb7c03b989 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.h
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.h
@@ -51,6 +51,9 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
CollisionObject2DSW *owner;
bool _static;
Rect2 aabb;
+ // Owner's collision_mask/layer, used to detect changes in layers.
+ uint32_t collision_mask;
+ uint32_t collision_layer;
int subindex;
uint64_t pass;
Map<Element *, PairData *> paired;
@@ -115,8 +118,12 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
UnpairCallback unpair_callback;
void *unpair_userdata;
- void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static);
- void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static);
+ static _FORCE_INLINE_ bool _test_collision_mask(uint32_t p_mask1, uint32_t p_layer1, uint32_t p_mask2, uint32_t p_layer2) {
+ return p_mask1 & p_layer2 || p_mask2 & p_layer1;
+ }
+
+ void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter);
+ void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit);
template <bool use_aabb, bool use_segment>
_FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index);
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/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index c7b556deba..20d4b9aa1a 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -97,8 +97,13 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
}
bool PinJoint2DSW::setup(real_t p_step) {
+ if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ return false;
+ }
+
Space2DSW *space = A->get_space();
ERR_FAIL_COND_V(!space, false);
+
rA = A->get_transform().basis_xform(anchor_A);
rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
@@ -257,6 +262,10 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
}
bool GrooveJoint2DSW::setup(real_t p_step) {
+ if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ return false;
+ }
+
// calculate endpoints in worldspace
Vector2 ta = A->get_transform().xform(A_groove_1);
Vector2 tb = A->get_transform().xform(A_groove_2);
@@ -342,6 +351,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
//////////////////////////////////////////////
bool DampedSpringJoint2DSW::setup(real_t p_step) {
+ if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ return false;
+ }
+
rA = A->get_transform().basis_xform(anchor_A);
rB = B->get_transform().basis_xform(anchor_B);
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 1040437ca7..6d64f4126c 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -927,10 +927,10 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const {
return body->get_max_contacts_reported();
}
-void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
+void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
- body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata);
+ body->set_force_integration_callback(p_callable, p_udata);
}
bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) {
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index 65c5df0fce..efa0784245 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/servers/physics_2d/physics_server_2d_sw.h
@@ -242,7 +242,7 @@ public:
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
virtual int body_get_max_contacts_reported(RID p_body) const override;
- virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override;
+ virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
virtual void body_set_pickable(RID p_body, bool p_pickable) override;
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index 3577f706de..88ac742e40 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.h
+++ b/servers/physics_2d/physics_server_2d_wrap_mt.h
@@ -245,7 +245,7 @@ public:
FUNC2(body_set_omit_force_integration, RID, bool);
FUNC1RC(bool, body_is_omitting_force_integration, RID);
- FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &);
+ FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override {
return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_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);
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-03 19:29:03 +0000