19 files changed, 521 insertions, 386 deletions

diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 6485c8d1e9..532cb259b3 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -215,7 +215,9 @@ void Area2DSW::call_queries() {
 
 		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
 			if (E->get().state == 0) { // Nothing happened
-				E = E->next();
+				Map<BodyKey, BodyState>::Element *next = E->next();
+				monitored_bodies.erase(E);
+				E = next;
 				continue;
 			}
 
@@ -250,7 +252,9 @@ void Area2DSW::call_queries() {
 
 		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
 			if (E->get().state == 0) { // Nothing happened
-				E = E->next();
+				Map<BodyKey, BodyState>::Element *next = E->next();
+				monitored_areas.erase(E);
+				E = next;
 				continue;
 			}
 
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index 21ad57e344..eb9fc0e800 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -40,31 +40,48 @@ bool AreaPair2DSW::setup(real_t p_step) {
 		result = true;
 	}
 
+	process_collision = false;
 	if (result != colliding) {
-		if (result) {
-			if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
-				body->add_area(area);
-			}
-			if (area->has_monitor_callback()) {
-				area->add_body_to_query(body, body_shape, area_shape);
-			}
-
-		} else {
-			if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
-				body->remove_area(area);
-			}
-			if (area->has_monitor_callback()) {
-				area->remove_body_from_query(body, body_shape, area_shape);
-			}
+		if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			process_collision = true;
+		} else if (area->has_monitor_callback()) {
+			process_collision = true;
 		}
 
 		colliding = result;
 	}
 
-	return false; //never do any post solving
+	return process_collision;
+}
+
+bool AreaPair2DSW::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			body->add_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->add_body_to_query(body, body_shape, area_shape);
+		}
+	} else {
+		if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			body->remove_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->remove_body_from_query(body, body_shape, area_shape);
+		}
+	}
+
+	return false; // Never do any post solving.
 }
 
 void AreaPair2DSW::solve(real_t p_step) {
+	// Nothing to do.
 }
 
 AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) {
@@ -72,7 +89,6 @@ 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;
-	colliding = false;
 	body->add_constraint(this, 0);
 	area->add_constraint(this);
 	if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
@@ -103,33 +119,48 @@ bool Area2Pair2DSW::setup(real_t p_step) {
 		result = true;
 	}
 
+	process_collision = false;
 	if (result != colliding) {
-		if (result) {
-			if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
-				area_b->add_area_to_query(area_a, shape_a, shape_b);
-			}
-
-			if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
-				area_a->add_area_to_query(area_b, shape_b, shape_a);
-			}
-
-		} else {
-			if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
-				area_b->remove_area_from_query(area_a, shape_a, shape_b);
-			}
-
-			if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
-				area_a->remove_area_from_query(area_b, shape_b, shape_a);
-			}
+		if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+			process_collision = true;
+		} else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+			process_collision = true;
 		}
 
 		colliding = result;
 	}
 
-	return false; //never do any post solving
+	return process_collision;
+}
+
+bool Area2Pair2DSW::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+			area_b->add_area_to_query(area_a, shape_a, shape_b);
+		}
+
+		if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+			area_a->add_area_to_query(area_b, shape_b, shape_a);
+		}
+	} else {
+		if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+			area_b->remove_area_from_query(area_a, shape_a, shape_b);
+		}
+
+		if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+			area_a->remove_area_from_query(area_b, shape_b, shape_a);
+		}
+	}
+
+	return false; // Never do any post solving.
 }
 
 void Area2Pair2DSW::solve(real_t p_step) {
+	// Nothing to do.
 }
 
 Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) {
@@ -137,7 +168,6 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area
 	area_b = p_area_b;
 	shape_a = p_shape_a;
 	shape_b = p_shape_b;
-	colliding = false;
 	area_a->add_constraint(this);
 	area_b->add_constraint(this);
 }
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
index 4015aad5d1..4632a307d9 100644
--- a/servers/physics_2d/area_pair_2d_sw.h
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -36,30 +36,34 @@
 #include "constraint_2d_sw.h"
 
 class AreaPair2DSW : public Constraint2DSW {
-	Body2DSW *body;
-	Area2DSW *area;
-	int body_shape;
-	int area_shape;
-	bool colliding;
+	Body2DSW *body = nullptr;
+	Area2DSW *area = nullptr;
+	int body_shape = 0;
+	int area_shape = 0;
+	bool colliding = false;
+	bool process_collision = false;
 
 public:
-	bool setup(real_t p_step);
-	void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape);
 	~AreaPair2DSW();
 };
 
 class Area2Pair2DSW : public Constraint2DSW {
-	Area2DSW *area_a;
-	Area2DSW *area_b;
-	int shape_a;
-	int shape_b;
-	bool colliding;
+	Area2DSW *area_a = nullptr;
+	Area2DSW *area_b = nullptr;
+	int shape_a = 0;
+	int shape_b = 0;
+	bool colliding = false;
+	bool process_collision = false;
 
 public:
-	bool setup(real_t p_step);
-	void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b);
 	~Area2Pair2DSW();
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index d0636047b7..f78a487e27 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -104,31 +104,17 @@ void Body2DSW::set_active(bool p_active) {
 	}
 
 	active = p_active;
-	if (!p_active) {
-		if (get_space()) {
-			get_space()->body_remove_from_active_list(&active_list);
-		}
-	} else {
+
+	if (active) {
 		if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
-			return; //static bodies can't become active
-		}
-		if (get_space()) {
+			// Static bodies can't be active.
+			active = false;
+		} else if (get_space()) {
 			get_space()->body_add_to_active_list(&active_list);
 		}
-
-		//still_time=0;
-	}
-	/*
-	if (!space)
-		return;
-
-	for(int i=0;i<get_shape_count();i++) {
-		Shape &s=shapes[i];
-		if (s.bpid>0) {
-			get_space()->get_broadphase()->set_active(s.bpid,active);
-		}
+	} else if (get_space()) {
+		get_space()->body_remove_from_active_list(&active_list);
 	}
-*/
 }
 
 void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) {
@@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *p_space) {
 		if (active) {
 			get_space()->body_add_to_active_list(&active_list);
 		}
-		/*
-		_update_queries();
-		if (is_active()) {
-			active=false;
-			set_active(true);
-		}
-		*/
 	}
 
 	first_integration = false;
@@ -591,16 +570,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 +605,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 +637,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/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index feced36a2b..ff31825b83 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto
 		int least_deep = -1;
 		real_t min_depth = 1e10;
 
+		const Transform2D &transform_A = A->get_transform();
+		const Transform2D &transform_B = B->get_transform();
+
 		for (int i = 0; i <= contact_count; i++) {
 			Contact &c = (i == contact_count) ? contact : contacts[i];
-			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
+			Vector2 global_A = transform_A.basis_xform(c.local_A);
+			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
 
 			Vector2 axis = global_A - global_B;
 			real_t depth = axis.dot(c.normal);
@@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() {
 	real_t max_separation = space->get_contact_max_separation();
 	real_t max_separation2 = max_separation * max_separation;
 
+	const Transform2D &transform_A = A->get_transform();
+	const Transform2D &transform_B = B->get_transform();
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 
@@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() {
 		} else {
 			c.reused = false;
 
-			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
+			Vector2 global_A = transform_A.basis_xform(c.local_A);
+			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
 			Vector2 axis = global_A - global_B;
 			real_t depth = axis.dot(c.normal);
 
@@ -220,12 +226,24 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {
 }
 
 bool BodyPair2DSW::setup(real_t p_step) {
-	//cannot collide
-	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
 	}
 
+	report_contacts_only = false;
+	if (!dynamic_A && !dynamic_B) {
+		if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
+			report_contacts_only = true;
+		} else {
+			collided = false;
+			return false;
+		}
+	}
+
 	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
 		collided = false;
 		return false;
@@ -236,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
 
 	_validate_contacts();
 
-	Vector2 offset_A = A->get_transform().get_origin();
+	const Vector2 &offset_A = A->get_transform().get_origin();
 	Transform2D xform_Au = A->get_transform().untranslated();
 	Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
 
 	Transform2D xform_Bu = B->get_transform();
-	xform_Bu.elements[2] -= A->get_transform().get_origin();
+	xform_Bu.elements[2] -= offset_A;
 	Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
 	Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
@@ -262,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) {
 	if (!collided) {
 		//test ccd (currently just a raycast)
 
-		if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) {
 			if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) {
 				collided = true;
 			}
 		}
 
-		if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) {
 			if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) {
 				collided = true;
 			}
@@ -328,9 +346,21 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		}
 	}
 
+	return true;
+}
+
+bool BodyPair2DSW::pre_solve(real_t p_step) {
+	if (!collided || oneway_disabled) {
+		return false;
+	}
+
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	real_t bias = 0.3;
+
+	Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
+	Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
+
 	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
 		if (shape_A_ptr->get_custom_bias() == 0) {
 			bias = shape_B_ptr->get_custom_bias();
@@ -341,56 +371,49 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		}
 	}
 
-	cc = 0;
-
 	real_t inv_dt = 1.0 / p_step;
 
 	bool do_process = false;
 
+	const Vector2 &offset_A = A->get_transform().get_origin();
+	const Transform2D &transform_A = A->get_transform();
+	const Transform2D &transform_B = B->get_transform();
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
+		c.active = false;
 
-		Vector2 global_A = xform_Au.xform(c.local_A);
-		Vector2 global_B = xform_Bu.xform(c.local_B);
+		Vector2 global_A = transform_A.basis_xform(c.local_A);
+		Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
 
-		real_t depth = c.normal.dot(global_A - global_B);
+		Vector2 axis = global_A - global_B;
+		real_t depth = axis.dot(c.normal);
 
 		if (depth <= 0 || !c.reused) {
-			c.active = false;
 			continue;
 		}
 
-		c.active = true;
 #ifdef DEBUG_ENABLED
 		if (space->is_debugging_contacts()) {
 			space->add_debug_contact(global_A + offset_A);
 			space->add_debug_contact(global_B + offset_A);
 		}
 #endif
-		int gather_A = A->can_report_contacts();
-		int gather_B = B->can_report_contacts();
 
 		c.rA = global_A;
 		c.rB = global_B - offset_B;
 
-		if (gather_A | gather_B) {
-			//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
-
-			global_A += offset_A;
-			global_B += offset_A;
+		if (A->can_report_contacts()) {
+			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
+			A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
+		}
 
-			if (gather_A) {
-				Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
-				A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
-			}
-			if (gather_B) {
-				Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
-				B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
-			}
+		if (B->can_report_contacts()) {
+			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+			B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
 		}
 
-		if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
-			c.active = false;
+		if (report_contacts_only) {
 			collided = false;
 			continue;
 		}
@@ -418,8 +441,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
 			// Apply normal + friction impulse
 			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
 
-			A->apply_impulse(-P, c.rA);
-			B->apply_impulse(P, c.rB);
+			if (dynamic_A) {
+				A->apply_impulse(-P, c.rA);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(P, c.rB);
+			}
 		}
 #endif
 
@@ -431,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}
 
+		c.active = true;
 		do_process = true;
 	}
 
@@ -438,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
 }
 
 void BodyPair2DSW::solve(real_t p_step) {
-	if (!collided) {
+	if (!collided || oneway_disabled) {
 		return;
 	}
 
 	for (int i = 0; i < contact_count; ++i) {
 		Contact &c = contacts[i];
-		cc++;
 
 		if (!c.active) {
 			continue;
@@ -471,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) {
 
 		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-		A->apply_bias_impulse(-jb, c.rA);
-		B->apply_bias_impulse(jb, c.rB);
+		if (dynamic_A) {
+			A->apply_bias_impulse(-jb, c.rA);
+		}
+		if (dynamic_B) {
+			B->apply_bias_impulse(jb, c.rB);
+		}
 
 		real_t jn = -(c.bounce + vn) * c.mass_normal;
 		real_t jnOld = c.acc_normal_impulse;
@@ -487,8 +518,12 @@ void BodyPair2DSW::solve(real_t p_step) {
 
 		Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
 
-		A->apply_impulse(-j, c.rA);
-		B->apply_impulse(j, c.rB);
+		if (dynamic_A) {
+			A->apply_impulse(-j, c.rA);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(j, c.rB);
+		}
 	}
 }
 
@@ -501,9 +536,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh
 	space = A->get_space();
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
-	contact_count = 0;
-	collided = false;
-	oneway_disabled = false;
 }
 
 BodyPair2DSW::~BodyPair2DSW() {
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
index 31ab9b9017..4b42b44c92 100644
--- a/servers/physics_2d/body_pair_2d_sw.h
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW {
 			Body2DSW *B;
 		};
 
-		Body2DSW *_arr[2];
+		Body2DSW *_arr[2] = { nullptr, nullptr };
 	};
 
-	int shape_A;
-	int shape_B;
+	int shape_A = 0;
+	int shape_B = 0;
 
-	Space2DSW *space;
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
+	Space2DSW *space = nullptr;
 
 	struct Contact {
 		Vector2 position;
@@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW {
 
 	Vector2 sep_axis;
 	Contact contacts[MAX_CONTACTS];
-	int contact_count;
-	bool collided;
-	bool oneway_disabled;
-	int cc;
+	int contact_count = 0;
+	bool collided = false;
+	bool oneway_disabled = false;
+	bool report_contacts_only = false;
 
 	bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false);
 	void _validate_contacts();
@@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW {
 	_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
 
 public:
-	bool setup(real_t p_step);
-	void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B);
 	~BodyPair2DSW();
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/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 2db3961f41..c395e59694 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -143,8 +143,8 @@ public:
 		return shapes[p_index].metadata;
 	}
 
-	_FORCE_INLINE_ Transform2D get_transform() const { return transform; }
-	_FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
+	_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
 	_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
 
 	void set_shape_as_disabled(int p_idx, bool p_disabled);
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
index 49ae4dd848..5e8dfbe570 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; }
 
@@ -71,6 +63,7 @@ public:
 	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
 
 	virtual bool setup(real_t p_step) = 0;
+	virtual bool pre_solve(real_t p_step) = 0;
 	virtual void solve(real_t p_step) = 0;
 
 	virtual ~Constraint2DSW() {}
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index c7b556deba..eaec582f9b 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -97,8 +97,16 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
 }
 
 bool PinJoint2DSW::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		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;
 
@@ -143,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) {
 
 	bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
 
-	// apply accumulated impulse
-	A->apply_impulse(-P, rA);
-	if (B) {
-		B->apply_impulse(P, rB);
-	}
-
 	return true;
 }
 
@@ -156,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
 	return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
 }
 
+bool PinJoint2DSW::pre_solve(real_t p_step) {
+	// Apply accumulated impulse.
+	if (dynamic_A) {
+		A->apply_impulse(-P, rA);
+	}
+	if (B && dynamic_B) {
+		B->apply_impulse(P, rB);
+	}
+
+	return true;
+}
+
 void PinJoint2DSW::solve(real_t p_step) {
 	// compute relative velocity
 	Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
@@ -169,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) {
 
 	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
 
-	A->apply_impulse(-impulse, rA);
-	if (B) {
+	if (dynamic_A) {
+		A->apply_impulse(-impulse, rA);
+	}
+	if (B && dynamic_B) {
 		B->apply_impulse(impulse, rB);
 	}
 
@@ -257,10 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
 }
 
 bool GrooveJoint2DSW::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space, false);
+
 	// calculate endpoints in worldspace
 	Vector2 ta = A->get_transform().xform(A_groove_1);
 	Vector2 tb = A->get_transform().xform(A_groove_2);
-	Space2DSW *space = A->get_space();
 
 	// calculate axis
 	Vector2 n = -(tb - ta).orthogonal().normalized();
@@ -299,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
 	real_t _b = get_bias();
 	gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias());
 
-	// apply accumulated impulse
-	A->apply_impulse(-jn_acc, rA);
-	B->apply_impulse(jn_acc, rB);
-
 	correct = true;
 	return true;
 }
 
+bool GrooveJoint2DSW::pre_solve(real_t p_step) {
+	// Apply accumulated impulse.
+	if (dynamic_A) {
+		A->apply_impulse(-jn_acc, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(jn_acc, rB);
+	}
+
+	return true;
+}
+
 void GrooveJoint2DSW::solve(real_t p_step) {
 	// compute impulse
 	Vector2 vr = relative_velocity(A, B, rA, rB);
@@ -319,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) {
 
 	j = jn_acc - jOld;
 
-	A->apply_impulse(-j, rA);
-	B->apply_impulse(j, rB);
+	if (dynamic_A) {
+		A->apply_impulse(-j, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j, rB);
+	}
 }
 
 GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) :
@@ -342,6 +379,13 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
 //////////////////////////////////////////////
 
 bool DampedSpringJoint2DSW::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
 	rA = A->get_transform().basis_xform(anchor_A);
 	rB = B->get_transform().basis_xform(anchor_B);
 
@@ -360,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) {
 	target_vrn = 0.0f;
 	v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
 
-	// apply spring force
+	// Calculate spring force.
 	real_t f_spring = (rest_length - dist) * stiffness;
-	Vector2 j = n * f_spring * (p_step);
+	j = n * f_spring * (p_step);
+
+	return true;
+}
 
