From 266314ba26c1b9591d5da48f1656335b22528b60 Mon Sep 17 00:00:00 2001 From: Jordan Schidlowsky Date: Fri, 4 Dec 2020 22:12:47 -0600 Subject: [PATCH] make 2d constraint solving more deterministic by solving in push order (cherry picked from commit 043b6c2d93a560209fcd59fbff6de083659835bc) --- servers/physics_2d/area_pair_2d_sw.cpp | 2 +- servers/physics_2d/body_2d_sw.cpp | 9 +++------ servers/physics_2d/body_2d_sw.h | 12 +++++++----- servers/physics_2d/body_pair_2d_sw.cpp | 5 ++--- servers/physics_2d/joints_2d_sw.cpp | 21 ++++++++++----------- servers/physics_2d/physics_2d_server_sw.cpp | 2 +- servers/physics_2d/step_2d_sw.cpp | 7 +++---- 7 files changed, 27 insertions(+), 31 deletions(-) diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index fb4113d6383..54991f05e3f 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -89,7 +89,7 @@ AreaPair2DSW::~AreaPair2DSW() { if (area->has_monitor_callback()) area->remove_body_from_query(body, body_shape, area_shape); } - body->remove_constraint(this); + body->remove_constraint(this, 0); area->remove_constraint(this); } diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index b4f837b3557..9cb22563545 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -576,16 +576,13 @@ void Body2DSW::integrate_velocities(real_t p_step) { } void Body2DSW::wakeup_neighbours() { - - for (Map::Element *E = constraint_map.front(); E; E = E->next()) { - - const Constraint2DSW *c = E->key(); + for (List >::Element *E = constraint_list.front(); E; E = E->next()) { + const Constraint2DSW *c = E->get().first; Body2DSW **n = c->get_body_ptr(); int bc = c->get_body_count(); for (int i = 0; i < bc; i++) { - - if (i == E->get()) + if (i == E->get().second) continue; Body2DSW *b = n[i]; if (b->mode != Physics2DServer::BODY_MODE_RIGID) diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 6f3a2b9ab7c..1755b411106 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -33,6 +33,8 @@ #include "area_2d_sw.h" #include "collision_object_2d_sw.h" +#include "core/list.h" +#include "core/pair.h" #include "core/vset.h" class Constraint2DSW; @@ -84,7 +86,7 @@ class Body2DSW : public CollisionObject2DSW { virtual void _shapes_changed(); Transform2D new_transform; - Map constraint_map; + List > constraint_list; struct AreaCMP { @@ -180,10 +182,10 @@ public: _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_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint); } - const Map &get_constraint_map() const { return constraint_map; } - _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + _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; } + _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 52f10d9a09d..282cc60824f 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -516,7 +516,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh } BodyPair2DSW::~BodyPair2DSW() { - - A->remove_constraint(this); - B->remove_constraint(this); + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); } diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 0a1a38f9a5f..f1dc872b8f3 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -204,11 +204,12 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p } PinJoint2DSW::~PinJoint2DSW() { - - if (A) - A->remove_constraint(this); - if (B) - B->remove_constraint(this); + if (A) { + A->remove_constraint(this, 0); + } + if (B) { + B->remove_constraint(this, 1); + } } ////////////////////////////////////////////// @@ -350,9 +351,8 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ } GrooveJoint2DSW::~GrooveJoint2DSW() { - - A->remove_constraint(this); - B->remove_constraint(this); + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); } ////////////////////////////////////////////// @@ -463,7 +463,6 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve } DampedSpringJoint2DSW::~DampedSpringJoint2DSW() { - - A->remove_constraint(this); - B->remove_constraint(this); + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); } diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index dd1e78d094e..8bc6e731e6d 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -598,7 +598,7 @@ void Physics2DServerSW::body_set_space(RID p_body, RID p_space) { if (body->get_space() == space) return; //pointless - body->clear_constraint_map(); + body->clear_constraint_list(); body->set_space(space); }; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 05005b0fe06..b5e88c70da9 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -37,9 +37,8 @@ void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constrain p_body->set_island_next(*p_island); *p_island = p_body; - for (Map::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - - Constraint2DSW *c = (Constraint2DSW *)E->key(); + for (const List >::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 c->set_island_step(_step); @@ -47,7 +46,7 @@ void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constrain *p_constraint_island = c; for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) + if (i == E->get().second) continue; Body2DSW *b = c->get_body_ptr()[i]; if (b->get_island_step() == _step || b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC)