Adding a tracking confidence state to XRPose

8 files changed, 53 insertions, 5 deletions

diff --git a/doc/classes/XRPose.xml b/doc/classes/XRPose.xml
index 0de2bc9e48..4a09122b40 100644
--- a/doc/classes/XRPose.xml
+++ b/doc/classes/XRPose.xml
@@ -34,8 +34,22 @@
 			- [code]grip[/code] defines the location where the user grips the controller
 			- [code]skeleton[/code] defines the root location a hand mesh should be placed when using hand tracking and the animated skeleton supplied by the XR runtime.
 		</member>
+		<member name="tracking_confidence" type="int" setter="set_tracking_confidence" getter="get_tracking_confidence" enum="XRPose.TrackingConfidence" default="0">
+			The tracking confidence for this pose, provides insight on how accurate the spatial positioning of this record is.
+		</member>
 		<member name="transform" type="Transform3D" setter="set_transform" getter="get_transform" default="Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0)">
 			The transform containing the original and transform as reported by the XR runtime.
 		</member>
 	</members>
+	<constants>
+		<constant name="XR_TRACKING_CONFIDENCE_NONE" value="0" enum="TrackingConfidence">
+			No tracking information is available for this pose.
+		</constant>
+		<constant name="XR_TRACKING_CONFIDENCE_LOW" value="1" enum="TrackingConfidence">
+			Tracking information may be inaccurate or estimated. For instance with inside out tracking this would indicate a controller may be (partially) obscured.
+		</constant>
+		<constant name="XR_TRACKING_CONFIDENCE_HIGH" value="2" enum="TrackingConfidence">
+			Tracking information is deemed accurate and up to date.
+		</constant>
+	</constants>
 </class>
diff --git a/doc/classes/XRPositionalTracker.xml b/doc/classes/XRPositionalTracker.xml
index 439bcfc382..c39a4aa7a7 100644
--- a/doc/classes/XRPositionalTracker.xml
+++ b/doc/classes/XRPositionalTracker.xml
@@ -54,8 +54,9 @@
 			<argument index="1" name="transform" type="Transform3D" />
 			<argument index="2" name="linear_velocity" type="Vector3" />
 			<argument index="3" name="angular_velocity" type="Vector3" />
+			<argument index="4" name="tracking_confidence" type="int" enum="XRPose.TrackingConfidence" />
 			<description>
-				Sets the transform, linear velocity and angular velocity for the given pose. This method is called by a [XRInterface] implementation and should not be used directly.
+				Sets the transform, linear velocity, angular velocity and tracking confidence for the given pose. This method is called by a [XRInterface] implementation and should not be used directly.
 			</description>
 		</method>
 	</methods>
diff --git a/modules/mobile_vr/mobile_vr_interface.cpp b/modules/mobile_vr/mobile_vr_interface.cpp
index 59854ad527..49044c4afe 100644
--- a/modules/mobile_vr/mobile_vr_interface.cpp
+++ b/modules/mobile_vr/mobile_vr_interface.cpp
@@ -181,6 +181,7 @@ void MobileVRInterface::set_position_from_sensors() {
 		orientation = rotate * orientation;
 
 		tracking_state = XRInterface::XR_NORMAL_TRACKING;
+		tracking_confidence = XRPose::XR_TRACKING_CONFIDENCE_HIGH;
 	};
 
 	///@TODO improve this, the magnetometer is very fidgety sometimes flipping the axis for no apparent reason (probably a bug on my part)
@@ -193,6 +194,7 @@ void MobileVRInterface::set_position_from_sensors() {
 		orientation = Basis(transform_quat);
 
 		tracking_state = XRInterface::XR_NORMAL_TRACKING;
+		tracking_confidence = XRPose::XR_TRACKING_CONFIDENCE_HIGH;
 	} else if (has_grav) {
 		// use gravity vector to make sure down is down...
 		// transform gravity into our world space
@@ -512,7 +514,7 @@ void MobileVRInterface::process() {
 
 		if (head.is_valid()) {
 			// Set our head position, note in real space, reference frame and world scale is applied later
-			head->set_pose("default", head_transform, Vector3(), Vector3());
+			head->set_pose("default", head_transform, Vector3(), Vector3(), tracking_confidence);
 		}
 	};
 };