-	A->apply_impulse(-j, rA);
-	B->apply_impulse(j, rB);
+bool DampedSpringJoint2DSW::pre_solve(real_t p_step) {
+	// Apply spring force.
+	if (dynamic_A) {
+		A->apply_impulse(-j, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j, rB);
+	}
 
 	return true;
 }
@@ -380,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) {
 	target_vrn = vrn + v_damp;
 	Vector2 j = n * v_damp * n_mass;
 
-	A->apply_impulse(-j, rA);
-	B->apply_impulse(j, rB);
+	if (dynamic_A) {
+		A->apply_impulse(-j, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j, rB);
+	}
 }
 
 void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index 628de972ae..ccc5c585a0 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW {
 	real_t bias;
 	real_t max_bias;
 
+protected:
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
 public:
 	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
 	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
@@ -49,8 +53,9 @@ public:
 	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
 
-	virtual bool setup(real_t p_step) { return false; }
-	virtual void solve(real_t p_step) {}
+	virtual bool setup(real_t p_step) override { return false; }
+	virtual bool pre_solve(real_t p_step) override { return false; }
+	virtual void solve(real_t p_step) override {}
 
 	void copy_settings_from(Joint2DSW *p_joint);
 
@@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW {
 	real_t softness;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; }
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
 
-	virtual bool setup(real_t p_step);
-	virtual void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
 	real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
@@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW {
 	bool correct;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
 
-	virtual bool setup(real_t p_step);
-	virtual void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b);
 };
@@ -153,15 +160,17 @@ class DampedSpringJoint2DSW : public Joint2DSW {
 
 	Vector2 rA, rB;
 	Vector2 n;
+	Vector2 j;
 	real_t n_mass;
 	real_t target_vrn;
 	real_t v_coef;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
 
-	virtual bool setup(real_t p_step);
-	virtual void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
 	real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
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/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 6e7e802a8b..6cc086b9b7 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -502,6 +502,7 @@ Variant CapsuleShape2DSW::get_data() const {
 void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 	int support_idx = -1;
 	real_t d = -1e10;
+	r_amount = 0;
 
 	for (int i = 0; i < point_count; i++) {
 		//test point
@@ -520,7 +521,7 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su
 		}
 	}
 
-	ERR_FAIL_COND(support_idx == -1);
+	ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found.");
 
 	r_amount = 1;
 	r_supports[0] = points[support_idx].pos;
@@ -580,6 +581,7 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
 }
 
 real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points.");
 	Rect2 aabb;
 	aabb.position = points[0].pos * p_scale;
 	for (int i = 0; i < point_count; i++) {
@@ -691,6 +693,10 @@ bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const {
 }
 
 bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	if (segments.size() == 0 || points.size() == 0) {
+		return false;
+	}
+
 	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
 
 	enum {
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 6613d19729..b8cb4cddc5 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -29,102 +29,98 @@
 /*************************************************************************/
 
 #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
+#define CONSTRAINT_COUNT_RESERVE 1024
+
+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;
+
+	if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		// Only dynamic bodies are tested for activation.
+		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 *c = (Constraint2DSW *)E->get().first;
-		if (c->get_island_step() == _step) {
-			continue; //already processed
+		Constraint2DSW *constraint = (Constraint2DSW *)E->get().first;
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
 		}
-		c->set_island_step(_step);
-		c->set_island_next(*p_constraint_island);
-		*p_constraint_island = c;
+		constraint->set_island_step(_step);
+		p_constraint_island.push_back(constraint);
+		all_constraints.push_back(constraint);
 
-		for (int i = 0; i < c->get_body_count(); i++) {
+		for (int i = 0; i < constraint->get_body_count(); i++) {
 			if (i == E->get().second) {
 				continue;
 			}
-			Body2DSW *b = c->get_body_ptr()[i];
-			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
-				continue; //no go
+			Body2DSW *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
 			}
-			_populate_island(c->get_body_ptr()[i], p_island, p_constraint_island);
+			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);
 		}
 	}
 }
 
