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

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 @@ private:
 
 	typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;
 
+	SelfList<JoltArea3D> call_queries_element;
+
 	OverlapsById bodies_by_id;
 	OverlapsById areas_by_id;
 
@@ -115,8 +117,13 @@ private:
 
 	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 @@ public:
 	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,8 +1226,6 @@ 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) {
@@ -1227,7 +1233,7 @@ void JoltBody3D::pre_step(float p_step, JPH::Body &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: {

modules/jolt_physics/objects/jolt_body_3d.h

@@ -57,6 +57,8 @@ public:
 	};
 
 private:
+	SelfList<JoltBody3D> call_queries_element;
+
 	LocalVector<RID> exceptions;
 	LocalVector<Contact> contacts;
 	LocalVector<JoltArea3D *> areas;
@@ -96,7 +98,7 @@ private:
 
 	uint32_t locked_axes = 0;
 
-	bool sync_state = false;
+	bool sleep_allowed = true;
 	bool sleep_initially = false;
 	bool custom_center_of_mass = false;
 	bool custom_integrator = false;
@@ -108,11 +110,13 @@ private:
 
 	virtual void _add_to_space() override;
 
+	void _enqueue_call_queries();
+	void _dequeue_call_queries();
+
 	void _integrate_forces(float p_step, JPH::Body &p_jolt_body);
 
 	void _move_kinematic(float p_step, JPH::Body &p_jolt_body);
 
-	void _pre_step_static(float p_step, JPH::Body &p_jolt_body);
 	void _pre_step_rigid(float p_step, JPH::Body &p_jolt_body);
 	void _pre_step_kinematic(float p_step, JPH::Body &p_jolt_body);
 
@@ -128,6 +132,7 @@ private:
 	void _update_group_filter();
 	void _update_joint_constraints();
 	void _update_possible_kinematic_contacts();
+	void _update_sleep_allowed();
 
 	void _destroy_joint_constraints();
 
@@ -144,6 +149,7 @@ private:
 	void _exceptions_changed();
 	void _axis_lock_changed();
 	void _contact_reporting_changed();
+	void _sleep_allowed_changed();
 
 public:
 	JoltBody3D();
@@ -175,8 +181,9 @@ public:
 	void put_to_sleep() { set_is_sleeping(true); }
 	void wake_up() { set_is_sleeping(false); }
 
-	bool can_sleep() const;
-	void set_can_sleep(bool p_enabled);
+	bool is_sleep_allowed() const { return sleep_allowed; }
+	bool is_sleep_actually_allowed() const;
+	void set_is_sleep_allowed(bool p_enabled);
 
 	Basis get_principal_inertia_axes() const;
 	Vector3 get_inverse_inertia() const;
@@ -238,7 +245,7 @@ public:
 	void add_joint(JoltJoint3D *p_joint);
 	void remove_joint(JoltJoint3D *p_joint);
 
-	void call_queries(JPH::Body &p_jolt_body);
+	void call_queries();
 
 	virtual void pre_step(float p_step, JPH::Body &p_jolt_body) override;
 

modules/jolt_physics/objects/jolt_shaped_object_3d.cpp

@@ -160,13 +160,14 @@ void JoltShapedObject3D::_update_shape() {
 	const JoltWritableBody3D body = space->write_body(jolt_id);
 	ERR_FAIL_COND(body.is_invalid());
 
-	previous_jolt_shape = jolt_shape;
-	jolt_shape = build_shape();
-
-	if (jolt_shape == previous_jolt_shape) {
+	JPH::ShapeRefC new_shape = build_shape();
+	if (new_shape == jolt_shape) {
 		return;
 	}
 
+	previous_jolt_shape = jolt_shape;
+	jolt_shape = new_shape;
+
 	space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate);
 
 	_shapes_built();

modules/jolt_physics/objects/jolt_soft_body_3d.cpp

@@ -438,7 +438,7 @@ void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
 	}
 }
 
-bool JoltSoftBody3D::can_sleep() const {
+bool JoltSoftBody3D::is_sleep_allowed() const {
 	if (!in_space()) {
 		return true;
 	}
@@ -449,7 +449,7 @@ bool JoltSoftBody3D::can_sleep() const {
 	return body->GetAllowSleeping();
 }
 
-void JoltSoftBody3D::set_can_sleep(bool p_enabled) {
+void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) {
 	if (!in_space()) {
 		return;
 	}
@@ -532,7 +532,7 @@ Variant JoltSoftBody3D::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));
@@ -555,7 +555,7 @@ void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant
 			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));

