Godot Physics collisions and solver processed on threads

Use ThreadWorkPool to process physics step tasks in multiple threads. Collisions are all processed in parallel and solving impulses is
processed in parallel for rigid body islands.

Additional changes:
- Proper islands for soft bodies linked to active bodies
- All moving areas are on separate islands (can be parallelized)
- Fix inconsistencies with body islands (Kinematic bodies could link
bodies together or not depending on the processing order)
- Completely prevent static bodies to be active (it could cause islands
to be wrongly created and cause dangerous multi-threading operations as
well as inconsistencies in created islands)
- Apply impulses only on dynamic bodies to avoid unsafe multi-threaded
operations (static bodies can be on multiple islands)
- Removed inverted iterations when populating body islands, it's now
faster in regular order (maybe after fixing inconsistencies)

20 files changed, 527 insertions, 247 deletions

diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp
index 4de5f1ba47..2073df7dee 100644
--- a/servers/physics_3d/area_pair_3d_sw.cpp
+++ b/servers/physics_3d/area_pair_3d_sw.cpp
@@ -40,31 +40,48 @@ bool AreaPair3DSW::setup(real_t p_step) {
 		result = true;
 	}
 
+	process_collision = false;
 	if (result != colliding) {
-		if (result) {
-			if (area->get_space_override_mode() != PhysicsServer3D::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() != PhysicsServer3D::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() != PhysicsServer3D::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 AreaPair3DSW::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::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() != PhysicsServer3D::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 AreaPair3DSW::solve(real_t p_step) {
+	// Nothing to do.
 }
 
 AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) {
@@ -72,7 +89,6 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *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() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
@@ -103,33 +119,48 @@ bool Area2Pair3DSW::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 Area2Pair3DSW::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 Area2Pair3DSW::solve(real_t p_step) {
+	// Nothing to do.
 }
 
 Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) {
@@ -137,7 +168,6 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *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_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h
index fbdaa25cbb..596d893082 100644
--- a/servers/physics_3d/area_pair_3d_sw.h
+++ b/servers/physics_3d/area_pair_3d_sw.h
@@ -40,11 +40,13 @@ class AreaPair3DSW : public Constraint3DSW {
 	Area3DSW *area;
 	int body_shape;
 	int area_shape;
-	bool colliding;
+	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;
 
 	AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape);
 	~AreaPair3DSW();
@@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW {
 	Area3DSW *area_b;
 	int shape_a;
 	int shape_b;
-	bool colliding;
+	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;
 
 	Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b);
 	~Area2Pair3DSW();
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index d54345821d..4357c474e4 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -145,31 +145,17 @@ void Body3DSW::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 == PhysicsServer3D::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;
+	} else if (get_space()) {
+		get_space()->body_remove_from_active_list(&active_list);
 	}
-	/*
-	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);
-		}
-	}
-*/
 }
 
 void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
@@ -392,13 +378,6 @@ void Body3DSW::set_space(Space3DSW *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 = true;
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index 28c854466f..cdb3da665e 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -212,14 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) {
 }
 
 bool BodyPair3DSW::setup(real_t p_step) {
-	//cannot collide
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::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;
 	}
 
-	bool report_contacts_only = false;
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	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 {
@@ -237,7 +239,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
 
 	validate_contacts();
 
-	Vector3 offset_A = A->get_transform().get_origin();
+	const Vector3 &offset_A = A->get_transform().get_origin();
 	Transform xform_Au = Transform(A->get_transform().basis, Vector3());
 	Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
 
@@ -248,27 +250,37 @@ bool BodyPair3DSW::setup(real_t p_step) {
 	Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
 	Shape3DSW *shape_B_ptr = B->get_shape(shape_B);
 
-	bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
-	this->collided = collided;
+	collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
 
 	if (!collided) {
 		//test ccd (currently just a raycast)
 
-		if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) {
 			_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
 		}
 
-		if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) {
 			_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
 		}
 
 		return false;
 	}
 
+	return true;
+}
+
+bool BodyPair3DSW::pre_solve(real_t p_step) {
+	if (!collided) {
+		return false;
+	}
+
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	real_t bias = (real_t)0.3;
 
+	Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
+	Shape3DSW *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();
@@ -283,22 +295,26 @@ bool BodyPair3DSW::setup(real_t p_step) {
 
 	bool do_process = false;
 
+	const Basis &basis_A = A->get_transform().basis;
+	const Basis &basis_B = B->get_transform().basis;
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 		c.active = false;
 
-		Vector3 global_A = xform_Au.xform(c.local_A);
-		Vector3 global_B = xform_Bu.xform(c.local_B);
+		Vector3 global_A = basis_A.xform(c.local_A);
+		Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
 
-		real_t depth = c.normal.dot(global_A - global_B);
+		Vector3 axis = global_A - global_B;
+		real_t depth = axis.dot(c.normal);
 
 		if (depth <= 0) {
 			continue;
 		}
 
 #ifdef DEBUG_ENABLED
-
 		if (space->is_debugging_contacts()) {
+			const Vector3 &offset_A = A->get_transform().get_origin();
 			space->add_debug_contact(global_A + offset_A);
 			space->add_debug_contact(global_B + offset_A);
 		}
@@ -338,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) {
 		c.depth = depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
-		B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
+		if (dynamic_A) {
+			A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
+		}
+		if (dynamic_B) {
+			B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
+		}
 		c.acc_bias_impulse = 0;
 		c.acc_bias_impulse_center_of_mass = 0;
 
@@ -361,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) {
 		return;
 	}
 
+	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 		if (!c.active) {
@@ -384,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-			A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step);
-			B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step);
+			if (dynamic_A) {
+				A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
+			}
+			if (dynamic_B) {
+				B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
+			}
 
 			crbA = A->get_biased_angular_velocity().cross(c.rA);
 			crbB = B->get_biased_angular_velocity().cross(c.rB);
