From 3e7da32448420959525c7aa24d4651089981008a Mon Sep 17 00:00:00 2001
From: Asaduji <39374509+Asaduji@users.noreply.github.com>
Date: Tue, 25 Feb 2025 18:40:17 +0100
Subject: [PATCH] Fix physics interpolation in XRNode3D and XRCamera3D

---
 doc/classes/XRCamera3D.xml |  3 ++
 doc/classes/XRNode3D.xml   |  1 +
 scene/3d/xr_nodes.cpp      | 95 +++++++++++++++++++++++++++++++++++++-
 scene/3d/xr_nodes.h        |  4 ++
 4 files changed, 101 insertions(+), 2 deletions(-)

diff --git a/doc/classes/XRCamera3D.xml b/doc/classes/XRCamera3D.xml
index e49e884f33e..ca1cd431f68 100644
--- a/doc/classes/XRCamera3D.xml
+++ b/doc/classes/XRCamera3D.xml
@@ -10,4 +10,7 @@
 	<tutorials>
 		<link title="XR documentation index">$DOCS_URL/tutorials/xr/index.html</link>
 	</tutorials>
+	<members>
+		<member name="physics_interpolation_mode" type="int" setter="set_physics_interpolation_mode" getter="get_physics_interpolation_mode" overrides="Node" enum="Node.PhysicsInterpolationMode" default="2" />
+	</members>
 </class>
diff --git a/doc/classes/XRNode3D.xml b/doc/classes/XRNode3D.xml
index fce3708d9ab..cc2f4b874e7 100644
--- a/doc/classes/XRNode3D.xml
+++ b/doc/classes/XRNode3D.xml
@@ -46,6 +46,7 @@
 		</method>
 	</methods>
 	<members>
+		<member name="physics_interpolation_mode" type="int" setter="set_physics_interpolation_mode" getter="get_physics_interpolation_mode" overrides="Node" enum="Node.PhysicsInterpolationMode" default="2" />
 		<member name="pose" type="StringName" setter="set_pose_name" getter="get_pose_name" default="&amp;&quot;default&quot;">
 			The name of the pose we're bound to. Which poses a tracker supports is not known during design time.
 			Godot defines number of standard pose names such as [code]aim[/code] and [code]grip[/code] but other may be configured within a given [XRInterface].
diff --git a/scene/3d/xr_nodes.cpp b/scene/3d/xr_nodes.cpp
index 43def378bd5..f475094e9b6 100644
--- a/scene/3d/xr_nodes.cpp
+++ b/scene/3d/xr_nodes.cpp
@@ -72,7 +72,49 @@ void XRCamera3D::_removed_tracker(const StringName &p_tracker_name, int p_tracke
 
 void XRCamera3D::_pose_changed(const Ref<XRPose> &p_pose) {
 	if (p_pose->get_name() == pose_name) {
-		set_transform(p_pose->get_adjusted_transform());
+		Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+		if (is_inside_tree() && parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level() && !is_physics_interpolated()) {
+			pose_offset = p_pose->get_adjusted_transform();
+		} else {
+			set_transform(p_pose->get_adjusted_transform());
+		}
+	}
+}
+
+void XRCamera3D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			if (!Engine::get_singleton()->is_editor_hint()) {
+				set_desired_process_modes(true, true);
+			}
+		} break;
+
+		case NOTIFICATION_INTERNAL_PROCESS: {
+			if (!is_inside_tree() || is_physics_interpolated() || Engine::get_singleton()->is_editor_hint()) {
+				break;
+			}
+
+			Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+			if (parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level()) {
+				set_global_transform(parent->get_global_transform_interpolated() * pose_offset);
+			}
+		} break;
+
+		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			if (!is_inside_tree() || is_physics_interpolated() || Engine::get_singleton()->is_editor_hint()) {
+				break;
+			}
+
+			Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+			// If we want interpolated physics nodes like RayCast3D to follow this node properly,
+			// the transform must be relative to the non interpolated parent when we're on physics process
+			if (parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level()) {
+				set_transform(pose_offset);
+			}
+		} break;
 	}
 }
 
