Rework XR positional trackers

5 files changed, 113 insertions, 39 deletions

diff --git a/modules/mobile_vr/mobile_vr_interface.cpp b/modules/mobile_vr/mobile_vr_interface.cpp
index fc1a118e4f..7dd26c6905 100644
--- a/modules/mobile_vr/mobile_vr_interface.cpp
+++ b/modules/mobile_vr/mobile_vr_interface.cpp
@@ -126,6 +126,8 @@ void MobileVRInterface::set_position_from_sensors() {
 	// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
 	// but in reality this only offers 3 dof (yaw, pitch, roll) orientation
 
+	Basis orientation;
+
 	uint64_t ticks = OS::get_singleton()->get_ticks_usec();
 	uint64_t ticks_elapsed = ticks - last_ticks;
 	float delta_time = (double)ticks_elapsed / 1000000.0;
@@ -207,8 +209,8 @@ void MobileVRInterface::set_position_from_sensors() {
 		};
 	};
 
-	// JIC
-	orientation.orthonormalize();
+	// and copy to our head transform
+	head_transform.basis = orientation.orthonormalized();
 
 	last_ticks = ticks;
 };
@@ -318,7 +320,7 @@ bool MobileVRInterface::initialize() {
 	ERR_FAIL_NULL_V(xr_server, false);
 
 	if (!initialized) {
-		// reset our sensor data and orientation
+		// reset our sensor data
 		mag_count = 0;
 		has_gyro = false;
 		sensor_first = true;
@@ -326,9 +328,15 @@ bool MobileVRInterface::initialize() {
 		mag_next_max = Vector3(-10000, -10000, -10000);
 		mag_current_min = Vector3(0, 0, 0);
 		mag_current_max = Vector3(0, 0, 0);
+		head_transform.basis = Basis();
+		head_transform.origin = Vector3(0.0, eye_height, 0.0);
 
-		// reset our orientation
-		orientation = Basis();
+		// we must create a tracker for our head
+		head.instantiate();
+		head->set_tracker_type(XRServer::TRACKER_HEAD);
+		head->set_tracker_name("head");
+		head->set_tracker_desc("Players head");
+		xr_server->add_tracker(head);
 
 		// make this our primary interface
 		xr_server->set_primary_interface(this);
@@ -343,10 +351,19 @@ bool MobileVRInterface::initialize() {
 
 void MobileVRInterface::uninitialize() {
 	if (initialized) {
+		// do any cleanup here...
 		XRServer *xr_server = XRServer::get_singleton();
-		if (xr_server != nullptr && xr_server->get_primary_interface() == this) {
-			// no longer our primary interface
-			xr_server->set_primary_interface(nullptr);
+		if (xr_server != nullptr) {
+			if (head.is_valid()) {
+				xr_server->remove_tracker(head);
+
+				head.unref();
+			}
+
+			if (xr_server->get_primary_interface() == this) {
+				// no longer our primary interface
+				xr_server->set_primary_interface(nullptr);
+			}
 		}
 
 		initialized = false;
@@ -377,11 +394,10 @@ Transform3D MobileVRInterface::get_camera_transform() {
 		float world_scale = xr_server->get_world_scale();
 
 		// just scale our origin point of our transform
-		Transform3D hmd_transform;
-		hmd_transform.basis = orientation;
-		hmd_transform.origin = Vector3(0.0, eye_height * world_scale, 0.0);
+		Transform3D _head_transform = head_transform;
+		_head_transform.origin *= world_scale;
 
-		transform_for_eye = (xr_server->get_reference_frame()) * hmd_transform;
+		transform_for_eye = (xr_server->get_reference_frame()) * _head_transform;
 	}
 
 	return transform_for_eye;
@@ -409,11 +425,10 @@ Transform3D MobileVRInterface::get_transform_for_view(uint32_t p_view, const Tra
 		};
 
 		// just scale our origin point of our transform
-		Transform3D hmd_transform;
-		hmd_transform.basis = orientation;
-		hmd_transform.origin = Vector3(0.0, eye_height * world_scale, 0.0);
+		Transform3D _head_transform = head_transform;
+		_head_transform.origin *= world_scale;
 
-		transform_for_eye = p_cam_transform * (xr_server->get_reference_frame()) * hmd_transform * transform_for_eye;
+		transform_for_eye = p_cam_transform * (xr_server->get_reference_frame()) * _head_transform * transform_for_eye;
 	} else {
 		// huh? well just return what we got....
 		transform_for_eye = p_cam_transform;
@@ -476,7 +491,16 @@ void MobileVRInterface::process() {
 	_THREAD_SAFE_METHOD_
 
 	if (initialized) {
+		// update our head transform orientation
 		set_position_from_sensors();
+
+		// update our head transform position (should be constant)
+		head_transform.origin = Vector3(0.0, eye_height, 0.0);
+
+		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());
+		}
 	};
 };
 
diff --git a/modules/mobile_vr/mobile_vr_interface.h b/modules/mobile_vr/mobile_vr_interface.h
index a843e1188b..34bd1da43b 100644
--- a/modules/mobile_vr/mobile_vr_interface.h
+++ b/modules/mobile_vr/mobile_vr_interface.h
@@ -53,7 +53,6 @@ class MobileVRInterface : public XRInterface {
 private:
 	bool initialized = false;
 	XRInterface::TrackingStatus tracking_state;
-	Basis orientation;
 
 	// 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;
@@ -68,6 +67,10 @@ private:
 	double k2 = 0.215;
 	double aspect = 1.0;
 
+	// at a minimum we need a tracker for our head
+	Ref<XRPositionalTracker> head;
+	Transform3D head_transform;
+
 	/*
 		logic for processing our sensor data, this was originally in our positional tracker logic but I think
 		that doesn't make sense in hindsight. It only makes marginally more sense to park it here for now,
diff --git a/modules/webxr/doc_classes/WebXRInterface.xml b/modules/webxr/doc_classes/WebXRInterface.xml
index 16d671c9e9..6e224a8242 100644
--- a/modules/webxr/doc_classes/WebXRInterface.xml
+++ b/modules/webxr/doc_classes/WebXRInterface.xml
@@ -87,7 +87,7 @@
 		There are several ways to handle "controller" input:
 		- Using [XRController3D] nodes and their [signal XRController3D.button_pressed] and [signal XRController3D.button_released] signals. This is how controllers are typically handled in AR/VR apps in Godot, however, this will only work with advanced VR controllers like the Oculus Touch or Index controllers, for example. The buttons codes are defined by [url=https://immersive-web.github.io/webxr-gamepads-module/#xr-standard-gamepad-mapping]Section 3.3 of the WebXR Gamepads Module[/url].
 		- Using [method Node._unhandled_input] and [InputEventJoypadButton] or [InputEventJoypadMotion]. This works the same as normal joypads, except the [member InputEvent.device] starts at 100, so the left controller is 100 and the right controller is 101, and the button codes are also defined by [url=https://immersive-web.github.io/webxr-gamepads-module/#xr-standard-gamepad-mapping]Section 3.3 of the WebXR Gamepads Module[/url].
-		- Using the [signal select], [signal squeeze] and related signals. This method will work for both advanced VR controllers, and non-traditional "controllers" like a tap on the screen, a spoken voice command or a button press on the device itself. The [code]controller_id[/code] passed to these signals is the same id as used in [member XRController3D.controller_id].
+		- Using the [signal select], [signal squeeze] and related signals. This method will work for both advanced VR controllers, and non-traditional "controllers" like a tap on the screen, a spoken voice command or a button press on the device itself.
 		You can use one or all of these methods to allow your game or app to support a wider or narrower set of devices and input methods, or to allow more advanced interactions with more advanced devices.
 	</description>
 	<tutorials>
diff --git a/modules/webxr/webxr_interface_js.cpp b/modules/webxr/webxr_interface_js.cpp
index 10c17aa672..2676b3cf80 100644
--- a/modules/webxr/webxr_interface_js.cpp
+++ b/modules/webxr/webxr_interface_js.cpp
@@ -163,9 +163,14 @@ String WebXRInterfaceJS::get_reference_space_type() const {
 
 Ref<XRPositionalTracker> WebXRInterfaceJS::get_controller(int p_controller_id) const {
 	XRServer *xr_server = XRServer::get_singleton();
-	ERR_FAIL_NULL_V(xr_server, nullptr);
+	ERR_FAIL_NULL_V(xr_server, Ref<XRPositionalTracker>());
 
-	return xr_server->find_by_type_and_id(XRServer::TRACKER_CONTROLLER, p_controller_id);
+	// TODO support more then two controllers
+	if (p_controller_id >= 0 && p_controller_id < 2) {
+		return controllers[p_controller_id];
+	};
+
+	return Ref<XRPositionalTracker>();
 }
 
 String WebXRInterfaceJS::get_visibility_state() const {
@@ -224,6 +229,13 @@ bool WebXRInterfaceJS::initialize() {
 			return false;
 		}
 
+		// we must create a tracker for our head
+		head_tracker.instantiate();
+		head_tracker->set_tracker_type(XRServer::TRACKER_HEAD);
+		head_tracker->set_tracker_name("head");
+		head_tracker->set_tracker_desc("Players head");
+		xr_server->add_tracker(head_tracker);
+
 		// make this our primary interface
 		xr_server->set_primary_interface(this);
 
@@ -254,9 +266,17 @@ bool WebXRInterfaceJS::initialize() {
 void WebXRInterfaceJS::uninitialize() {
 	if (initialized) {
 		XRServer *xr_server = XRServer::get_singleton();
-		if (xr_server != nullptr && xr_server->get_primary_interface() == this) {
-			// no longer our primary interface
-			xr_server->set_primary_interface(nullptr);
+		if (xr_server != nullptr) {
+			if (head_tracker.is_valid()) {
+				xr_server->remove_tracker(head_tracker);
+
+				head_tracker.unref();
+			}
+
+			if (xr_server->get_primary_interface() == this) {
+				// no longer our primary interface
+				xr_server->set_primary_interface(nullptr);
+			}
 		}
 
 		godot_webxr_uninitialize();
@@ -373,9 +393,9 @@ Vector<BlitToScreen> WebXRInterfaceJS::commit_views(RID p_render_target, const R
 	}
 
 	// @todo Refactor this to be based on "views" rather than "eyes".
-	godot_webxr_commit_for_eye(XRInterface::EYE_LEFT);
+	godot_webxr_commit_for_eye(1);
 	if (godot_webxr_get_view_count() > 1) {
-		godot_webxr_commit_for_eye(XRInterface::EYE_RIGHT);
+		godot_webxr_commit_for_eye(2);
 	}
 
 	return blit_to_screen;
@@ -385,6 +405,11 @@ void WebXRInterfaceJS::process() {
 	if (initialized) {
 		godot_webxr_sample_controller_data();
 
+		if (head_tracker.is_valid()) {
+			// TODO set default pose to our head location (i.e. get_camera_transform without world scale and reference frame applied)
+			// head_tracker->set_pose("default", head_transform, Vector3(), Vector3());
+		}
+
 		int controller_count = godot_webxr_get_controller_count();
 		if (controller_count == 0) {
 			return;
@@ -400,51 +425,70 @@ void WebXRInterfaceJS::_update_tracker(int p_controller_id) {
 	XRServer *xr_server = XRServer::get_singleton();
 	ERR_FAIL_NULL(xr_server);
 
-	Ref<XRPositionalTracker> tracker = xr_server->find_by_type_and_id(XRServer::TRACKER_CONTROLLER, p_controller_id + 1);
+	// need to support more then two controllers...
+	if (p_controller_id < 0 || p_controller_id > 1) {
+		return;
+	}
+
+	Ref<XRPositionalTracker> tracker = controllers[p_controller_id];
 	if (godot_webxr_is_controller_connected(p_controller_id)) {
 		if (tracker.is_null()) {
 			tracker.instantiate();
 			tracker->set_tracker_type(XRServer::TRACKER_CONTROLLER);
 			// Controller id's 0 and 1 are always the left and right hands.
 			if (p_controller_id < 2) {
-				tracker->set_tracker_name(p_controller_id == 0 ? "Left" : "Right");
+				tracker->set_tracker_name(p_controller_id == 0 ? "left_hand" : "right_hand");
+				tracker->set_tracker_desc(p_controller_id == 0 ? "Left hand controller" : "Right hand controller");
 				tracker->set_tracker_hand(p_controller_id == 0 ? XRPositionalTracker::TRACKER_HAND_LEFT : XRPositionalTracker::TRACKER_HAND_RIGHT);
+			} else {
+				char name[1024];
+				sprintf(name, "tracker_%i", p_controller_id);
+				tracker->set_tracker_name(name);
+				tracker->set_tracker_desc(name);
 			}
-			// Use the ids we're giving to our "virtual" gamepads.
-			tracker->set_joy_id(p_controller_id + 100);
 			xr_server->add_tracker(tracker);
 		}
 
-		Input *input = Input::get_singleton();
-
 		float *tracker_matrix = godot_webxr_get_controller_transform(p_controller_id);
 		if (tracker_matrix) {
+			// Note, poses should NOT have world scale and our reference frame applied!
 			Transform3D transform = _js_matrix_to_transform(tracker_matrix);
-			tracker->set_position(transform.origin);
-			tracker->set_orientation(transform.basis);
+			tracker->set_pose("default", transform, Vector3(), Vector3());
 			free(tracker_matrix);
 		}
 
+		// TODO implement additional poses such as "aim" and "grip"
+
 		int *buttons = godot_webxr_get_controller_buttons(p_controller_id);
 		if (buttons) {
+			// TODO buttons should be named properly, this is just a temporary fix
 			for (int i = 0; i < buttons[0]; i++) {
-				input->joy_button(p_controller_id + 100, (JoyButton)i, *((float *)buttons + (i + 1)));
+				char name[1024];
+				sprintf(name, "button_%i", i);
+
+				float value = *((float *)buttons + (i + 1));
+				bool state = value > 0.0;
+				tracker->set_input(name, state);
 			}
 			free(buttons);
 		}
 
 		int *axes = godot_webxr_get_controller_axes(p_controller_id);
 		if (axes) {
+			// TODO again just a temporary fix, split these between proper float and vector2 inputs
 			for (int i = 0; i < axes[0]; i++) {
-				Input::JoyAxisValue joy_axis;
-				joy_axis.min = -1;
-				joy_axis.value = *((float *)axes + (i + 1));
-				input->joy_axis(p_controller_id + 100, (JoyAxis)i, joy_axis);
+				char name[1024];
+				sprintf(name, "axis_%i", i);
+
+				float value = *((float *)axes + (i + 1));
+				;
+				tracker->set_input(name, value);
 			}
 			free(axes);
 		}
 	} else if (tracker.is_valid()) {
 		xr_server->remove_tracker(tracker);
+		controllers[p_controller_id].unref();
 	}
 }
 
@@ -454,7 +498,7 @@ void WebXRInterfaceJS::_on_controller_changed() {
 	for (int i = 0; i < 2; i++) {
 		bool controller_connected = godot_webxr_is_controller_connected(i);
 		if (controllers_state[i] != controller_connected) {
-			Input::get_singleton()->joy_connection_changed(i + 100, controller_connected, i == 0 ? "Left" : "Right", "");
+			// Input::get_singleton()->joy_connection_changed(i + 100, controller_connected, i == 0 ? "Left" : "Right", "");
 			controllers_state[i] = controller_connected;
 		}
 	}
diff --git a/modules/webxr/webxr_interface_js.h b/modules/webxr/webxr_interface_js.h
index eb77f35f39..6e6548c946 100644
--- a/modules/webxr/webxr_interface_js.h
+++ b/modules/webxr/webxr_interface_js.h
@@ -46,6 +46,7 @@ class WebXRInterfaceJS : public WebXRInterface {
 
 private:
 	bool initialized;
+	Ref<XRPositionalTracker> head_tracker;
 
 	String session_mode;
 	String required_features;
@@ -53,7 +54,9 @@ private:
 	String requested_reference_space_types;
 	String reference_space_type;
 
+	// TODO maybe turn into a vector to support more then 2 controllers...
 	bool controllers_state[2];
+	Ref<XRPositionalTracker> controllers[2];
 	Size2 render_targetsize;
 
 	Transform3D _js_matrix_to_transform(float *p_js_matrix);