diff --git a/modules/mobile_vr/mobile_vr_interface.h b/modules/mobile_vr/mobile_vr_interface.h
index ac04763569..47dc33c0c7 100644
--- a/modules/mobile_vr/mobile_vr_interface.h
+++ b/modules/mobile_vr/mobile_vr_interface.h
@@ -51,6 +51,7 @@ class MobileVRInterface : public XRInterface {
 private:
 	bool initialized = false;
 	XRInterface::TrackingStatus tracking_state;
+	XRPose::TrackingConfidence tracking_confidence = XRPose::XR_TRACKING_CONFIDENCE_NONE;
 
 	// Just set some defaults for these. At some point we need to look at adding a lookup table for common device + headset combos and/or support reading cardboard QR codes
 	double eye_height = 1.85;
diff --git a/servers/xr/xr_pose.cpp b/servers/xr/xr_pose.cpp
index 0862fefef5..400f13b9c2 100644
--- a/servers/xr/xr_pose.cpp
+++ b/servers/xr/xr_pose.cpp
@@ -33,6 +33,10 @@
 #include "servers/xr_server.h"
 
 void XRPose::_bind_methods() {
+	BIND_ENUM_CONSTANT(XR_TRACKING_CONFIDENCE_NONE);
+	BIND_ENUM_CONSTANT(XR_TRACKING_CONFIDENCE_LOW);
+	BIND_ENUM_CONSTANT(XR_TRACKING_CONFIDENCE_HIGH);
+
 	ClassDB::bind_method(D_METHOD("set_has_tracking_data", "has_tracking_data"), &XRPose::set_has_tracking_data);
 	ClassDB::bind_method(D_METHOD("get_has_tracking_data"), &XRPose::get_has_tracking_data);
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "has_tracking_data"), "set_has_tracking_data", "get_has_tracking_data");
@@ -53,6 +57,10 @@ void XRPose::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_angular_velocity", "velocity"), &XRPose::set_angular_velocity);
 	ClassDB::bind_method(D_METHOD("get_angular_velocity"), &XRPose::get_angular_velocity);
 	ADD_PROPERTY(PropertyInfo(Variant::STRING, "angular_velocity"), "set_angular_velocity", "get_angular_velocity");
