Deterministic constraint map in Godot Physics solver

- Common constraint map in CollisionObject instead of separate ones in
rigid body, soft body, area
- Removed index storage in rigid body constraints (it was only used to
test bodies from a constraint against a specific body)
- Added incremental constraint id as key in constraint map to help with
determinism
- In 2D, this system replaces the previous List implementation which was
making the solver slower (by 30% in some cases)

servers/physics_2d/area_2d_sw.h

@@ -34,11 +34,9 @@
 #include "collision_object_2d_sw.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_2d.h"
-//#include "servers/physics_3d/query_sw.h"
 
 class Space2DSW;
 class Body2DSW;
-class Constraint2DSW;
 
 class Area2DSW : public CollisionObject2DSW {
 	PhysicsServer2D::AreaSpaceOverrideMode space_override_mode;
@@ -94,17 +92,10 @@ class Area2DSW : public CollisionObject2DSW {
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
-	//virtual void shape_changed_notify(Shape2DSW *p_shape);
-	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
-	Set<Constraint2DSW *> constraints;
-
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
 public:
-	//_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; }
-	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
-
 	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); }
 
@@ -147,11 +138,6 @@ class Area2DSW : public CollisionObject2DSW {
 	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
-	_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); }
-	_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); }
-	_FORCE_INLINE_ const Set<Constraint2DSW *> &get_constraints() const { return constraints; }
-	_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
-
 	void set_monitorable(bool p_monitorable);
 	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 

servers/physics_2d/area_pair_2d_sw.cpp

@@ -89,7 +89,7 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area,
 	area = p_area;
 	body_shape = p_body_shape;
 	area_shape = p_area_shape;
-	body->add_constraint(this, 0);
+	body->add_constraint(this);
 	area->add_constraint(this);
 	if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
 		p_body->set_active(true);
@@ -105,7 +105,7 @@ AreaPair2DSW::~AreaPair2DSW() {
 			area->remove_body_from_query(body, body_shape, area_shape);
 		}
 	}
-	body->remove_constraint(this, 0);
+	body->remove_constraint(this);
 	area->remove_constraint(this);
 }
 

servers/physics_2d/body_2d_sw.cpp

@@ -541,22 +541,24 @@ void Body2DSW::integrate_velocities(real_t p_step) {
 }
 
 void Body2DSW::wakeup_neighbours() {
-	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 (const CollisionObject2DSW::T_ConstraintMap::Element *E = get_constraint_map().front(); E; E = E->next()) {
+		const Constraint2DSW *constraint = E->get();
 
-		for (int i = 0; i < bc; i++) {
-			if (i == E->get().second) {
+		Body2DSW **bodies = constraint->get_body_ptr();
+		int body_count = constraint->get_body_count();
+		for (int i = 0; i < body_count; i++) {
+			Body2DSW *body = bodies[i];
+
+			if (body == this) {
 				continue;
 			}
-			Body2DSW *b = n[i];
-			if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) {
+
+			if (body->mode != PhysicsServer2D::BODY_MODE_RIGID) {
 				continue;
 			}
 
-			if (!b->is_active()) {
-				b->set_active(true);
+			if (!body->is_active()) {
+				body->set_active(true);
 			}
 		}
 	}

servers/physics_2d/body_2d_sw.h

@@ -33,12 +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;
-
 class Body2DSW : public CollisionObject2DSW {
 	PhysicsServer2D::BodyMode mode;
 
@@ -85,8 +81,6 @@ class Body2DSW : public CollisionObject2DSW {
 	virtual void _shapes_changed();
 	Transform2D new_transform;
 
-	List<Pair<Constraint2DSW *, int>> constraint_list;
-
 	struct AreaCMP {
 		Area2DSW *area;
 		int refCount;
@@ -172,11 +166,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_ 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; }
 

servers/physics_2d/body_pair_2d_sw.cpp

@@ -534,11 +534,11 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh
 	shape_A = p_shape_A;
 	shape_B = p_shape_B;
 	space = A->get_space();
-	A->add_constraint(this, 0);
-	B->add_constraint(this, 1);
+	A->add_constraint(this);
+	B->add_constraint(this);
 }
 
 BodyPair2DSW::~BodyPair2DSW() {
-	A->remove_constraint(this, 0);
-	B->remove_constraint(this, 1);
+	A->remove_constraint(this);
+	B->remove_constraint(this);
 }

servers/physics_2d/collision_object_2d_sw.h

@@ -32,6 +32,8 @@
 #define COLLISION_OBJECT_2D_SW_H
 
 #include "broad_phase_2d_sw.h"
+#include "constraint_2d_sw.h"
+#include "core/templates/map.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_2d.h"
 #include "shape_2d_sw.h"
@@ -45,6 +47,8 @@ class CollisionObject2DSW : public ShapeOwner2DSW {
 		TYPE_BODY
 	};
 
+	typedef Map<uint32_t, Constraint2DSW *> T_ConstraintMap;
+
 private:
 	Type type;
 	RID self;
@@ -77,6 +81,8 @@ class CollisionObject2DSW : public ShapeOwner2DSW {
 	uint32_t collision_layer;
 	bool _static;
 
+	T_ConstraintMap constraint_map;
+
 	SelfList<CollisionObject2DSW> pending_shape_update_list;
 
 	void _update_shapes();
@@ -183,6 +189,11 @@ class CollisionObject2DSW : public ShapeOwner2DSW {
 	void remove_shape(Shape2DSW *p_shape);
 	void remove_shape(int p_index);
 
+	_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraint_map.insert(p_constraint->get_constraint_id(), p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint->get_constraint_id()); }
+	const T_ConstraintMap &get_constraint_map() const { return constraint_map; }
+	_FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); }
+
 	virtual void set_space(Space2DSW *p_space) = 0;
 
 	_FORCE_INLINE_ bool is_static() const { return _static; }

servers/physics_2d/constraint_2d_sw.cpp

@@ -0,0 +1,33 @@
+/*************************************************************************/
+/*  constraint_2d_sw.cpp                                                 */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "constraint_2d_sw.h"
+
+SafeNumeric<uint32_t> Constraint2DSW::constraint_id_counter;

servers/physics_2d/constraint_2d_sw.h

@@ -31,7 +31,11 @@
 #ifndef CONSTRAINT_2D_SW_H
 #define CONSTRAINT_2D_SW_H
 
-#include "body_2d_sw.h"
+#include "core/math/math_defs.h"
+#include "core/templates/rid.h"
+#include "core/templates/safe_refcount.h"
+
+class Body2DSW;
 
 class Constraint2DSW {
 	Body2DSW **_body_ptr;
@@ -41,18 +45,24 @@ class Constraint2DSW {
 
 	RID self;
 
+	static SafeNumeric<uint32_t> constraint_id_counter;
+	uint32_t constraint_id = 0;
+
 protected:
 	Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) {
 		_body_ptr = p_body_ptr;
 		_body_count = p_body_count;
 		island_step = 0;
 		disabled_collisions_between_bodies = true;
+		constraint_id = constraint_id_counter.increment();
 	}
 
 public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
+	_FORCE_INLINE_ uint32_t get_constraint_id() const { return constraint_id; }
+
 	_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; }
 

servers/physics_2d/joints_2d_sw.cpp

@@ -215,9 +215,9 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
 
 	softness = 0;
 
-	p_body_a->add_constraint(this, 0);
+	p_body_a->add_constraint(this);
 	if (p_body_b) {
-		p_body_b->add_constraint(this, 1);
+		p_body_b->add_constraint(this);
 	}
 }
 
@@ -370,8 +370,8 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
 	B_anchor = B->get_inv_transform().xform(p_b_anchor);
 	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal();
 
-	A->add_constraint(this, 0);
-	B->add_constraint(this, 1);
+	A->add_constraint(this);
+	B->add_constraint(this);
 }
 
 //////////////////////////////////////////////
@@ -482,6 +482,6 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve
 	stiffness = 20;
 	damping = 1.5;
 
-	A->add_constraint(this, 0);
-	B->add_constraint(this, 1);
+	A->add_constraint(this);
+	B->add_constraint(this);
 }

servers/physics_2d/joints_2d_sw.h

@@ -70,7 +70,7 @@ class Joint2DSW : public Constraint2DSW {
 		for (int i = 0; i < get_body_count(); i++) {
 			Body2DSW *body = get_body_ptr()[i];
 			if (body) {
-				body->remove_constraint(this, i);
+				body->remove_constraint(this);
 			}
 		}
 	};

servers/physics_2d/physics_server_2d_sw.cpp

@@ -304,7 +304,7 @@ void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) {
 		return; //pointless
 	}
 
-	area->clear_constraints();
+	area->clear_constraint_map();
 	area->set_space(space);
 };
 
@@ -548,7 +548,7 @@ void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) {
 		return; //pointless
 	}
 
-	body->clear_constraint_list();
+	body->clear_constraint_map();
 	body->set_space(space);
 };
 

servers/physics_2d/step_2d_sw.cpp

@@ -46,26 +46,30 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
 		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()) {
-		Constraint2DSW *constraint = (Constraint2DSW *)E->get().first;
+	for (const CollisionObject2DSW::T_ConstraintMap::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
+		Constraint2DSW *constraint = E->get();
 		if (constraint->get_island_step() == _step) {
 			continue; // Already processed.
 		}
 		constraint->set_island_step(_step);
 		p_constraint_island.push_back(constraint);
+
 		all_constraints.push_back(constraint);
 
-		for (int i = 0; i < constraint->get_body_count(); i++) {
-			if (i == E->get().second) {
-				continue;
-			}
-			Body2DSW *other_body = constraint->get_body_ptr()[i];
+		// Find connected rigid bodies.
+		Body2DSW **bodies = constraint->get_body_ptr();
+		int body_count = constraint->get_body_count();
+		for (int i = 0; i < body_count; i++) {
+			Body2DSW *other_body = bodies[i];
+
 			if (other_body->get_island_step() == _step) {
-				continue; // Already processed.
+				continue; // Already processed (can be this body).
 			}
+
 			if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
 				continue; // Static bodies don't connect islands.
 			}
+
 			_populate_island(other_body, p_body_island, p_constraint_island);
 		}
 	}
@@ -163,7 +167,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
 
 	while (aml.first()) {
-		for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+		for (const CollisionObject2DSW::T_ConstraintMap::Element *E = aml.first()->self()->get_constraint_map().front(); E; E = E->next()) {
 			Constraint2DSW *constraint = E->get();
 			if (constraint->get_island_step() == _step) {
 				continue;