Fix kinematic bodies not synchronizing state when using Jolt Physics

modules/jolt_physics/objects/jolt_body_3d.cpp

@@ -210,20 +210,6 @@ void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) {
 	p_jolt_body.MoveKinematic(new_position, new_rotation, p_step);
 }
 
-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()) {
-		_enqueue_call_queries();
-	}
-}
-
 JPH::EAllowedDOFs JoltBody3D::_calculate_allowed_dofs() const {
 	if (is_static()) {
 		return JPH::EAllowedDOFs::All;
@@ -1237,13 +1223,18 @@ void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
 		} break;
 		case PhysicsServer3D::BODY_MODE_RIGID:
 		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
-			_pre_step_rigid(p_step, p_jolt_body);
+			_integrate_forces(p_step, p_jolt_body);
 		} break;
 		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
-			_pre_step_kinematic(p_step, p_jolt_body);
+			_update_gravity(p_jolt_body);
+			_move_kinematic(p_step, p_jolt_body);
 		} break;
 	}
 
+	if (_should_call_queries()) {
+		_enqueue_call_queries();
+	}
+
 	contact_count = 0;
 }
 

modules/jolt_physics/objects/jolt_body_3d.h

@@ -110,6 +110,7 @@ private:
 
 	virtual void _add_to_space() override;
 
+	bool _should_call_queries() const { return state_sync_callback.is_valid() || custom_integration_callback.is_valid(); }
 	void _enqueue_call_queries();
 	void _dequeue_call_queries();
 
@@ -117,9 +118,6 @@ private:
 
 	void _move_kinematic(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);
-
 	JPH::EAllowedDOFs _calculate_allowed_dofs() const;
 
 	JPH::MassProperties _calculate_mass_properties(const JPH::Shape &p_shape) const;