@@ -400,8 +426,12 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
 
-				A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
-				B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
+				if (dynamic_A) {
+					A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
+				}
+				if (dynamic_B) {
+					B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
+				}
 			}
 
 			c.active = true;
@@ -421,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			A->apply_impulse(-j, c.rA + A->get_center_of_mass());
-			B->apply_impulse(j, c.rB + B->get_center_of_mass());
+			if (dynamic_A) {
+				A->apply_impulse(-j, c.rA + A->get_center_of_mass());
+			}
+			if (dynamic_B) {
+				B->apply_impulse(j, c.rB + B->get_center_of_mass());
+			}
 
 			c.active = true;
 		}
@@ -464,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			jt = c.acc_tangent_impulse - jtOld;
 
-			A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
-			B->apply_impulse(jt, c.rB + B->get_center_of_mass());
+			if (dynamic_A) {
+				A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
+			}
+			if (dynamic_B) {
+				B->apply_impulse(jt, c.rB + B->get_center_of_mass());
+			}
 
 			c.active = true;
 		}
@@ -481,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh
 	space = A->get_space();
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
-	contact_count = 0;
-	collided = false;
 }
 
 BodyPair3DSW::~BodyPair3DSW() {
@@ -564,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() {
 }
 
 bool BodySoftBodyPair3DSW::setup(real_t p_step) {
+	body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
 	if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
 		collided = false;
 		return false;
@@ -585,12 +623,22 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 	Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
 	Shape3DSW *shape_B_ptr = soft_body->get_shape(0);
 
-	bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
-	this->collided = collided;
+	collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+
+	return collided;
+}
+
+bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
+	if (!collided) {
+		return false;
+	}
 
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	real_t bias = (real_t)0.3;
+
+	Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
+
 	if (shape_A_ptr->get_custom_bias()) {
 		bias = shape_A_ptr->get_custom_bias();
 	}
@@ -599,6 +647,8 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 
 	bool do_process = false;
 
+	const Transform &transform_A = body->get_transform();
+
 	uint32_t contact_count = contacts.size();
 	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
 		Contact &c = contacts[contact_index];
@@ -609,10 +659,10 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 			continue;
 		}
 
-		Vector3 global_A = xform_Au.xform(c.local_A);
+		Vector3 global_A = transform_A.xform(c.local_A);
 		Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
-
-		real_t depth = c.normal.dot(global_A - global_B);
+		Vector3 axis = global_A - global_B;
+		real_t depth = axis.dot(c.normal);
 
 		if (depth <= 0) {
 			continue;
@@ -629,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		}
 #endif
 
-		c.rA = global_A - xform_Au.origin - body->get_center_of_mass();
+		c.rA = global_A - transform_A.origin - body->get_center_of_mass();
 		c.rB = global_B;
 
 		if (body->can_report_contacts()) {
@@ -637,7 +687,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 			body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
 		}
 
-		if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		if (body_dynamic) {
 			body->set_active(true);
 		}
 
@@ -651,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		c.depth = depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
+		if (body_dynamic) {
+			body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
+		}
 		soft_body->apply_node_impulse(c.index_B, j_vec);
 		c.acc_bias_impulse = 0;
 		c.acc_bias_impulse_center_of_mass = 0;
@@ -675,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 		return;
 	}
 
+	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
+
 	uint32_t contact_count = contacts.size();
 	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
 		Contact &c = contacts[contact_index];
@@ -697,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-			body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), MAX_BIAS_ROTATION / p_step);
+			if (body_dynamic) {
+				body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av);
+			}
 			soft_body->apply_node_bias_impulse(c.index_B, jb);
 
 			crbA = body->get_biased_angular_velocity().cross(c.rA);
@@ -712,7 +768,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
 
-				body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
+				if (body_dynamic) {
+					body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
+				}
 				soft_body->apply_node_bias_impulse(c.index_B, jb_com);
 			}
 
@@ -732,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			body->apply_impulse(-j, c.rA + body->get_center_of_mass());
+			if (body_dynamic) {
+				body->apply_impulse(-j, c.rA + body->get_center_of_mass());
+			}
 			soft_body->apply_node_impulse(c.index_B, j);
 
 			c.active = true;
@@ -773,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			jt = c.acc_tangent_impulse - jtOld;
 
-			body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
+			if (body_dynamic) {
+				body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
+			}
 			soft_body->apply_node_impulse(c.index_B, jt);
 
 			c.active = true;
@@ -781,7 +843,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 	}
 }
 
-BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) {
+BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) :
+		BodyContact3DSW(&body, 1) {
 	body = p_A;
 	soft_body = p_B;
 	body_shape = p_shape_A;
diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h
index 74dddfa6aa..3f425ba2d7 100644
--- a/servers/physics_3d/body_pair_3d_sw.h
+++ b/servers/physics_3d/body_pair_3d_sw.h
@@ -57,9 +57,9 @@ protected:
 	};
 
 	Vector3 sep_axis;
-	bool collided;
+	bool collided = false;
 
-	Space3DSW *space;
+	Space3DSW *space = nullptr;
 
 	BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
 			Constraint3DSW(p_body_ptr, p_body_count) {
@@ -77,16 +77,21 @@ class BodyPair3DSW : public BodyContact3DSW {
 			Body3DSW *B;
 		};
 
-		Body3DSW *_arr[2];
+		Body3DSW *_arr[2] = { nullptr, nullptr };
 	};
 
-	int shape_A;
-	int shape_B;
+	int shape_A = 0;
+	int shape_B = 0;
+
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
+	bool report_contacts_only = false;
 
 	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
 
 	Contact contacts[MAX_CONTACTS];
-	int contact_count;
+	int contact_count = 0;
 
 	static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
 
@@ -96,18 +101,21 @@ class BodyPair3DSW : public BodyContact3DSW {
 	bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_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;
 
 	BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B);
 	~BodyPair3DSW();
 };
 
 class BodySoftBodyPair3DSW : public BodyContact3DSW {
-	Body3DSW *body;
-	SoftBody3DSW *soft_body;
+	Body3DSW *body = nullptr;
+	SoftBody3DSW *soft_body = nullptr;
 
-	int body_shape;
+	int body_shape = 0;
+
+	bool body_dynamic = false;
 
 	LocalVector<Contact> contacts;
 
@@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW {
 	void validate_contacts();
 
 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;
+
+	virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; }
+	virtual int get_soft_body_count() const override { return 1; }
 
 	BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B);
 	~BodySoftBodyPair3DSW();
diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h
index 16a31e167d..7b44726ef5 100644
--- a/servers/physics_3d/constraint_3d_sw.h
+++ b/servers/physics_3d/constraint_3d_sw.h
@@ -31,7 +31,8 @@
 #ifndef CONSTRAINT_SW_H
 #define CONSTRAINT_SW_H
 
-#include "body_3d_sw.h"
+class Body3DSW;
+class SoftBody3DSW;
 
 class Constraint3DSW {
 	Body3DSW **_body_ptr;
@@ -61,6 +62,9 @@ public:
 	_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
+	virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; }
+	virtual int get_soft_body_count() const { return 0; }
+
 	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
@@ -68,6 +72,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 ~Constraint3DSW() {}
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
index 167f797bfe..e9efddf165 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
@@ -109,7 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
 }
 
 bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
 		return false;
 	}
 
@@ -265,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
 			real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
 			m_appliedImpulse += impulse;
 			Vector3 impulse_vector = normal * impulse;
-			A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
-			B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
 		}
 	}
 
@@ -287,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
 
 			Vector3 impulse = m_swingAxis * impulseMag;
 
-			A->apply_torque_impulse(impulse);
-			B->apply_torque_impulse(-impulse);
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-impulse);
+			}
 		}
 
 		// solve twist limit
@@ -303,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
 
 			Vector3 impulse = m_twistAxis * impulseMag;
 
-			A->apply_torque_impulse(impulse);
-			B->apply_torque_impulse(-impulse);
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-impulse);
+			}
 		}
 	}
 }
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
index 4e4d4e7f0c..b871ea50db 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
@@ -102,10 +102,10 @@ public:
 	bool m_solveSwingLimit;
 
 public:
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
 
-	virtual bool setup(real_t p_timestep);
-	virtual void solve(real_t p_timestep);
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
 
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
index a86e8b4e76..7c504764a7 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
@@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
 
 real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
 		real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
-		Body3DSW *body0, Body3DSW *body1) {
+		Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
 	if (!needApplyTorques()) {
 		return 0.0f;
 	}
@@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
 
 	Vector3 motorImp = clippedMotorImpulse * axis;
 
-	body0->apply_torque_impulse(motorImp);
-	if (body1) {
+	if (p_body0_dynamic) {
+		body0->apply_torque_impulse(motorImp);
+	}
+	if (body1 && p_body1_dynamic) {
 		body1->apply_torque_impulse(-motorImp);
 	}
 
@@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
 		real_t jacDiagABInv,
 		Body3DSW *body1, const Vector3 &pointInA,
 		Body3DSW *body2, const Vector3 &pointInB,
+		bool p_body1_dynamic, bool p_body2_dynamic,
 		int limit_index,
 		const Vector3 &axis_normal_on_a,
 		const Vector3 &anchorPos) {
@@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
 	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 
 	Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
-	body1->apply_impulse(impulse_vector, rel_pos1);
-	body2->apply_impulse(-impulse_vector, rel_pos2);
+	if (p_body1_dynamic) {
+		body1->apply_impulse(impulse_vector, rel_pos1);
+	}
+	if (p_body2_dynamic) {
+		body2->apply_impulse(-impulse_vector, rel_pos2);
+	}
 	return normalImpulse;
 }
 
@@ -303,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
 }
 
 bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
 		return false;
 	}
 
@@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
 					jacDiagABInv,
 					A, pointInA,
 					B, pointInB,
+					dynamic_A, dynamic_B,
 					i, linear_axis, m_AnchorPos);
 		}
 	}
@@ -398,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
 
 			angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
 
-			m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B);
+			m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B);
 		}
 	}
 }
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
index d61a033231..8af76cefc2 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
@@ -120,7 +120,7 @@ public:
 	int testLimitValue(real_t test_value);
 
 	//! apply the correction impulses for two bodies