-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_contraint(uint32_t p_constraint_index, void *p_userdata) {
+	Constraint2DSW *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const {
+	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]->pre_solve(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) {
-	for (int i = 0; i < p_iterations; i++) {
-		Constraint2DSW *ci = p_island;
-		while (ci) {
-			ci->solve(p_delta);
-			ci = ci->get_island_next();
+void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
+	const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index];
+
+	for (int i = 0; i < iterations; i++) {
+		uint32_t constraint_count = constraint_island.size();
+		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+			constraint_island[constraint_index]->solve(delta);
 		}
 	}
 }
 
-void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) {
+void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
 	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 (!b->sleep_test(p_delta)) {
+		if (!body->sleep_test(delta)) {
 			can_sleep = false;
 		}
-
-		b = b->get_island_next();
 	}
 
-	//put all to sleep or wake up everyoen
-
-	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
-		}
+	// 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];
 
-		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();
 	}
 }
 
@@ -133,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->setup(); //update inertias, etc
 
+	iterations = p_iterations;
+	delta = p_delta;
+
 	const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
 
 	/* INTEGRATE FORCES */
@@ -157,94 +156,85 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
-	/* GENERATE CONSTRAINT ISLANDS */
+	/* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
+
+	uint32_t island_count = 0;
+
+	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()) {
+			Constraint2DSW *constraint = E->get();
+			if (constraint->get_island_step() == _step) {
+				continue;
+			}
+			constraint->set_island_step(_step);
+
+			// Each constraint can be on a separate island for areas as there's no solving phase.
+			++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();
+
+			all_constraints.push_back(constraint);
+			constraint_island.push_back(constraint);
+		}
+		p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
 
-	Body2DSW *island_list = nullptr;
-	Constraint2DSW *constraint_island_list = nullptr;
 	b = body_list->first();
 
-	int island_count = 0;
+	uint32_t body_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);
-
-			island->set_island_list_next(island_list);
-			island_list = 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);
 
-			if (constraint_island) {
-				constraint_island->set_island_list_next(constraint_island_list);
-				constraint_island_list = constraint_island;
-				island_count++;
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
 			}
-		}
-		b = b->next();
-	}
+			LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
-	p_space->set_island_count(island_count);
+			_populate_island(body, body_island, constraint_island);
 
-	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
 
-	while (aml.first()) {
-		for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint2DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
+			if (constraint_island.is_empty()) {
+				--island_count;
 			}
-			c->set_island_step(_step);
-			c->set_island_next(nullptr);
-			c->set_island_list_next(constraint_island_list);
-			constraint_island_list = c;
 		}
-		p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
+		b = b->next();
 	}
 
+	p_space->set_island_count((int)island_count);
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
-	/* 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;
-			}
+	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
-			ci = ci->get_island_list_next();
-		}
-	}
+	uint32_t total_contraint_count = all_constraints.size();
+	work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -252,15 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
+	/* PRE-SOLVE CONSTRAINT ISLANDS */
+
+	// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
+	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
+		_pre_solve_island(constraint_islands[island_index]);
+	}
+
 	/* 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();
-		}
+	// Warning: _solve_island modifies the constraint islands for optimization purpose,
+	// their content is not reliable after these calls and shouldn't be used anymore.
+	if (island_count > 1) {
+		work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr);
+	} else if (island_count > 0) {
+		_solve_island(0);
 	}
 
 	{ //profile
@@ -280,12 +276,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]);
 	}
 
 	{ //profile
@@ -294,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 		//profile_begtime=profile_endtime;
 	}
 
+	all_constraints.clear();
+
 	p_space->update();
 	p_space->unlock();
 	_step++;
@@ -301,4 +295,14 @@ 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);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+
+	work_pool.init();
+}
+
+Step2DSW::~Step2DSW() {
+	work_pool.finish();
 }
diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h
index 83b9130608..c51fd73a79 100644
--- a/servers/physics_2d/step_2d_sw.h
+++ b/servers/physics_2d/step_2d_sw.h
@@ -33,17 +33,31 @@
 
 #include "space_2d_sw.h"
 
+#include "core/templates/local_vector.h"
+#include "core/templates/thread_work_pool.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);
+	int iterations = 0;
+	real_t delta = 0.0;
+
+	ThreadWorkPool work_pool;
+
+	LocalVector<LocalVector<Body2DSW *>> body_islands;
+	LocalVector<LocalVector<Constraint2DSW *>> constraint_islands;
+	LocalVector<Constraint2DSW *> all_constraints;
+
+	void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island);
+	void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+	void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const;
+	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
+	void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const;
 
 public:
 	void step(Space2DSW *p_space, real_t p_delta, int p_iterations);
 	Step2DSW();
+	~Step2DSW();
 };
 
 #endif // STEP_2D_SW_H