modules/jolt_physics/objects/jolt_soft_body_3d.h

@@ -124,8 +124,8 @@ public:
 	bool is_sleeping() const;
 	void set_is_sleeping(bool p_enabled);
 
-	bool can_sleep() const;
-	void set_can_sleep(bool p_enabled);
+	bool is_sleep_allowed() const;
+	void set_is_sleep_allowed(bool p_enabled);
 
 	void put_to_sleep() { set_is_sleeping(true); }
 	void wake_up() { set_is_sleeping(false); }

modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp

@@ -86,10 +86,6 @@ void JoltContactListener3D::OnSoftBodyContactAdded(const JPH::Body &p_soft_body,
 
 #endif
 
-bool JoltContactListener3D::_is_listening_for(const JPH::Body &p_body) const {
-	return listening_for.has(p_body.GetID());
-}
-
 bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) {
 	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
 		return false;
@@ -178,16 +174,19 @@ bool JoltContactListener3D::_try_apply_surface_velocities(const JPH::Body &p_jol
 	return true;
 }
 
-bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
-	if (p_body1.IsSensor() || p_body2.IsSensor()) {
+bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
 		return false;
 	}
 
-	if (!_is_listening_for(p_body1) && !_is_listening_for(p_body2)) {
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	if (!body1->reports_contacts() && !body2->reports_contacts()) {
 		return false;
 	}
 
-	const JPH::SubShapeIDPair shape_pair(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
+	const JPH::SubShapeIDPair shape_pair(p_jolt_body1.GetID(), p_manifold.mSubShapeID1, p_jolt_body2.GetID(), p_manifold.mSubShapeID2);
 
 	Manifold &manifold = [&]() -> Manifold & {
 		const MutexLock write_lock(write_mutex);
@@ -201,8 +200,7 @@ bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JP
 	manifold.depth = p_manifold.mPenetrationDepth;
 
 	JPH::CollisionEstimationResult collision;
-
-	JPH::EstimateCollisionResponse(p_body1, p_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5);
+	JPH::EstimateCollisionResponse(p_jolt_body1, p_jolt_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5);
 
 	for (JPH::uint i = 0; i < contact_count; ++i) {
 		const JPH::RVec3 relative_point1 = JPH::RVec3(p_manifold.mRelativeContactPointsOn1[i]);
@@ -211,8 +209,8 @@ bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JP
 		const JPH::RVec3 world_point1 = p_manifold.mBaseOffset + relative_point1;
 		const JPH::RVec3 world_point2 = p_manifold.mBaseOffset + relative_point2;
 
-		const JPH::Vec3 velocity1 = p_body1.GetPointVelocity(world_point1);
-		const JPH::Vec3 velocity2 = p_body2.GetPointVelocity(world_point2);
+		const JPH::Vec3 velocity1 = p_jolt_body1.GetPointVelocity(world_point1);
+		const JPH::Vec3 velocity2 = p_jolt_body2.GetPointVelocity(world_point2);
 
 		const JPH::CollisionEstimationResult::Impulse &impulse = collision.mImpulses[i];
 
@@ -532,13 +530,7 @@ void JoltContactListener3D::_flush_area_exits() {
 	area_exits.clear();
 }
 
-void JoltContactListener3D::listen_for(JoltShapedObject3D *p_object) {
-	listening_for.insert(p_object->get_jolt_id());
-}
-
 void JoltContactListener3D::pre_step() {
-	listening_for.clear();
-
 #ifdef DEBUG_ENABLED
 	debug_contact_count = 0;
 #endif

modules/jolt_physics/spaces/jolt_contact_listener_3d.h

@@ -85,7 +85,6 @@ class JoltContactListener3D final
 	};
 
 	HashMap<JPH::SubShapeIDPair, Manifold, ShapePairHasher> manifolds_by_shape_pair;
-	HashSet<JPH::BodyID, BodyIDHasher> listening_for;
 	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_overlaps;
 	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_enters;
 	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_exits;
@@ -107,12 +106,10 @@ class JoltContactListener3D final
 	virtual void OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) override;
 #endif
 
-	bool _is_listening_for(const JPH::Body &p_body) const;
-
 	bool _try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
 	bool _try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings);
 	bool _try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
-	bool _try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings);
+	bool _try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings);
 	bool _try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold);
 	bool _try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair);
 	bool _try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair);
