Fix interpolation in XR nodes

(cherry picked from commit 9e1b9fb1bc)

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>

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].

scene/3d/xr_nodes.cpp

@@ -72,7 +72,35 @@ 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, false);
+			}
+		} 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;
 	}
 }
 
@@ -196,6 +224,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 +431,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 +477,32 @@ 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);
+			}
+		} 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;
+	}
+}
+
 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);
 
@@ -714,6 +776,12 @@ void XROrigin3D::_set_current(bool p_enabled, bool p_update_others) {
 		ERR_FAIL_NULL(xr_server);
 
 		xr_server->set_world_origin(get_global_transform());
+
+		if (is_physics_interpolated()) {
+			set_process_internal(true);
+		}
+	} else if (is_physics_interpolated()) {
+		set_process_internal(false);
 	}
 
 	// Check if we need to update our other origin nodes accordingly
@@ -784,10 +852,16 @@ void XROrigin3D::_notification(int p_what) {
 
 		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED:
 		case NOTIFICATION_TRANSFORM_CHANGED: {
-			if (current && !Engine::get_singleton()->is_editor_hint()) {
+			if (current && !Engine::get_singleton()->is_editor_hint() && !is_physics_interpolated_and_enabled()) {
 				xr_server->set_world_origin(get_global_transform());
 			}
 		} break;
+
+		case NOTIFICATION_INTERNAL_PROCESS: {
+			if (current && !Engine::get_singleton()->is_editor_hint() && is_physics_interpolated_and_enabled()) {
+				xr_server->set_world_origin(get_global_transform_interpolated());
+			}
+		} break;
 	}
 
 	if (current) {
@@ -800,3 +874,9 @@ void XROrigin3D::_notification(int p_what) {
 		}
 	}
 }
+
+void XROrigin3D::_physics_interpolated_changed() {
+	if (current && !Engine::get_singleton()->is_editor_hint()) {
+		set_process_internal(is_physics_interpolated());
+	}
+}

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;
@@ -198,6 +202,7 @@ private:
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
+	virtual void _physics_interpolated_changed() override;
 
 public:
 	PackedStringArray get_configuration_warnings() const override;