Refactor post-step operations in Jolt module to be done as needed

(cherry picked from commit a30410b)

modules/jolt_physics/objects/jolt_area_3d.cpp

@@ -61,26 +61,6 @@ 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_shapes(true);
 
@@ -138,6 +118,8 @@ void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_i
 	shape_indices.self = find_shape_index(p_self_shape_id);
 
 	p_overlap.pending_added.push_back(shape_indices);
+
+	_events_changed();
 }
 
 bool JoltArea3D::_remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
@@ -150,6 +132,8 @@ bool JoltArea3D::_remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p
 	p_overlap.pending_removed.push_back(shape_pair->value);
 	p_overlap.shape_pairs.remove(shape_pair);
 
+	_events_changed();
+
 	return true;
 }
 
@@ -226,10 +210,16 @@ void JoltArea3D::_force_bodies_entered() {
 	for (KeyValue<JPH::BodyID, Overlap> &E : bodies_by_id) {
 		Overlap &body = E.value;
 
+		if (unlikely(body.shape_pairs.is_empty())) {
+			continue;
+		}
+
 		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : body.shape_pairs) {
 			body.pending_removed.erase(P.value);
 			body.pending_added.push_back(P.value);
 		}
+
+		_events_changed();
 	}
 }
 
@@ -238,11 +228,17 @@ void JoltArea3D::_force_bodies_exited(bool p_remove) {
 		const JPH::BodyID &id = E.key;
 		Overlap &body = E.value;
 
+		if (unlikely(body.shape_pairs.is_empty())) {
+			continue;
+		}
+
 		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : body.shape_pairs) {
 			body.pending_added.erase(P.value);
 			body.pending_removed.push_back(P.value);
 		}
 
