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, 66 insertions, 36 deletions

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);
 }