make 2d constraint solving more deterministic by solving in push order

7 files changed, 23 insertions, 21 deletions

diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index d7bceb9f02..b1589b203f 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() {
 			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 75c9a95739..a3eaff9c7f 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -562,13 +562,13 @@ void Body2DSW::integrate_velocities(real_t p_step) {
 }
 
 void Body2DSW::wakeup_neighbours() {
-	for (Map<Constraint2DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) {
-		const Constraint2DSW *c = E->key();
+	for (List<Pair<Constraint2DSW *, int>>::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];
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index bbc22a67df..19e4b92a99 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/templates/list.h"
+#include "core/templates/pair.h"
 #include "core/templates/vset.h"
 
 class Constraint2DSW;
@@ -83,7 +85,7 @@ class Body2DSW : public CollisionObject2DSW {
 	virtual void _shapes_changed();
 	Transform2D new_transform;
 
-	Map<Constraint2DSW *, int> constraint_map;
+	List<Pair<Constraint2DSW *, int>> constraint_list;
 
 	struct AreaCMP {
 		Area2DSW *area;
@@ -179,10 +181,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<Constraint2DSW *, int> &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<Pair<Constraint2DSW *, int>> &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 2021aab17c..bb6629becb 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -514,6 +514,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 e7d26645e9..743f69d7d4 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -199,10 +199,10 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
 
 PinJoint2DSW::~PinJoint2DSW() {
 	if (A) {
-		A->remove_constraint(this);
+		A->remove_constraint(this, 0);
 	}
 	if (B) {
-		B->remove_constraint(this);
+		B->remove_constraint(this, 1);
 	}
 }
 
@@ -339,8 +339,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);
 }
 
 //////////////////////////////////////////////
@@ -436,6 +436,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_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 755804fe36..223fd0114a 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -554,7 +554,7 @@ void PhysicsServer2DSW::body_set_space(RID p_body, RID p_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 c7711bcd1d..56b31a884d 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -36,8 +36,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<Constraint2DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
-		Constraint2DSW *c = (Constraint2DSW *)E->key();
+	for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) {
+		Constraint2DSW *c = (Constraint2DSW *)E->get().first;
 		if (c->get_island_step() == _step) {
 			continue; //already processed
 		}
@@ -46,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];