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)

10 files changed, 129 insertions, 53 deletions

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