+		_events_changed();
+
 		if (p_remove) {
 			body.shape_pairs.clear();
 			_notify_body_exited(id);
@@ -254,22 +250,34 @@ void JoltArea3D::_force_areas_entered() {
 	for (KeyValue<JPH::BodyID, Overlap> &E : areas_by_id) {
 		Overlap &area = E.value;
 
+		if (unlikely(area.shape_pairs.is_empty())) {
+			continue;
+		}
+
 		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : area.shape_pairs) {
 			area.pending_removed.erase(P.value);
 			area.pending_added.push_back(P.value);
 		}
+
+		_events_changed();
 	}
 }
 
 void JoltArea3D::_force_areas_exited(bool p_remove) {
 	for (KeyValue<JPH::BodyID, Overlap> &E : areas_by_id) {
 		Overlap &area = E.value;
 
+		if (unlikely(area.shape_pairs.is_empty())) {
+			continue;
+		}
+
 		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : area.shape_pairs) {
 			area.pending_added.erase(P.value);
 			area.pending_removed.push_back(P.value);
 		}
 
+		_events_changed();
+
 		if (p_remove) {
 			area.shape_pairs.clear();
 		}
@@ -315,6 +323,10 @@ void JoltArea3D::_space_changed() {
 	_update_default_gravity();
 }
 
+void JoltArea3D::_events_changed() {
+	_enqueue_call_queries();
+}
+
 void JoltArea3D::_body_monitoring_changed() {
 	if (has_body_monitor_callback()) {
 		_force_bodies_entered();
@@ -666,11 +678,13 @@ void JoltArea3D::body_exited(const JPH::BodyID &p_body_id, bool p_notify) {
 		return;
 	}
 
-	for (KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
+	for (const KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
 		overlap->pending_added.erase(E.value);
 		overlap->pending_removed.push_back(E.value);
 	}
 
+	_events_changed();
+
 	overlap->shape_pairs.clear();
 
 	if (p_notify) {
@@ -693,16 +707,12 @@ void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) {
 		overlap->pending_removed.push_back(E.value);
 	}
 
+	_events_changed();
+
 	overlap->shape_pairs.clear();
 }
 
 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

@@ -119,8 +119,6 @@ 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();
@@ -147,6 +145,7 @@ class JoltArea3D final : public JoltShapedObject3D {
 
 	virtual void _space_changing() override;
 	virtual void _space_changed() override;
+	void _events_changed();
 	void _body_monitoring_changed();
 	void _area_monitoring_changed();
 	void _monitorable_changed();
@@ -230,8 +229,6 @@ class JoltArea3D final : public JoltShapedObject3D {
 
 	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_object_3d.cpp

@@ -138,12 +138,6 @@ bool JoltObject3D::can_interact_with(const JoltObject3D &p_other) const {
 	}
 }
 
-void JoltObject3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
-}
-
-void JoltObject3D::post_step(float p_step, JPH::Body &p_jolt_body) {
-}
-
 String JoltObject3D::to_string() const {
 	Object *instance = get_instance();
 	return instance != nullptr ? instance->to_string() : "<unknown>";

modules/jolt_physics/objects/jolt_object_3d.h

@@ -150,8 +150,7 @@ class JoltObject3D {
 
 	virtual bool reports_contacts() const = 0;
 
-	virtual void pre_step(float p_step, JPH::Body &p_jolt_body);
-	virtual void post_step(float p_step, JPH::Body &p_jolt_body);
+	virtual void pre_step(float p_step, JPH::Body &p_jolt_body) {}
 
 	String to_string() const;
 };

modules/jolt_physics/objects/jolt_shaped_object_3d.cpp

@@ -142,14 +142,26 @@ JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape(bool p_optimize) {
 	return shape_result.Get();
 }
 
+void JoltShapedObject3D::_enqueue_shapes_changed() {
+	if (space != nullptr) {
+		space->enqueue_shapes_changed(&shapes_changed_element);
+	}
+}
+
+void JoltShapedObject3D::_dequeue_shapes_changed() {
+	if (space != nullptr) {
+		space->dequeue_shapes_changed(&shapes_changed_element);
+	}
+}
+
 void JoltShapedObject3D::_enqueue_needs_optimization() {
-	if (!needs_optimization_element.in_list()) {
+	if (space != nullptr) {
 		space->enqueue_needs_optimization(&needs_optimization_element);
 	}
 }
 
 void JoltShapedObject3D::_dequeue_needs_optimization() {
-	if (needs_optimization_element.in_list()) {
+	if (space != nullptr) {
 		space->dequeue_needs_optimization(&needs_optimization_element);
 	}
 }
@@ -162,8 +174,11 @@ void JoltShapedObject3D::_shapes_changed() {
 void JoltShapedObject3D::_space_changing() {
 	JoltObject3D::_space_changing();
 
+	_dequeue_shapes_changed();
 	_dequeue_needs_optimization();
 
+	previous_jolt_shape = nullptr;
+
 	if (space != nullptr) {
 		const JoltWritableBody3D body = space->write_body(jolt_id);
 		ERR_FAIL_COND(body.is_invalid());
@@ -174,6 +189,7 @@ void JoltShapedObject3D::_space_changing() {
 
 JoltShapedObject3D::JoltShapedObject3D(ObjectType p_object_type) :
 		JoltObject3D(p_object_type),
+		shapes_changed_element(this),
 		needs_optimization_element(this) {
 	jolt_settings->mAllowSleeping = true;
 	jolt_settings->mFriction = 1.0f;
@@ -319,6 +335,8 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
 
 	space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate);
 
+	_enqueue_shapes_changed();
+
 	if (!p_optimize_compound && jolt_shape->GetType() == JPH::EShapeType::Compound) {
 		_enqueue_needs_optimization();
 	} else {
@@ -371,6 +389,10 @@ void JoltShapedObject3D::clear_shapes() {
 	_shapes_changed();
 }
 
+void JoltShapedObject3D::clear_previous_shape() {
+	previous_jolt_shape = nullptr;
+}
+
 int JoltShapedObject3D::find_shape_index(uint32_t p_shape_instance_id) const {
 	for (int i = 0; i < (int)shapes.size(); ++i) {
 		if (shapes[i].get_id() == p_shape_instance_id) {
@@ -452,9 +474,3 @@ void JoltShapedObject3D::set_shape_disabled(int p_index, bool p_disabled) {
 
 	_shapes_changed();
 }
-
-void JoltShapedObject3D::post_step(float p_step, JPH::Body &p_jolt_body) {
-	JoltObject3D::post_step(p_step, p_jolt_body);
-
-	previous_jolt_shape = nullptr;
-}

modules/jolt_physics/objects/jolt_shaped_object_3d.h

@@ -46,6 +46,7 @@ class JoltShapedObject3D : public JoltObject3D {
 	friend class JoltShape3D;
 
 protected:
+	SelfList<JoltShapedObject3D> shapes_changed_element;
 	SelfList<JoltShapedObject3D> needs_optimization_element;
 
 	Vector3 scale = Vector3(1, 1, 1);
@@ -63,6 +64,9 @@ class JoltShapedObject3D : public JoltObject3D {
 	JPH::ShapeRefC _try_build_single_shape();
 	JPH::ShapeRefC _try_build_compound_shape(bool p_optimize);
 
+	void _enqueue_shapes_changed();
+	void _dequeue_shapes_changed();
+
 	void _enqueue_needs_optimization();
 	void _dequeue_needs_optimization();
 
@@ -108,6 +112,7 @@ class JoltShapedObject3D : public JoltObject3D {
 	void set_shape(int p_index, JoltShape3D *p_shape);
 
 	void clear_shapes();
+	void clear_previous_shape();
 
 	int get_shape_count() const { return shapes.size(); }
 
@@ -125,8 +130,6 @@ class JoltShapedObject3D : public JoltObject3D {
 
 	bool is_shape_disabled(int p_index) const;
 	void set_shape_disabled(int p_index, bool p_disabled);
-
-	virtual void post_step(float p_step, JPH::Body &p_jolt_body) override;
 };
 
 #endif // JOLT_SHAPED_OBJECT_3D_H

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -87,16 +87,10 @@ void JoltSpace3D::_pre_step(float p_step) {
 void JoltSpace3D::_post_step(float p_step) {
 	contact_listener->post_step();
 
-	// WARNING: The list of active bodies may have changed between `pre_step` and `post_step`.
-
-	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);
-
-	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);
+	while (shapes_changed_list.first()) {
+		JoltShapedObject3D *object = shapes_changed_list.first()->self();
+		shapes_changed_list.remove(shapes_changed_list.first());
+		object->clear_previous_shape();
 	}
 }
 
@@ -435,6 +429,18 @@ void JoltSpace3D::dequeue_call_queries(SelfList<JoltArea3D> *p_area) {
 	}
 }
 
+void JoltSpace3D::enqueue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
+	if (!p_object->in_list()) {
+		shapes_changed_list.add(p_object);
+	}
+}
+
+void JoltSpace3D::dequeue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
+	if (p_object->in_list()) {
+		shapes_changed_list.remove(p_object);
+	}
+}
+
 void JoltSpace3D::enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
 	if (!p_object->in_list()) {
 		needs_optimization_list.add(p_object);

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -61,6 +61,7 @@ class JoltShapedObject3D;
 class JoltSpace3D {
 	SelfList<JoltBody3D>::List body_call_queries_list;
 	SelfList<JoltArea3D>::List area_call_queries_list;
+	SelfList<JoltShapedObject3D>::List shapes_changed_list;
 	SelfList<JoltShapedObject3D>::List needs_optimization_list;
 
 	RID rid;
@@ -144,6 +145,9 @@ class JoltSpace3D {
 	void dequeue_call_queries(SelfList<JoltBody3D> *p_body);
 	void dequeue_call_queries(SelfList<JoltArea3D> *p_area);
 
+	void enqueue_shapes_changed(SelfList<JoltShapedObject3D> *p_object);
+	void dequeue_shapes_changed(SelfList<JoltShapedObject3D> *p_object);
+
 	void enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);
 	void dequeue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);