+
+	ClassDB::bind_method(D_METHOD("set_tracking_confidence", "tracking_confidence"), &XRPose::set_tracking_confidence);
+	ClassDB::bind_method(D_METHOD("get_tracking_confidence"), &XRPose::get_tracking_confidence);
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "tracking_confidence"), "set_tracking_confidence", "get_tracking_confidence");
 }
 
 void XRPose::set_has_tracking_data(const bool p_has_tracking_data) {
@@ -108,3 +116,11 @@ void XRPose::set_angular_velocity(const Vector3 p_velocity) {
 Vector3 XRPose::get_angular_velocity() const {
 	return angular_velocity;
 }
+
+void XRPose::set_tracking_confidence(const XRPose::TrackingConfidence p_tracking_confidence) {
+	tracking_confidence = p_tracking_confidence;
+}
+
+XRPose::TrackingConfidence XRPose::get_tracking_confidence() const {
+	return tracking_confidence;
+}
diff --git a/servers/xr/xr_pose.h b/servers/xr/xr_pose.h
index 8c1fc63360..f306c22390 100644
--- a/servers/xr/xr_pose.h
+++ b/servers/xr/xr_pose.h
@@ -37,12 +37,20 @@ class XRPose : public RefCounted {
 	GDCLASS(XRPose, RefCounted);
 
 public:
+	// TrackingConfidence gives an indication of how reliable our transform data is.
+	enum TrackingConfidence {
+		XR_TRACKING_CONFIDENCE_NONE, // No tracking information is available for this pose.
+		XR_TRACKING_CONFIDENCE_LOW, // Tracking information may be inaccurate or estimated.
+		XR_TRACKING_CONFIDENCE_HIGH // Tracking information is deemed accurate and up to date.
+	};
+
 private:
 	bool has_tracking_data = false;
 	StringName name;
 	Transform3D transform;
 	Vector3 linear_velocity;
 	Vector3 angular_velocity;
+	TrackingConfidence tracking_confidence = XR_TRACKING_CONFIDENCE_NONE;
 
 protected:
 	static void _bind_methods();
@@ -63,6 +71,11 @@ public:
 
 	void set_angular_velocity(const Vector3 p_velocity);
 	Vector3 get_angular_velocity() const;
+
+	void set_tracking_confidence(const TrackingConfidence p_tracking_confidence);
+	TrackingConfidence get_tracking_confidence() const;
 };
 
+VARIANT_ENUM_CAST(XRPose::TrackingConfidence);
+
 #endif
diff --git a/servers/xr/xr_positional_tracker.cpp b/servers/xr/xr_positional_tracker.cpp
index 74907a5675..62e011654e 100644
--- a/servers/xr/xr_positional_tracker.cpp
+++ b/servers/xr/xr_positional_tracker.cpp
@@ -56,7 +56,7 @@ void XRPositionalTracker::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("has_pose", "name"), &XRPositionalTracker::has_pose);
 	ClassDB::bind_method(D_METHOD("get_pose", "name"), &XRPositionalTracker::get_pose);
 	ClassDB::bind_method(D_METHOD("invalidate_pose", "name"), &XRPositionalTracker::invalidate_pose);
-	ClassDB::bind_method(D_METHOD("set_pose", "name", "transform", "linear_velocity", "angular_velocity"), &XRPositionalTracker::set_pose);
+	ClassDB::bind_method(D_METHOD("set_pose", "name", "transform", "linear_velocity", "angular_velocity", "tracking_confidence"), &XRPositionalTracker::set_pose);
 	ADD_SIGNAL(MethodInfo("pose_changed", PropertyInfo(Variant::OBJECT, "pose", PROPERTY_HINT_RESOURCE_TYPE, "XRPose")));
 
 	ClassDB::bind_method(D_METHOD("get_input", "name"), &XRPositionalTracker::get_input);
@@ -137,7 +137,7 @@ void XRPositionalTracker::invalidate_pose(const StringName &p_action_name) {
 	}
 }
 
-void XRPositionalTracker::set_pose(const StringName &p_action_name, const Transform3D &p_transform, const Vector3 &p_linear_velocity, const Vector3 &p_angular_velocity) {
+void XRPositionalTracker::set_pose(const StringName &p_action_name, const Transform3D &p_transform, const Vector3 &p_linear_velocity, const Vector3 &p_angular_velocity, const XRPose::TrackingConfidence p_tracking_confidence) {
 	Ref<XRPose> new_pose;
 
 	new_pose.instantiate();
@@ -146,6 +146,7 @@ void XRPositionalTracker::set_pose(const StringName &p_action_name, const Transf
 	new_pose->set_transform(p_transform);
 	new_pose->set_linear_velocity(p_linear_velocity);
 	new_pose->set_angular_velocity(p_angular_velocity);
+	new_pose->set_tracking_confidence(p_tracking_confidence);
 
 	poses[p_action_name] = new_pose;
 	emit_signal("pose_changed", new_pose);
diff --git a/servers/xr/xr_positional_tracker.h b/servers/xr/xr_positional_tracker.h
index 2bcbf2c018..2f358cbb21 100644
--- a/servers/xr/xr_positional_tracker.h
+++ b/servers/xr/xr_positional_tracker.h
@@ -82,7 +82,7 @@ public:
 	bool has_pose(const StringName &p_action_name) const;
 	Ref<XRPose> get_pose(const StringName &p_action_name) const;
 	void invalidate_pose(const StringName &p_action_name);
-	void set_pose(const StringName &p_action_name, const Transform3D &p_transform, const Vector3 &p_linear_velocity, const Vector3 &p_angular_velocity);
+	void set_pose(const StringName &p_action_name, const Transform3D &p_transform, const Vector3 &p_linear_velocity, const Vector3 &p_angular_velocity, const XRPose::TrackingConfidence p_tracking_confidence = XRPose::XR_TRACKING_CONFIDENCE_HIGH);
 
 	Variant get_input(const StringName &p_action_name) const;
 	void set_input(const StringName &p_action_name, const Variant &p_value);