From ca4e1fa4241f1e0f852e2da846094ba2b54f0bc9 Mon Sep 17 00:00:00 2001
From: Silc 'Tokage' Renew <tokage.it.lab@gmail.com>
Date: Mon, 7 Mar 2022 20:12:29 +0900
Subject: [PATCH] implement get final pose to skeleton

---
 doc/classes/Skeleton3D.xml |  7 +++++++
 scene/3d/skeleton_3d.cpp   | 12 ++++++++++++
 scene/3d/skeleton_3d.h     |  1 +
 3 files changed, 20 insertions(+)

diff --git a/doc/classes/Skeleton3D.xml b/doc/classes/Skeleton3D.xml
index 80a36acacc92..35dccd5c6c1d 100644
--- a/doc/classes/Skeleton3D.xml
+++ b/doc/classes/Skeleton3D.xml
@@ -93,6 +93,13 @@
 				Returns the amount of bones in the skeleton.
 			</description>
 		</method>
+		<method name="get_bone_final_pose" qualifiers="const">
+			<return type="Transform3D" />
+			<argument index="0" name="bone_idx" type="int" />
+			<description>
+				Returns the final transform of the specified bone, if pose is overridden, it take that into account.
+			</description>
+		</method>
 		<method name="get_bone_global_pose" qualifiers="const">
 			<return type="Transform3D" />
 			<argument index="0" name="bone_idx" type="int" />
diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp
index 598897456dfa..8520b19cd537 100644
--- a/scene/3d/skeleton_3d.cpp
+++ b/scene/3d/skeleton_3d.cpp
@@ -743,6 +743,17 @@ Transform3D Skeleton3D::get_bone_pose(int p_bone) const {
 	return bones[p_bone].pose_cache;
 }
 
+Transform3D Skeleton3D::get_bone_final_pose(int p_bone) const {
+	const int bone_size = bones.size();
+	ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D());
+	((Skeleton3D *)this)->bones.write[p_bone].update_pose_cache();
+	if (bones[p_bone].local_pose_override_amount >= CMP_EPSILON || bones[p_bone].global_pose_override_amount >= CMP_EPSILON) {
+		return ((Skeleton3D *)this)->global_pose_to_local_pose(p_bone, get_bone_global_pose(p_bone));
+	} else {
+		return bones[p_bone].pose_cache;
+	}
+}
+
 void Skeleton3D::_make_dirty() {
 	if (dirty) {
 		return;
@@ -1216,6 +1227,7 @@ void Skeleton3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("clear_bones"), &Skeleton3D::clear_bones);
 
 	ClassDB::bind_method(D_METHOD("get_bone_pose", "bone_idx"), &Skeleton3D::get_bone_pose);
+	ClassDB::bind_method(D_METHOD("get_bone_final_pose", "bone_idx"), &Skeleton3D::get_bone_final_pose);
 	ClassDB::bind_method(D_METHOD("set_bone_pose_position", "bone_idx", "position"), &Skeleton3D::set_bone_pose_position);
 	ClassDB::bind_method(D_METHOD("set_bone_pose_rotation", "bone_idx", "rotation"), &Skeleton3D::set_bone_pose_rotation);
 	ClassDB::bind_method(D_METHOD("set_bone_pose_scale", "bone_idx", "scale"), &Skeleton3D::set_bone_pose_scale);
diff --git a/scene/3d/skeleton_3d.h b/scene/3d/skeleton_3d.h
index 80ff2a1f79e7..848f859b805d 100644
--- a/scene/3d/skeleton_3d.h
+++ b/scene/3d/skeleton_3d.h
@@ -215,6 +215,7 @@ class Skeleton3D : public Node3D {
 	void set_bone_pose_scale(int p_bone, const Vector3 &p_scale);
 
 	Transform3D get_bone_pose(int p_bone) const;
+	Transform3D get_bone_final_pose(int p_bone) const;
 
 	Vector3 get_bone_pose_position(int p_bone) const;
 	Quaternion get_bone_pose_rotation(int p_bone) const;