-	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1);
+	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic);
 };
 
 class G6DOFTranslationalLimitMotor3DSW {
@@ -166,6 +166,7 @@ public:
 			real_t jacDiagABInv,
 			Body3DSW *body1, const Vector3 &pointInA,
 			Body3DSW *body2, const Vector3 &pointInB,
+			bool p_body1_dynamic, bool p_body2_dynamic,
 			int limit_index,
 			const Vector3 &axis_normal_on_a,
 			const Vector3 &anchorPos);
@@ -234,10 +235,10 @@ protected:
 public:
 	Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
 
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; }
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
 
-	virtual bool setup(real_t p_timestep);
-	virtual void solve(real_t p_timestep);
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	//! Calcs global transform of the offsets
 	/*!
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
index 90b82f4680..bb8858c28a 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
@@ -155,7 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
 }
 
 bool HingeJoint3DSW::setup(real_t p_step) {
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
 		return false;
 	}
 
@@ -279,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
 			real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
 			m_appliedImpulse += impulse;
 			Vector3 impulse_vector = normal * impulse;
-			A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
-			B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
 		}
 	}
 
@@ -322,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
 				angularError *= (real_t(1.) / denom2) * relaxation;
 			}
 
-			A->apply_torque_impulse(-velrelOrthog + angularError);
-			B->apply_torque_impulse(velrelOrthog - angularError);
+			if (dynamic_A) {
+				A->apply_torque_impulse(-velrelOrthog + angularError);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(velrelOrthog - angularError);
+			}
 
 			// solve limit
 			if (m_solveLimit) {
@@ -337,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
 				impulseMag = m_accLimitImpulse - temp;
 
 				Vector3 impulse = axisA * impulseMag * m_limitSign;
-				A->apply_torque_impulse(impulse);
-				B->apply_torque_impulse(-impulse);
+				if (dynamic_A) {
+					A->apply_torque_impulse(impulse);
+				}
+				if (dynamic_B) {
+					B->apply_torque_impulse(-impulse);
+				}
 			}
 		}
 
@@ -359,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
 			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
 			Vector3 motorImp = clippedMotorImpulse * axisA;
 
-			A->apply_torque_impulse(motorImp + angularLimit);
-			B->apply_torque_impulse(-motorImp - angularLimit);
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp + angularLimit);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp - angularLimit);
+			}
 		}
 	}
 }
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h
index b6117aa0bc..2100f5de44 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.h
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h
@@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW {
 	real_t m_appliedImpulse;
 
 public:
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; }
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
 
-	virtual bool setup(real_t p_step);
-	virtual void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
 	real_t get_hinge_angle();
 
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
index 75d87992d1..8eb84d1c2f 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
@@ -50,7 +50,10 @@ subject to the following restrictions:
 #include "pin_joint_3d_sw.h"
 
 bool PinJoint3DSW::setup(real_t p_step) {
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
 		return false;
 	}
 
@@ -123,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) {
 
 		m_appliedImpulse += impulse;
 		Vector3 impulse_vector = normal * impulse;
-		A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
-		B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+		}
 
 		normal[i] = 0;
 	}
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h
index 1875983527..3d91452850 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.h
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.h
@@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW {
 	Vector3 m_pivotInB;
 
 public:
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; }
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::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 void solve(real_t p_step) override;
 
 	void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
 	real_t get_param(PhysicsServer3D::PinJointParam p_param) const;
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
index 2e1ee8e770..8bd1951311 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
@@ -127,7 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &
 //-----------------------------------------------------------------------------
 
 bool SliderJoint3DSW::setup(real_t p_step) {
-	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
 		return false;
 	}
 
@@ -200,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
 		// calcutate and apply impulse
 		real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
 		Vector3 impulse_vector = normal * normalImpulse;
-		A->apply_impulse(impulse_vector, m_relPosA);
-		B->apply_impulse(-impulse_vector, m_relPosB);
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, m_relPosA);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, m_relPosB);
+		}
 		if (m_poweredLinMotor && (!i)) { // apply linear motor
 			if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
 				real_t desiredMotorVel = m_targetLinMotorVelocity;
@@ -221,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
 				m_accumulatedLinMotorImpulse = new_acc;
 				// apply clamped impulse
 				impulse_vector = normal * normalImpulse;
-				A->apply_impulse(impulse_vector, m_relPosA);
-				B->apply_impulse(-impulse_vector, m_relPosB);
+				if (dynamic_A) {
+					A->apply_impulse(impulse_vector, m_relPosA);
+				}
+				if (dynamic_B) {
+					B->apply_impulse(-impulse_vector, m_relPosB);
+				}
 			}
 		}
 	}
@@ -256,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
 		angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
 	}
 	// apply impulse
-	A->apply_torque_impulse(-velrelOrthog + angularError);
-	B->apply_torque_impulse(velrelOrthog - angularError);
+	if (dynamic_A) {
+		A->apply_torque_impulse(-velrelOrthog + angularError);
+	}
+	if (dynamic_B) {
+		B->apply_torque_impulse(velrelOrthog - angularError);
+	}
 	real_t impulseMag;
 	//solve angular limits
 	if (m_solveAngLim) {
@@ -268,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
 		impulseMag *= m_kAngle * m_softnessDirAng;
 	}
 	Vector3 impulse = axisA * impulseMag;
-	A->apply_torque_impulse(impulse);
-	B->apply_torque_impulse(-impulse);
+	if (dynamic_A) {
+		A->apply_torque_impulse(impulse);
+	}
+	if (dynamic_B) {
+		B->apply_torque_impulse(-impulse);
+	}
 	//apply angular motor
 	if (m_poweredAngMotor) {
 		if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
@@ -294,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
 			m_accumulatedAngMotorImpulse = new_acc;
 			// apply clamped impulse
 			Vector3 motorImp = angImpulse * axisA;
-			A->apply_torque_impulse(motorImp);
-			B->apply_torque_impulse(-motorImp);
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp);
+			}
 		}
 	}
 } // SliderJointSW::solveConstraint()
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h
index f52f6ace27..ef5891d0f9 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.h
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.h
@@ -240,10 +240,10 @@ public:
 	void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
 	real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
 
-	bool setup(real_t p_step);
-	void solve(real_t p_step);
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
 
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
 };
 
 #endif // SLIDER_JOINT_SW_H
diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h
index 225a71aca9..e2514674ea 100644
--- a/servers/physics_3d/joints_3d_sw.h
+++ b/servers/physics_3d/joints_3d_sw.h
@@ -35,9 +35,14 @@
 #include "constraint_3d_sw.h"
 
 class Joint3DSW : public Constraint3DSW {
+protected:
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
 public:
-	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 true; }
+	virtual void solve(real_t p_step) override {}
 
 	void copy_settings_from(Joint3DSW *p_joint) {
 		set_self(p_joint->get_self());
diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h
index 6c0451ff45..98e554218b 100644
--- a/servers/physics_3d/soft_body_3d_sw.h
+++ b/servers/physics_3d/soft_body_3d_sw.h
@@ -106,6 +106,8 @@ class SoftBody3DSW : public CollisionObject3DSW {
 
 	VSet<RID> exceptions;
 
+	uint64_t island_step = 0;
+
 public:
 	SoftBody3DSW();
 
@@ -124,6 +126,9 @@ public:
 	_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
 	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
 
+	_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; }
+
 	virtual void set_space(Space3DSW *p_space);
 
 	void set_mesh(const Ref<Mesh> &p_mesh);
diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index 06f3227eab..ba18bac611 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -37,39 +37,90 @@
 #define BODY_ISLAND_SIZE_RESERVE 512
 #define ISLAND_COUNT_RESERVE 128
 #define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
 
 void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
 	p_body->set_island_step(_step);
-	p_body_island.push_back(p_body);
 
-	// Faster with reversed iterations.
-	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) {
-		Constraint3DSW *c = (Constraint3DSW *)E->key();
-		if (c->get_island_step() == _step) {
-			continue; //already processed
+	if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		// Only dynamic bodies are tested for activation.
+		p_body_island.push_back(p_body);
+	}
+
+	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
+		Constraint3DSW *constraint = (Constraint3DSW *)E->key();
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
 		}
-		c->set_island_step(_step);
-		p_constraint_island.push_back(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++) {
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
 			if (i == E->get()) {
 				continue;
 			}
-			Body3DSW *b = c->get_body_ptr()[i];
-			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-				continue; //no go
+			Body3DSW *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(other_body, p_body_island, p_constraint_island);
+		}
+
+		// Find connected soft bodies.
+		for (int i = 0; i < constraint->get_soft_body_count(); i++) {
+			SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i);
+			if (soft_body->get_island_step() == _step) {
+				continue; // Already processed.
 			}
-			_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
+			_populate_island_soft_body(soft_body, p_body_island, p_constraint_island);
 		}
 	}
 }
 
-void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) {
+void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
+	p_soft_body->set_island_step(_step);
+
+	for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
+		Constraint3DSW *constraint = (Constraint3DSW *)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);
+
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
+			Body3DSW *body = constraint->get_body_ptr()[i];
+			if (body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(body, p_body_island, p_constraint_island);
+		}
+	}
+}
+
+void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+	Constraint3DSW *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &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) {
 		Constraint3DSW *constraint = p_constraint_island[constraint_index];
-		if (p_constraint_island[constraint_index]->setup(p_delta)) {
+		if (p_constraint_island[constraint_index]->pre_solve(delta)) {
 			// Keep this constraint for solving.
 			p_constraint_island[valid_constraint_count++] = constraint;
 		}
@@ -77,15 +128,17 @@ void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island,
 	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
+void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
+	LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index];
+
 	int current_priority = 1;
 
-	uint32_t constraint_count = p_constraint_island.size();
+	uint32_t constraint_count = constraint_island.size();
 	while (constraint_count > 0) {
-		for (int i = 0; i < p_iterations; i++) {
+		for (int i = 0; i < iterations; i++) {
 			// Go through all iterations.
 			for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-				p_constraint_island[constraint_index]->solve(p_delta);
+				constraint_island[constraint_index]->solve(delta);
 			}
 		}
 
@@ -93,28 +146,24 @@ void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island,
 		uint32_t priority_constraint_count = 0;
 		++current_priority;
 		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-			Constraint3DSW *constraint = p_constraint_island[constraint_index];
+			Constraint3DSW *constraint = constraint_island[constraint_index];
 			if (constraint->get_priority() >= current_priority) {
 				// Keep this constraint for the next iteration.
-				p_constraint_island[priority_constraint_count++] = constraint;
+				constraint_island[priority_constraint_count++] = constraint;
 			}
 		}
 		constraint_count = priority_constraint_count;
 	}
 }
 
-void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) {
+void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const {
 	bool can_sleep = true;
 
 	uint32_t body_count = p_body_island.size();
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
 		Body3DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
-		if (!body->sleep_test(p_delta)) {
+		if (!body->sleep_test(delta)) {
 			can_sleep = false;
 		}
 	}
@@ -123,10 +172,6 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
 		Body3DSW *body = p_body_island[body_index];
 
-		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			continue; // Ignore for static.
-		}
-
 		bool active = body->is_active();
 
 		if (active == can_sleep) {
@@ -140,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->setup(); //update inertias, etc
 
+	iterations = p_iterations;
+	delta = p_delta;
+
 	const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
 
 	const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
@@ -175,12 +223,39 @@ void Step3DSW::step(Space3DSW *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<Area3DSW>::List &aml = p_space->get_moved_area_list();
+
+	while (aml.first()) {
+		for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+			Constraint3DSW *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<Constraint3DSW *> &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<Area3DSW> *)aml.first()); //faster to remove here
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
 
 	b = body_list->first();
 
 	uint32_t body_island_count = 0;
-	uint32_t island_count = 0;
 
 	while (b) {
 		Body3DSW *body = b->self();
@@ -204,7 +279,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 			_populate_island(body, body_island, constraint_island);
 
-			body_islands.push_back(body_island);
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
 
 			if (constraint_island.is_empty()) {
 				--island_count;
@@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		b = b->next();
 	}
 
-	p_space->set_island_count((int)island_count);
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
 
-	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
+	sb = soft_body_list->first();
+	while (sb) {
+		SoftBody3DSW *soft_body = sb->self();
 
-	while (aml.first()) {
-		for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint3DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
+		if (soft_body->get_island_step() != _step) {
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
 			}
-			c->set_island_step(_step);
+			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			body_island.clear();
+			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
+
 			++island_count;
 			if (constraint_islands.size() < island_count) {
 				constraint_islands.resize(island_count);
 			}
 			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
 			constraint_island.clear();
-			constraint_island.push_back(c);
-		}
-		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
-	}
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
-	sb = soft_body_list->first();
-	while (sb) {
-		for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint3DSW *c = E->get();
-			if (c->get_island_step() == _step) {
-				continue;
+			_populate_island_soft_body(soft_body, body_island, constraint_island);
+
+			if (body_island.is_empty()) {
+				--body_island_count;
 			}
-			c->set_island_step(_step);
-			++island_count;
-			if (constraint_islands.size() < island_count) {
-				constraint_islands.resize(island_count);
+
+			if (constraint_island.is_empty()) {
+				--island_count;
 			}
-			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
-			constraint_island.clear();
-			constraint_island.push_back(c);
 		}
 		sb = sb->next();
 	}
 
+	p_space->set_island_count((int)island_count);
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
-	/* SETUP CONSTRAINT ISLANDS */
+	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
-	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
-		_setup_island(constraint_islands[island_index], p_delta);
-	}
+	uint32_t total_contraint_count = all_constraints.size();
+	work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -272,12 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
-	/* SOLVE CONSTRAINT ISLANDS */
+	/* 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) {
-		// Warning: _solve_island modifies the constraint islands for optimization purpose,
-		// their content is not reliable after these calls and shouldn't be used anymore.
-		_solve_island(constraint_islands[island_index], p_iterations, p_delta);
+		_pre_solve_island(constraint_islands[island_index]);
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	// 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, &Step3DSW::_solve_island, nullptr);
+	} else if (island_count > 0) {
+		_solve_island(0);
 	}
 
 	{ //profile
@@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 	/* SLEEP / WAKE UP ISLANDS */
 
 	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
