Merge pull request #100983 from mihe/jolt/only-iterate-active-bodies

Improve performance with many static/sleeping bodies when using Jolt Physics

Author: akien-mga

modules/jolt_physics/objects/jolt_area_3d.cpp

@@ -59,6 +59,26 @@ JPH::ObjectLayer JoltArea3D::_get_object_layer() const {
 	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
 }
 
+bool JoltArea3D::_has_pending_events() const {
+	if (body_monitor_callback.is_valid()) {
+		for (const KeyValue<JPH::BodyID, Overlap> &E : bodies_by_id) {
+			if (!E.value.pending_added.is_empty() || !E.value.pending_removed.is_empty()) {
+				return true;
+			}
+		}
+	}
+
+	if (area_monitor_callback.is_valid()) {
+		for (const KeyValue<JPH::BodyID, Overlap> &E : areas_by_id) {
+			if (!E.value.pending_added.is_empty() || !E.value.pending_removed.is_empty()) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
 void JoltArea3D::_add_to_space() {
 	jolt_shape = build_shape();
 
@@ -90,6 +110,18 @@ void JoltArea3D::_add_to_space() {
 	jolt_settings = nullptr;
 }
 
+void JoltArea3D::_enqueue_call_queries() {
+	if (space != nullptr) {
+		space->enqueue_call_queries(&call_queries_element);
+	}
+}
+
+void JoltArea3D::_dequeue_call_queries() {
+	if (space != nullptr) {
+		space->dequeue_call_queries(&call_queries_element);
+	}
+}
+
 void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
 	const JoltReadableBody3D other_jolt_body = space->read_body(p_body_id);
 	const JoltShapedObject3D *other_object = other_jolt_body.as_shaped();
@@ -270,6 +302,8 @@ void JoltArea3D::_space_changing() {
 		_force_bodies_exited(true);
 		_force_areas_exited(true);
 	}
+
+	_dequeue_call_queries();
 }
 
 void JoltArea3D::_space_changed() {
@@ -304,7 +338,8 @@ void JoltArea3D::_gravity_changed() {
 }
 
 JoltArea3D::JoltArea3D() :
-		JoltShapedObject3D(OBJECT_TYPE_AREA) {
+		JoltShapedObject3D(OBJECT_TYPE_AREA),
+		call_queries_element(this) {
 }
 
 bool JoltArea3D::is_default_area() const {
@@ -659,7 +694,13 @@ void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) {
 	overlap->shape_pairs.clear();
 }
 
-void JoltArea3D::call_queries(JPH::Body &p_jolt_body) {
+void JoltArea3D::call_queries() {
 	_flush_events(bodies_by_id, body_monitor_callback);
 	_flush_events(areas_by_id, area_monitor_callback);
 }
+
+void JoltArea3D::post_step(float p_step, JPH::Body &p_jolt_body) {
+	if (_has_pending_events()) {
+		_enqueue_call_queries();
+	}
+}

modules/jolt_physics/objects/jolt_area_3d.h

@@ -89,6 +89,8 @@ class JoltArea3D final : public JoltShapedObject3D {
 
 	typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;
 
+	SelfList<JoltArea3D> call_queries_element;
+
 	OverlapsById bodies_by_id;
 	OverlapsById areas_by_id;
 
@@ -115,8 +117,13 @@ class JoltArea3D final : public JoltShapedObject3D {
 
 	virtual JPH::EMotionType _get_motion_type() const override { return JPH::EMotionType::Kinematic; }
 
+	bool _has_pending_events() const;
+
 	virtual void _add_to_space() override;
 
+	void _enqueue_call_queries();
+	void _dequeue_call_queries();
+
 	void _add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
 	bool _remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
 
@@ -217,10 +224,12 @@ class JoltArea3D final : public JoltShapedObject3D {
 	void body_exited(const JPH::BodyID &p_body_id, bool p_notify = true);
 	void area_exited(const JPH::BodyID &p_body_id);
 
-	void call_queries(JPH::Body &p_jolt_body);
+	void call_queries();
 
 	virtual bool has_custom_center_of_mass() const override { return false; }
 	virtual Vector3 get_center_of_mass_custom() const override { return Vector3(); }
+
+	virtual void post_step(float p_step, JPH::Body &p_jolt_body) override;
 };
 
 #endif // JOLT_AREA_3D_H

modules/jolt_physics/objects/jolt_body_3d.cpp

@@ -128,6 +128,7 @@ void JoltBody3D::_add_to_space() {
 	jolt_settings->mAllowDynamicOrKinematic = true;
 	jolt_settings->mCollideKinematicVsNonDynamic = reports_all_kinematic_contacts();
 	jolt_settings->mUseManifoldReduction = !reports_contacts();
+	jolt_settings->mAllowSleeping = is_sleep_actually_allowed();
 	jolt_settings->mLinearDamping = 0.0f;
 	jolt_settings->mAngularDamping = 0.0f;
 	jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity();
@@ -153,11 +154,19 @@ void JoltBody3D::_add_to_space() {
 	jolt_settings = nullptr;
 }
 
-void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) {
-	if (!p_jolt_body.IsActive()) {
-		return;
+void JoltBody3D::_enqueue_call_queries() {
+	if (space != nullptr) {
+		space->enqueue_call_queries(&call_queries_element);
+	}
+}
+
+void JoltBody3D::_dequeue_call_queries() {
+	if (space != nullptr) {
+		space->dequeue_call_queries(&call_queries_element);
 	}
+}
 
+void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) {
 	_update_gravity(p_jolt_body);
 
 	if (!custom_integrator) {
@@ -182,8 +191,6 @@ void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) {
 		p_jolt_body.AddForce(to_jolt(constant_force));
 		p_jolt_body.AddTorque(to_jolt(constant_torque));
 	}
-
-	sync_state = true;
 }
 
 void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) {
@@ -201,27 +208,19 @@ void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) {
 	}
 
 	p_jolt_body.MoveKinematic(new_position, new_rotation, p_step);
-
-	sync_state = true;
-}
-
-void JoltBody3D::_pre_step_static(float p_step, JPH::Body &p_jolt_body) {
-	// Nothing to do.
 }
 
 void JoltBody3D::_pre_step_rigid(float p_step, JPH::Body &p_jolt_body) {
 	_integrate_forces(p_step, p_jolt_body);
+	_enqueue_call_queries();
 }
 
 void JoltBody3D::_pre_step_kinematic(float p_step, JPH::Body &p_jolt_body) {
 	_update_gravity(p_jolt_body);
-
 	_move_kinematic(p_step, p_jolt_body);
 
 	if (reports_contacts()) {
-		// This seems to emulate the behavior of Godot Physics, where kinematic bodies are set as active (and thereby
-		// have their state synchronized on every step) only if its max reported contacts is non-zero.
-		sync_state = true;
+		_enqueue_call_queries();
 	}
 }
 
@@ -428,6 +427,20 @@ void JoltBody3D::_update_possible_kinematic_contacts() {
 	}
 }
 
+void JoltBody3D::_update_sleep_allowed() {
+	const bool value = is_sleep_actually_allowed();
+
+	if (!in_space()) {
+		jolt_settings->mAllowSleeping = value;
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->SetAllowSleeping(value);
+}
+
 void JoltBody3D::_destroy_joint_constraints() {
 	for (JoltJoint3D *joint : joints) {
 		joint->destroy();
@@ -460,6 +473,7 @@ void JoltBody3D::_mode_changed() {
 	_update_object_layer();
 	_update_kinematic_transform();
 	_update_mass_properties();
+	_update_sleep_allowed();
 	wake_up();
 }
 
@@ -478,6 +492,7 @@ void JoltBody3D::_space_changing() {
 
 	_destroy_joint_constraints();
 	_exit_all_areas();
+	_dequeue_call_queries();
 }
 
 void JoltBody3D::_space_changed() {
@@ -486,9 +501,8 @@ void JoltBody3D::_space_changed() {
 	_update_kinematic_transform();
 	_update_group_filter();
 	_update_joint_constraints();
+	_update_sleep_allowed();
 	_areas_changed();
-
-	sync_state = false;
 }
 
 void JoltBody3D::_areas_changed() {
@@ -519,11 +533,18 @@ void JoltBody3D::_axis_lock_changed() {
 
 void JoltBody3D::_contact_reporting_changed() {
 	_update_possible_kinematic_contacts();
+	_update_sleep_allowed();
+	wake_up();
+}
+
+void JoltBody3D::_sleep_allowed_changed() {
+	_update_sleep_allowed();
 	wake_up();
 }
 
 JoltBody3D::JoltBody3D() :
-		JoltShapedObject3D(OBJECT_TYPE_BODY) {
+		JoltShapedObject3D(OBJECT_TYPE_BODY),
+		call_queries_element(this) {
 }
 
 JoltBody3D::~JoltBody3D() {
@@ -573,7 +594,7 @@ Variant JoltBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
 			return is_sleeping();
 		}
 		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
-			return can_sleep();
+			return is_sleep_allowed();
 		}
 		default: {
 			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
@@ -596,7 +617,7 @@ void JoltBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_
 			set_is_sleeping(p_value);
 		} break;
 		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
-			set_can_sleep(p_value);
+			set_is_sleep_allowed(p_value);
 		} break;
 		default: {
 			ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
@@ -712,6 +733,10 @@ bool JoltBody3D::is_sleeping() const {
 	return !body->IsActive();
 }
 
+bool JoltBody3D::is_sleep_actually_allowed() const {
+	return sleep_allowed && !(is_kinematic() && reports_contacts());
+}
+
 void JoltBody3D::set_is_sleeping(bool p_enabled) {
 	if (!in_space()) {
 		sleep_initially = p_enabled;
@@ -727,27 +752,14 @@ void JoltBody3D::set_is_sleeping(bool p_enabled) {
 	}
 }
 
-bool JoltBody3D::can_sleep() const {
-	if (!in_space()) {
-		return jolt_settings->mAllowSleeping;
-	}
-
-	const JoltReadableBody3D body = space->read_body(jolt_id);
-	ERR_FAIL_COND_V(body.is_invalid(), false);
-
-	return body->GetAllowSleeping();
-}
-
-void JoltBody3D::set_can_sleep(bool p_enabled) {
-	if (!in_space()) {
-		jolt_settings->mAllowSleeping = p_enabled;
+void JoltBody3D::set_is_sleep_allowed(bool p_enabled) {
+	if (sleep_allowed == p_enabled) {
 		return;
 	}
 
-	const JoltWritableBody3D body = space->write_body(jolt_id);
-	ERR_FAIL_COND(body.is_invalid());
+	sleep_allowed = p_enabled;
 
-	body->SetAllowSleeping(p_enabled);
+	_sleep_allowed_changed();
 }
 
 Basis JoltBody3D::get_principal_inertia_axes() const {
@@ -1187,11 +1199,7 @@ void JoltBody3D::remove_joint(JoltJoint3D *p_joint) {
 	_joints_changed();
 }
 
-void JoltBody3D::call_queries(JPH::Body &p_jolt_body) {
-	if (!sync_state) {
-		return;
-	}
-
+void JoltBody3D::call_queries() {
 	if (custom_integration_callback.is_valid()) {
 		const Variant direct_state_variant = get_direct_state();
 		const Variant *args[2] = { &direct_state_variant, &custom_integration_userdata };
@@ -1218,16 +1226,14 @@ void JoltBody3D::call_queries(JPH::Body &p_jolt_body) {
 			ERR_PRINT_ONCE(vformat("Failed to call state synchronization callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(state_sync_callback, args, 1, ce)));
 		}
 	}
-
-	sync_state = false;
 }
 
 void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
 	JoltObject3D::pre_step(p_step, p_jolt_body);
 
 	switch (mode) {
 		case PhysicsServer3D::BODY_MODE_STATIC: {
-			_pre_step_static(p_step, p_jolt_body);
+			// Will never happen.
 		} break;
 		case PhysicsServer3D::BODY_MODE_RIGID:
 		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {