Fix transform changes in `_integrate_forces` being overwritten

scene/2d/physics_body_2d.cpp

@@ -431,7 +431,9 @@ struct _RigidBody2DInOut {
 
 void RigidBody2D::_sync_body_state(PhysicsDirectBodyState2D *p_state) {
 	if (!freeze || freeze_mode != FREEZE_MODE_KINEMATIC) {
+		set_block_transform_notify(true);
 		set_global_transform(p_state->get_transform());
+		set_block_transform_notify(false);
 	}
 
 	linear_velocity = p_state->get_linear_velocity();
@@ -446,16 +448,16 @@ void RigidBody2D::_sync_body_state(PhysicsDirectBodyState2D *p_state) {
 void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
 	lock_callback();
 
-	set_block_transform_notify(true); // don't want notify (would feedback loop)
-
 	if (GDVIRTUAL_IS_OVERRIDDEN(_integrate_forces)) {
 		_sync_body_state(p_state);
 
 		GDVIRTUAL_CALL(_integrate_forces, p_state);
+
+		// Update the physics server with any new transform, to prevent it from being overwritten at the sync below.
+		force_update_transform();
 	}
 
 	_sync_body_state(p_state);
-	set_block_transform_notify(false); // want it back
 
 	if (contact_monitor) {
 		contact_monitor->locked = true;

scene/3d/physics_body_3d.cpp

@@ -485,7 +485,9 @@ struct _RigidBodyInOut {
 };
 
 void RigidBody3D::_sync_body_state(PhysicsDirectBodyState3D *p_state) {
+	set_ignore_transform_notification(true);
 	set_global_transform(p_state->get_transform());
+	set_ignore_transform_notification(false);
 
 	linear_velocity = p_state->get_linear_velocity();
 	angular_velocity = p_state->get_angular_velocity();
@@ -501,16 +503,16 @@ void RigidBody3D::_sync_body_state(PhysicsDirectBodyState3D *p_state) {
 void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	lock_callback();
 
-	set_ignore_transform_notification(true);
-
 	if (GDVIRTUAL_IS_OVERRIDDEN(_integrate_forces)) {
 		_sync_body_state(p_state);
 
 		GDVIRTUAL_CALL(_integrate_forces, p_state);
+
+		// Update the physics server with any new transform, to prevent it from being overwritten at the sync below.
+		force_update_transform();
 	}
 
 	_sync_body_state(p_state);
-	set_ignore_transform_notification(false);
 	_on_transform_changed();
 
 	if (contact_monitor) {
@@ -2927,7 +2929,10 @@ void PhysicalBone3D::_notification(int p_what) {
 }
 
 void PhysicalBone3D::_sync_body_state(PhysicsDirectBodyState3D *p_state) {
+	set_ignore_transform_notification(true);
 	set_global_transform(p_state->get_transform());
+	set_ignore_transform_notification(false);
+
 	linear_velocity = p_state->get_linear_velocity();
 	angular_velocity = p_state->get_angular_velocity();
 }
@@ -2937,16 +2942,16 @@ void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 		return;
 	}
 
-	set_ignore_transform_notification(true);
-
 	if (GDVIRTUAL_IS_OVERRIDDEN(_integrate_forces)) {
 		_sync_body_state(p_state);
 
 		GDVIRTUAL_CALL(_integrate_forces, p_state);
+
+		// Update the physics server with any new transform, to prevent it from being overwritten at the sync below.
+		force_update_transform();
 	}
 
 	_sync_body_state(p_state);
-	set_ignore_transform_notification(false);
 	_on_transform_changed();
 
 	Transform3D global_transform(p_state->get_transform());