From c11825686589696f4c03948c11068a30c6c91796 Mon Sep 17 00:00:00 2001
From: Chinmay Awale <chinubeta0710@gmail.com>
Date: Fri, 28 Jul 2023 10:09:54 +0530
Subject: [PATCH] added state sync after call to _integrate_forces

---
 scene/2d/physics_body_2d.cpp | 13 +++++++++----
 scene/2d/physics_body_2d.h   |  2 ++
 scene/3d/physics_body_3d.cpp | 32 ++++++++++++++++++++------------
 scene/3d/physics_body_3d.h   |  3 +++
 4 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index b9bde47507d..2dcf87ba81c 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -429,10 +429,7 @@ struct _RigidBody2DInOut {
 	int local_shape = 0;
 };
 
-void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
-	lock_callback();
-
-	set_block_transform_notify(true); // don't want notify (would feedback loop)
+void RigidBody2D::_sync_body_state(PhysicsDirectBodyState2D *p_state) {
 	if (!freeze || freeze_mode != FREEZE_MODE_KINEMATIC) {
 		set_global_transform(p_state->get_transform());
 	}
@@ -444,9 +441,17 @@ void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
 		sleeping = p_state->is_sleeping();
 		emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
 	}
+}
+
+void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
+	lock_callback();
+
+	set_block_transform_notify(true); // don't want notify (would feedback loop)
+	_sync_body_state(p_state);
 
 	GDVIRTUAL_CALL(_integrate_forces, p_state);
 
+	_sync_body_state(p_state);
 	set_block_transform_notify(false); // want it back
 
 	if (contact_monitor) {
diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h
index c4eb77d8613..f5448ead408 100644
--- a/scene/2d/physics_body_2d.h
+++ b/scene/2d/physics_body_2d.h
@@ -212,6 +212,8 @@ private:
 	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state);
 	void _body_state_changed(PhysicsDirectBodyState2D *p_state);
 
+	void _sync_body_state(PhysicsDirectBodyState2D *p_state);
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index e49b43c6501..cf1b865b197 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -484,10 +484,7 @@ struct _RigidBodyInOut {
 	int local_shape = 0;
 };
 
-void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
-	lock_callback();
-
-	set_ignore_transform_notification(true);
+void RigidBody3D::_sync_body_state(PhysicsDirectBodyState3D *p_state) {
 	set_global_transform(p_state->get_transform());
 
 	linear_velocity = p_state->get_linear_velocity();
@@ -499,9 +496,17 @@ void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 		sleeping = p_state->is_sleeping();
 		emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
 	}
+}
+
+void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
+	lock_callback();
+
+	set_ignore_transform_notification(true);
+	_sync_body_state(p_state);
 
 	GDVIRTUAL_CALL(_integrate_forces, p_state);
 
+	_sync_body_state(p_state);
 	set_ignore_transform_notification(false);
 	_on_transform_changed();
 
@@ -2915,25 +2920,28 @@ void PhysicalBone3D::_notification(int p_what) {
 	}
 }
 
+void PhysicalBone3D::_sync_body_state(PhysicsDirectBodyState3D *p_state) {
+	set_global_transform(p_state->get_transform());
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
+}
+
 void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	if (!simulate_physics || !_internal_simulate_physics) {
 		return;
 	}
 
-	linear_velocity = p_state->get_linear_velocity();
-	angular_velocity = p_state->get_angular_velocity();
+	set_ignore_transform_notification(true);
+	_sync_body_state(p_state);
 
 	GDVIRTUAL_CALL(_integrate_forces, p_state);
 
-	/// Update bone transform.
-
-	Transform3D global_transform(p_state->get_transform());
-
-	set_ignore_transform_notification(true);
-	set_global_transform(global_transform);
+	_sync_body_state(p_state);
 	set_ignore_transform_notification(false);
 	_on_transform_changed();
 
+	Transform3D global_transform(p_state->get_transform());
+
 	// Update skeleton
 	if (parent_skeleton) {
 		if (-1 != bone_id) {
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index d141c1aaa20..9798fc48450 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -222,6 +222,8 @@ private:
 	void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
 	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
 
+	void _sync_body_state(PhysicsDirectBodyState3D *p_state);
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
@@ -692,6 +694,7 @@ protected:
 	static void _bind_methods();
 
 private:
+	void _sync_body_state(PhysicsDirectBodyState3D *p_state);
 	static Skeleton3D *find_skeleton_parent(Node *p_parent);
 
 	void _update_joint_offset();