@@ -196,6 +238,9 @@ Vector<Plane> XRCamera3D::get_frustum() const {
 }
 
 XRCamera3D::XRCamera3D() {
+	//XRCamera3D gets its transform updated every render frame and shouldn't be interpolated
+	set_physics_interpolation_mode(Node::PHYSICS_INTERPOLATION_MODE_OFF);
+
 	XRServer *xr_server = XRServer::get_singleton();
 	ERR_FAIL_NULL(xr_server);
 
@@ -400,7 +445,13 @@ void XRNode3D::_removed_tracker(const StringName &p_tracker_name, int p_tracker_
 
 void XRNode3D::_pose_changed(const Ref<XRPose> &p_pose) {
 	if (p_pose.is_valid() && p_pose->get_name() == pose_name) {
-		set_transform(p_pose->get_adjusted_transform());
+		Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+		if (is_inside_tree() && parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level() && !is_physics_interpolated()) {
+			pose_offset = p_pose->get_adjusted_transform();
+		} else {
+			set_transform(p_pose->get_adjusted_transform());
+		}
 		_set_has_tracking_data(p_pose->get_has_tracking_data());
 	}
 }
@@ -440,7 +491,47 @@ void XRNode3D::_update_visibility() {
 	}
 }
 
+void XRNode3D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			if (!Engine::get_singleton()->is_editor_hint()) {
+				set_process_internal(true);
+				set_physics_process_internal(true);
+			}
+		} break;
+
+		case NOTIFICATION_INTERNAL_PROCESS: {
+			if (!is_inside_tree() || is_physics_interpolated() || Engine::get_singleton()->is_editor_hint()) {
+				break;
+			}
+
+			Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+			if (parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level()) {
+				set_global_transform(parent->get_global_transform_interpolated() * pose_offset);
+			}
+		} break;
+
+		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			if (!is_inside_tree() || is_physics_interpolated() || Engine::get_singleton()->is_editor_hint()) {
+				break;
+			}
+
+			Node3D *parent = Object::cast_to<Node3D>(get_parent());
+
+			// If we want interpolated physics nodes like RayCast3D to follow this node properly,
+			// the transform must be relative to the non interpolated parent when we're on physics process
+			if (parent && parent->is_physics_interpolated_and_enabled() && !is_set_as_top_level()) {
+				set_transform(pose_offset);
+			}
+		} break;
+	}
+}
+
 XRNode3D::XRNode3D() {
+	//XRNode3D gets its transform updated every render frame and shouldn't be interpolated
+	set_physics_interpolation_mode(Node::PHYSICS_INTERPOLATION_MODE_OFF);
+
 	XRServer *xr_server = XRServer::get_singleton();
 	ERR_FAIL_NULL(xr_server);
 
diff --git a/scene/3d/xr_nodes.h b/scene/3d/xr_nodes.h
index cbf8ad5dc0d..10d16d0b9d5 100644
--- a/scene/3d/xr_nodes.h
+++ b/scene/3d/xr_nodes.h
@@ -47,12 +47,14 @@ protected:
 	StringName tracker_name = "head";
 	StringName pose_name = SceneStringName(default_);
 	Ref<XRPositionalTracker> tracker;
+	Transform3D pose_offset;
 
 	void _bind_tracker();
 	void _unbind_tracker();
 	void _changed_tracker(const StringName &p_tracker_name, int p_tracker_type);
 	void _removed_tracker(const StringName &p_tracker_name, int p_tracker_type);
 	void _pose_changed(const Ref<XRPose> &p_pose);
+	void _notification(int p_what);
 
 public:
 	PackedStringArray get_configuration_warnings() const override;
@@ -80,6 +82,7 @@ private:
 	StringName pose_name = SceneStringName(default_);
 	bool has_tracking_data = false;
 	bool show_when_tracked = false;
+	Transform3D pose_offset;
 
 protected:
 	Ref<XRPositionalTracker> tracker;
@@ -96,6 +99,7 @@ protected:
 	void _set_has_tracking_data(bool p_has_tracking_data);
 
 	void _update_visibility();
+	void _notification(int p_what);
 
 public:
 	void _validate_property(PropertyInfo &p_property) const;