@@ -131,8 +128,6 @@ public:
 	explicit JoltContactListener3D(JoltSpace3D *p_space) :
 			space(p_space) {}
 
-	void listen_for(JoltShapedObject3D *p_object);
-
 	void pre_step();
 	void post_step();
 

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -63,55 +63,36 @@ constexpr double DEFAULT_SOLVER_ITERATIONS = 8;
 } // namespace
 
 void JoltSpace3D::_pre_step(float p_step) {
-	body_accessor.acquire_all();
-
 	contact_listener->pre_step();
 
-	const int body_count = body_accessor.get_count();
-
-	for (int i = 0; i < body_count; ++i) {
-		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
-			if (jolt_body->IsSoftBody()) {
-				continue;
-			}
-
-			JoltShapedObject3D *object = reinterpret_cast<JoltShapedObject3D *>(jolt_body->GetUserData());
-
-			object->pre_step(p_step, *jolt_body);
+	const JPH::BodyLockInterface &lock_iface = get_lock_iface();
+	const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody);
+	const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody);
 
-			if (object->reports_contacts()) {
-				contact_listener->listen_for(object);
-			}
-		}
+	for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) {
+		JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]);
+		JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+		object->pre_step(p_step, *jolt_body);
 	}
-
-	body_accessor.release();
 }
 
 void JoltSpace3D::_post_step(float p_step) {
-	body_accessor.acquire_all();
-
 	contact_listener->post_step();
 
-	const int body_count = body_accessor.get_count();
-
-	for (int i = 0; i < body_count; ++i) {
-		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
-			if (jolt_body->IsSoftBody()) {
-				continue;
-			}
+	// WARNING: The list of active bodies may have changed between `pre_step` and `post_step`.
 
-			JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+	const JPH::BodyLockInterface &lock_iface = get_lock_iface();
+	const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody);
+	const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody);
 
-			object->post_step(p_step, *jolt_body);
-		}
+	for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) {
+		JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]);
+		JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+		object->post_step(p_step, *jolt_body);
 	}
-
-	body_accessor.release();
 }
 
 JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
-		body_accessor(this),
 		job_system(p_job_system),
 		temp_allocator(new JoltTempAllocator()),
 		layers(new JoltLayers()),
@@ -208,44 +189,21 @@ void JoltSpace3D::step(float p_step) {
 	_post_step(p_step);
 
 	bodies_added_since_optimizing = 0;
-	has_stepped = true;
 	stepping = false;
 }
 
 void JoltSpace3D::call_queries() {
-	if (!has_stepped) {
-		// We need to skip the first invocation of this method, because there will be pending notifications that need to
-		// be flushed first, which can cause weird conflicts with things like `_integrate_forces`. This happens to also
-		// emulate the behavior of Godot Physics, where (active) collision objects must register to have `call_queries`
-		// invoked, which they don't do until the physics step, which happens after this.
-		//
-		// TODO: This would be better solved by just doing what Godot Physics does with `GodotSpace*D::active_list`.
-		return;
-	}
-
-	body_accessor.acquire_all();
-
-	const int body_count = body_accessor.get_count();
-
-	for (int i = 0; i < body_count; ++i) {
-		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
-			if (!jolt_body->IsSensor() && !jolt_body->IsSoftBody()) {
-				JoltBody3D *body = reinterpret_cast<JoltBody3D *>(jolt_body->GetUserData());
-				body->call_queries(*jolt_body);
-			}
-		}
+	while (body_call_queries_list.first()) {
+		JoltBody3D *body = body_call_queries_list.first()->self();
+		body_call_queries_list.remove(body_call_queries_list.first());
+		body->call_queries();
 	}
 
-	for (int i = 0; i < body_count; ++i) {
-		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
-			if (jolt_body->IsSensor()) {
-				JoltArea3D *area = reinterpret_cast<JoltArea3D *>(jolt_body->GetUserData());
-				area->call_queries(*jolt_body);
-			}
-		}
+	while (area_call_queries_list.first()) {
+		JoltArea3D *body = area_call_queries_list.first()->self();
+		area_call_queries_list.remove(area_call_queries_list.first());
+		body->call_queries();
 	}
-
-	body_accessor.release();
 }
 
 double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
@@ -445,6 +403,30 @@ void JoltSpace3D::try_optimize() {
 	bodies_added_since_optimizing = 0;
 }
 
+void JoltSpace3D::enqueue_call_queries(SelfList<JoltBody3D> *p_body) {
+	if (!p_body->in_list()) {
+		body_call_queries_list.add(p_body);
+	}
+}
+
+void JoltSpace3D::enqueue_call_queries(SelfList<JoltArea3D> *p_area) {
+	if (!p_area->in_list()) {
+		area_call_queries_list.add(p_area);
+	}
+}
+
+void JoltSpace3D::dequeue_call_queries(SelfList<JoltBody3D> *p_body) {
+	if (p_body->in_list()) {
+		body_call_queries_list.remove(p_body);
+	}
+}
+
+void JoltSpace3D::dequeue_call_queries(SelfList<JoltArea3D> *p_area) {
+	if (p_area->in_list()) {
+		area_call_queries_list.remove(p_area);
+	}
+}
+
 void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) {
 	physics_system->AddConstraint(p_jolt_ref);
 }

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -48,6 +48,7 @@
 #include <stdint.h>
 
 class JoltArea3D;
+class JoltBody3D;
 class JoltContactListener3D;
 class JoltJoint3D;
 class JoltLayers;
@@ -55,7 +56,8 @@ class JoltObject3D;
 class JoltPhysicsDirectSpaceState3D;
 
 class JoltSpace3D {
-	JoltBodyWriter3D body_accessor;
+	SelfList<JoltBody3D>::List body_call_queries_list;
+	SelfList<JoltArea3D>::List area_call_queries_list;
 
 	RID rid;
 
@@ -73,7 +75,6 @@ class JoltSpace3D {
 
 	bool active = false;
 	bool stepping = false;
-	bool has_stepped = false;
 
 	void _pre_step(float p_step);
 	void _post_step(float p_step);
@@ -132,6 +133,11 @@ public:
 
 	void try_optimize();
 
+	void enqueue_call_queries(SelfList<JoltBody3D> *p_body);
+	void enqueue_call_queries(SelfList<JoltArea3D> *p_area);
+	void dequeue_call_queries(SelfList<JoltBody3D> *p_body);
+	void dequeue_call_queries(SelfList<JoltArea3D> *p_area);
+
 	void add_joint(JPH::Constraint *p_jolt_ref);
 	void add_joint(JoltJoint3D *p_joint);
 	void remove_joint(JPH::Constraint *p_jolt_ref);