From a35196ba220f16940b5b048f441007b6600fec7c Mon Sep 17 00:00:00 2001
From: devloglogan <devloglogan@gmail.com>
Date: Mon, 29 Apr 2024 15:38:15 -0500
Subject: [PATCH] Store previous relative transforms in XRHandModifier3D

---
 scene/3d/xr_hand_modifier_3d.cpp | 24 ++++++++++++++++++++++++
 scene/3d/xr_hand_modifier_3d.h   |  3 +++
 2 files changed, 27 insertions(+)

diff --git a/scene/3d/xr_hand_modifier_3d.cpp b/scene/3d/xr_hand_modifier_3d.cpp
index 1e78a4630f5c..baaa9eee48d6 100644
--- a/scene/3d/xr_hand_modifier_3d.cpp
+++ b/scene/3d/xr_hand_modifier_3d.cpp
@@ -70,6 +70,11 @@ void XRHandModifier3D::_get_joint_data() {
 		return;
 	}
 
+	if (has_stored_previous_transforms) {
+		previous_relative_transforms.clear();
+		has_stored_previous_transforms = false;
+	}
+
 	// Table of bone names for different rig types.
 	static const String bone_names[XRHandTracker::HAND_JOINT_MAX] = {
 		"Palm",
@@ -196,6 +201,18 @@ void XRHandModifier3D::_process_modification() {
 
 	// Skip if no tracking data
 	if (!tracker->get_has_tracking_data()) {
+		if (!has_stored_previous_transforms) {
+			return;
+		}
+
+		// Apply previous relative transforms if they are stored.
+		for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
+			if (bone_update == BONE_UPDATE_FULL) {
+				skeleton->set_bone_pose_position(joints[joint].bone, previous_relative_transforms[joint].origin);
+			}
+
+			skeleton->set_bone_pose_rotation(joints[joint].bone, Quaternion(previous_relative_transforms[joint].basis));
+		}
 		return;
 	}
 
@@ -223,6 +240,12 @@ void XRHandModifier3D::_process_modification() {
 		return;
 	}
 
+	if (!has_stored_previous_transforms) {
+		previous_relative_transforms.resize(XRHandTracker::HAND_JOINT_MAX);
+		has_stored_previous_transforms = true;
+	}
+	Transform3D *previous_relative_transforms_ptr = previous_relative_transforms.ptrw();
+
 	for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
 		// Get the skeleton bone (skip if none).
 		const int bone = joints[joint].bone;
@@ -233,6 +256,7 @@ void XRHandModifier3D::_process_modification() {
 		// Calculate the relative relationship to the parent bone joint.
 		const int parent_joint = joints[joint].parent_joint;
 		const Transform3D relative_transform = inv_transforms[parent_joint] * transforms[joint];
+		previous_relative_transforms_ptr[joint] = relative_transform;
 
 		// Update the bone position if enabled by update mode.
 		if (bone_update == BONE_UPDATE_FULL) {
diff --git a/scene/3d/xr_hand_modifier_3d.h b/scene/3d/xr_hand_modifier_3d.h
index 67d1694d41a2..3d78f32b64f0 100644
--- a/scene/3d/xr_hand_modifier_3d.h
+++ b/scene/3d/xr_hand_modifier_3d.h
@@ -73,6 +73,9 @@ class XRHandModifier3D : public SkeletonModifier3D {
 	BoneUpdate bone_update = BONE_UPDATE_FULL;
 	JointData joints[XRHandTracker::HAND_JOINT_MAX];
 
+	bool has_stored_previous_transforms = false;
+	Vector<Transform3D> previous_relative_transforms;
+
 	void _get_joint_data();
 	void _tracker_changed(StringName p_tracker_name, XRServer::TrackerType p_tracker_type);
 };