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)

1 files changed, 100 insertions, 37 deletions

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;