-		_check_suspend(body_islands[island_index], p_delta);
+		_check_suspend(body_islands[island_index]);
 	}
 
 	/* UPDATE SOFT BODY CONSTRAINTS */
@@ -315,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
+	all_constraints.clear();
+
 	p_space->update();
 	p_space->unlock();
 	_step++;
@@ -325,4 +409,11 @@ Step3DSW::Step3DSW() {
 
 	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
 	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+
+	work_pool.init();
+}
+
+Step3DSW::~Step3DSW() {
+	work_pool.finish();
 }
diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h
index f406c35c3a..9c60004b24 100644
--- a/servers/physics_3d/step_3d_sw.h
+++ b/servers/physics_3d/step_3d_sw.h
@@ -34,21 +34,31 @@
 #include "space_3d_sw.h"
 
 #include "core/templates/local_vector.h"
+#include "core/templates/thread_work_pool.h"
 
 class Step3DSW {
 	uint64_t _step;
 
+	int iterations = 0;
+	real_t delta = 0.0;
+
+	ThreadWorkPool work_pool;
+
 	LocalVector<LocalVector<Body3DSW *>> body_islands;
 	LocalVector<LocalVector<Constraint3DSW *>> constraint_islands;
+	LocalVector<Constraint3DSW *> all_constraints;
 
 	void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
-	void _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta);
-	void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
-	void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta);
+	void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
+	void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+	void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const;
+	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr);
+	void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const;
 
 public:
 	void step(Space3DSW *p_space, real_t p_delta, int p_iterations);
 	Step3DSW();
+	~Step3DSW();
 };
 
 #endif // STEP__SW_H