1 files changed, 16 insertions, 41 deletions

diff --git a/scene/3d/physical_bone_simulator_3d.cpp b/scene/3d/physical_bone_simulator_3d.cpp
index ffe79e0892..8874c9cfc6 100644
--- a/scene/3d/physical_bone_simulator_3d.cpp
+++ b/scene/3d/physical_bone_simulator_3d.cpp
@@ -73,10 +73,15 @@ void PhysicalBoneSimulator3D::_pose_updated() {
 	}
 	ERR_FAIL_COND(skeleton->get_bone_count() != bones.size());
 	for (int i = 0; i < skeleton->get_bone_count(); i++) {
-		bones.write[i].global_pose = skeleton->get_bone_global_pose(i);
+		_bone_pose_updated(skeleton, i);
 	}
 }
 
+void PhysicalBoneSimulator3D::_bone_pose_updated(Skeleton3D *p_skeleton, int p_bone_id) {
+	ERR_FAIL_INDEX(p_bone_id, bones.size());
+	bones.write[p_bone_id].global_pose = p_skeleton->get_bone_global_pose(p_bone_id);
+}
+
 void PhysicalBoneSimulator3D::_set_active(bool p_active) {
 	if (!Engine::get_singleton()->is_editor_hint()) {
 		_reset_physical_bones_state();
@@ -285,11 +290,11 @@ void _pb_start_simulation(const PhysicalBoneSimulator3D *p_simulator, Node *p_no
 }
 
 void PhysicalBoneSimulator3D::physical_bones_start_simulation_on(const TypedArray<StringName> &p_bones) {
+	_pose_updated();
+
 	simulating = true;
 	_reset_physical_bones_state();
 
-	_pose_updated();
-
 	Vector<int> sim_bones;
 	if (p_bones.size() > 0) {
 		sim_bones.resize(p_bones.size());
@@ -357,47 +362,17 @@ void PhysicalBoneSimulator3D::_process_modification() {
 	if (!skeleton) {
 		return;
 	}
-	if (!enabled) {
-		for (int i = 0; i < bones.size(); i++) {
-			if (bones[i].physical_bone) {
-				if (bones[i].physical_bone->is_simulating_physics() == false) {
-					bones[i].physical_bone->reset_to_rest_position();
-				}
-			}
+	ERR_FAIL_COND(skeleton->get_bone_count() != bones.size());
+	for (int i = 0; i < skeleton->get_bone_count(); i++) {
+		if (!bones[i].physical_bone) {
+			continue;
 		}
-	} else {
-		ERR_FAIL_COND(skeleton->get_bone_count() != bones.size());
-		for (int i = 0; i < skeleton->get_bone_count(); i++) {
-			if (!bones[i].physical_bone) {
-				continue;
-			}
+		if (bones[i].physical_bone->is_simulating_physics() == false) {
+			_bone_pose_updated(skeleton, i);
+			bones[i].physical_bone->reset_to_rest_position();
+		} else if (simulating) {
 			skeleton->set_bone_global_pose(i, bones[i].global_pose);
 		}
-
-		// TODO:
-		// The above method is performance heavy and needs to be improved.
-		// Ideally, the processing of set_bone_global_pose within Skeleton3D should be improved,
-		// but the workaround available now is to convert the global pose to a local pose on the SkeletonModifier side.
-		// However, the follow method needs recursive processing for deformations within PhysicalBoneSimulator3D to account for update order.
-		/*
-		ERR_FAIL_COND(skeleton->get_bone_count() != bones.size());
-		LocalVector<Transform3D> local_poses;
-		for (int i = 0; i < skeleton->get_bone_count(); i++) {
-			Transform3D pt;
-			if (skeleton->get_bone_parent(i) >= 0) {
-				pt = get_bone_global_pose(skeleton->get_bone_parent(i));
-			}
-			local_poses.push_back(pt.affine_inverse() * bones[i].global_pose);
-		}
-		for (int i = 0; i < skeleton->get_bone_count(); i++) {
-			if (!bones[i].physical_bone) {
-				continue;
-			}
-			skeleton->set_bone_pose_position(i, local_poses[i].origin);
-			skeleton->set_bone_pose_rotation(i, local_poses[i].basis.get_rotation_quaternion());
-			skeleton->set_bone_pose_scale(i, local_poses[i].basis.get_scale());
-		}
-		*/
 	}
 }