refactor: handtracking

Signed-off-by: Schmarni <marnistromer@gmail.com>

src/objects/input/eye_pointer.rs

@@ -12,7 +12,6 @@ use glam::{vec3, Mat4};
 use serde::{Deserialize, Serialize};
 use stardust_xr::values::Datamap;
 use std::sync::Arc;
-use stereokit_rust::system::Input;
 
 #[derive(Default, Deserialize, Serialize)]
 pub struct EyeDatamap {
@@ -49,58 +48,59 @@ impl EyePointer {
 			pointer,
 		})
 	}
+	// TODO: implement eyetracking in bevy_mod_openxr, then reimplement with that
 	pub fn update(&self) {
-		let ray = Input::get_eyes();
+		// let ray = Input::get_eyes();
-		self.spatial
+		// self.spatial
-			.set_local_transform(Mat4::from_rotation_translation(
+		// 	.set_local_transform(Mat4::from_rotation_translation(
-				ray.orientation.into(),
+		// 		ray.orientation.into(),
-				ray.position.into(),
+		// 		ray.position.into(),
-			));
+		// 	));
-		{
+		// {
-			// Set pointer input datamap
+		// 	// Set pointer input datamap
-			*self.pointer.datamap.lock() = Datamap::from_typed(EyeDatamap { eye: 2 }).unwrap();
+		// 	*self.pointer.datamap.lock() = Datamap::from_typed(EyeDatamap { eye: 2 }).unwrap();
-		}
+		// }
-
+		//
-		// send input to all the input handlers that are the closest to the ray as possible
+		// // send input to all the input handlers that are the closest to the ray as possible
-		let rx = INPUT_HANDLER_REGISTRY
+		// let rx = INPUT_HANDLER_REGISTRY
-			.get_valid_contents()
+		// 	.get_valid_contents()
-			.into_iter()
+		// 	.into_iter()
-			// filter out all the disabled handlers
+		// 	// filter out all the disabled handlers
-			.filter(|handler| {
+		// 	.filter(|handler| {
-				let Some(node) = handler.spatial.node() else {
+		// 		let Some(node) = handler.spatial.node() else {
-					return false;
+		// 			return false;
-				};
+		// 		};
-				node.enabled()
+		// 		node.enabled()
-			})
-			// ray march to all the enabled handlers' fields
-			.map(|handler| {
-				let result = handler.field.ray_march(Ray {
-					origin: vec3(0.0, 0.0, 0.0),
-					direction: vec3(0.0, 0.0, -1.0),
-					space: self.spatial.clone(),
-				});
-				(vec![handler], result)
-			})
-			// make sure the field isn't at the pointer origin and that it's being hit
-			.filter(|(_, result)| result.deepest_point_distance > 0.01 && result.min_distance < 0.0)
-			// .inspect(|(_, result)| {
-			// 	dbg!(result);
 		// 	})
-			// now collect all handlers that are same distance if they're the closest
+		// 	// ray march to all the enabled handlers' fields
-			.reduce(|(mut handlers_a, result_a), (handlers_b, result_b)| {
+		// 	.map(|handler| {
-				if (result_a.deepest_point_distance - result_b.deepest_point_distance).abs() < 0.001
+		// 		let result = handler.field.ray_march(Ray {
-				{
+		// 			origin: vec3(0.0, 0.0, 0.0),
-					// distance is basically the same
+		// 			direction: vec3(0.0, 0.0, -1.0),
-					handlers_a.extend(handlers_b);
+		// 			space: self.spatial.clone(),
-					(handlers_a, result_a)
+		// 		});
-				} else if result_a.deepest_point_distance < result_b.deepest_point_distance {
+		// 		(vec![handler], result)
-					(handlers_a, result_a)
+		// 	})
-				} else {
+		// 	// make sure the field isn't at the pointer origin and that it's being hit
-					(handlers_b, result_b)
+		// 	.filter(|(_, result)| result.deepest_point_distance > 0.01 && result.min_distance < 0.0)
-				}
+		// 	// .inspect(|(_, result)| {
-			})
+		// 	// 	dbg!(result);
-			.map(|(rx, _)| rx)
+		// 	// })
-			.unwrap_or_default();
+		// 	// now collect all handlers that are same distance if they're the closest
-		self.pointer.set_handler_order(rx.iter());
+		// 	.reduce(|(mut handlers_a, result_a), (handlers_b, result_b)| {
+		// 		if (result_a.deepest_point_distance - result_b.deepest_point_distance).abs() < 0.001
+		// 		{
+		// 			// distance is basically the same
+		// 			handlers_a.extend(handlers_b);
+		// 			(handlers_a, result_a)
+		// 		} else if result_a.deepest_point_distance < result_b.deepest_point_distance {
+		// 			(handlers_a, result_a)
+		// 		} else {
+		// 			(handlers_b, result_b)
+		// 		}
+		// 	})
+		// 	.map(|(rx, _)| rx)
+		// 	.unwrap_or_default();
+		// self.pointer.set_handler_order(rx.iter());
 	}
 }

src/objects/input/sk_controller.rs

@@ -1,6 +1,6 @@
 use super::{get_sorted_handlers, CaptureManager};
 use crate::{
-	bevy_plugin::DbusConnection,
+	bevy_plugin::{DbusConnection, InputUpdate},
 	core::client::INTERNAL_CLIENT,
 	nodes::{
 		fields::{Field, FieldTrait},
@@ -55,17 +55,18 @@ impl Plugin for StardustControllerPlugin {
 	fn build(&self, app: &mut App) {
 		embedded_asset!(app, "src/objects/input", "cursor.glb");
 		app.add_systems(XrSessionCreated, spawn_controllers);
+		app.add_systems(InputUpdate, update_controllers);
 	}
 }
 
 fn update_controllers(
 	mut mats: ResMut<Assets<DefaultMaterial>>,
-	mut query: Query<(&SkController, &mut Transform)>,
+	mut query: Query<(&mut SkController, &mut Transform)>,
 	time: Res<OxrFrameState>,
 	base_space: Res<XrPrimaryReferenceSpace>,
 	session: ResMut<OxrSession>,
 ) {
-	for (controller, mut transform) in &mut query {
+	for (mut controller, mut transform) in query.iter_mut() {
 		let input_node = controller.input.spatial.node().unwrap();
 		let location = (|| {
 			let location = match session.locate_space(

src/objects/input/sk_hand.rs

@@ -1,3 +1,4 @@
+use crate::bevy_plugin::DbusConnection;
 use crate::core::client::INTERNAL_CLIENT;
 use crate::nodes::fields::{Field, FieldTrait};
 use crate::nodes::input::{InputDataType, InputHandler, INPUT_HANDLER_REGISTRY};
@@ -8,23 +9,269 @@ use crate::nodes::{
 	Node,
 };
 use crate::objects::{ObjectHandle, SpatialRef};
+use crate::DefaultMaterial;
+use bevy::asset::{AssetServer, Assets, Handle};
+use bevy::prelude::{Commands, Component, Entity, Query, Res, ResMut};
+use bevy_mod_openxr::helper_traits::{ToQuat, ToVec3};
+use bevy_mod_openxr::resources::OxrFrameState;
+use bevy_mod_openxr::session::OxrSession;
+use bevy_mod_openxr::spaces::OxrSpaceLocationFlags;
+use bevy_mod_xr::hands::{HandBone, HandSide};
+use bevy_mod_xr::spaces::XrPrimaryReferenceSpace;
 use color_eyre::eyre::Result;
 use glam::{Mat4, Quat, Vec3};
+use once_cell::sync::OnceCell;
 use serde::{Deserialize, Serialize};
 use stardust_xr::values::Datamap;
 use std::sync::Arc;
-use stereokit_rust::material::Material;
+use tracing::error;
-use stereokit_rust::sk::{DisplayMode, MainThreadToken, Sk};
-use stereokit_rust::system::{HandJoint, HandSource, Handed, Input, LinePoint, Lines};
-use stereokit_rust::util::Color128;
 use zbus::Connection;
 
-use super::{get_sorted_handlers, CaptureManager};
+fn update_joint(joint: &mut Joint, oxr_joint: openxr::HandJointLocation) {
+	let flags = OxrSpaceLocationFlags(oxr_joint.location_flags);
+	if flags.pos_valid() && flags.rot_valid() {
+		*joint = convert_joint(oxr_joint);
+	}
+}
 
-fn convert_joint(joint: HandJoint) -> Joint {
+fn update_hands(
+	mut mats: ResMut<Assets<DefaultMaterial>>,
+	mut query: Query<&mut SkHand>,
+	time: Res<OxrFrameState>,
+	base_space: Res<XrPrimaryReferenceSpace>,
+	session: ResMut<OxrSession>,
+) {
+	for mut hand in &mut query {
+		let joints = session
+			.locate_hand_joints(&hand.hand_tracker, &base_space, time.predicted_display_time)
+			.unwrap();
+		if let InputDataType::Hand(hand_input) = &mut *hand.input.data.lock() {
+			let input_node = hand.input.spatial.node().unwrap();
+			input_node.set_enabled(joints.is_some());
+			if let Some(joints) = joints.as_ref() {
+				update_joint(
+					&mut hand_input.thumb.tip,
+					joints[HandBone::ThumbTip as usize],
+				);
+				update_joint(
+					&mut hand_input.thumb.distal,
+					joints[HandBone::ThumbDistal as usize],
+				);
+				update_joint(
+					&mut hand_input.thumb.proximal,
+					joints[HandBone::ThumbProximal as usize],
+				);
+				update_joint(
+					&mut hand_input.thumb.metacarpal,
+					joints[HandBone::ThumbMetacarpal as usize],
+				);
+
+				for (finger, finger_index) in [
+					(&mut hand_input.index, 6),
+					(&mut hand_input.middle, 11),
+					(&mut hand_input.ring, 16),
+					(&mut hand_input.little, 21),
+				] {
+					update_joint(&mut finger.tip, joints[finger_index + 4]);
+					update_joint(&mut finger.distal, joints[finger_index + 3]);
+					update_joint(&mut finger.intermediate, joints[finger_index + 2]);
+					update_joint(&mut finger.proximal, joints[finger_index + 1]);
+					update_joint(&mut finger.metacarpal, joints[finger_index + 0]);
+					// Why?
+					finger.tip.radius = 0.0;
+				}
+				update_joint(&mut hand_input.palm, joints[HandBone::Palm as usize]);
+				hand.palm_spatial
+					.set_local_transform(Mat4::from_rotation_translation(
+						hand_input.palm.rotation.into(),
+						hand_input.palm.position.into(),
+					));
+				update_joint(&mut hand_input.wrist, joints[HandBone::Wrist as usize]);
+
+				hand_input.elbow = None;
+			}
+		}
+		if let Some(joints) = joints.as_ref() {
+			hand.datamap.pinch_strength = pinch_activation(joints);
+			hand.datamap.grab_strength = grip_activation(joints);
+			*hand.input.datamap.lock() = Datamap::from_typed(&hand.datamap).unwrap();
+		}
+		// remove the capture when it's removed from captures list
+		if let Some(capture) = &hand.capture {
+			if !hand
+				.input
+				.capture_requests
+				.get_valid_contents()
+				.contains(capture)
+			{
+				hand.capture.take();
+			}
+		}
+		// add the capture that's the closest if we don't have one
+		if hand.capture.is_none() {
+			hand.capture = hand
+				.input
+				.capture_requests
+				.get_valid_contents()
+				.into_iter()
+				.map(|handler| (handler.clone(), hand.compare_distance(&handler.field).abs()))
+				.reduce(|(handlers_a, distance_a), (handlers_b, distance_b)| {
+					if distance_a < distance_b {
+						(handlers_a, distance_a)
+					} else {
+						(handlers_b, distance_b)
+					}
+				})
+				.map(|(rx, _)| rx);
+		}
+
+		// make sure that if something is captured only send input to it
+		hand.input.captures.clear();
+		if let Some(capture) = &hand.capture {
+			hand.input.set_handler_order([capture].into_iter());
+			hand.input.captures.add_raw(capture);
+			return;
+		}
+
+		// send input to all the input handlers that are the closest to the ray as possible
+		hand.input.set_handler_order(
+			INPUT_HANDLER_REGISTRY
+				.get_valid_contents()
+				.into_iter()
+				// filter out all the disabled handlers
+				.filter(|handler| {
+					let Some(node) = handler.spatial.node() else {
+						return false;
+					};
+					node.enabled()
+				})
+				// filter out all the fields with disabled handlers
+				.filter(|handler| {
+					let Some(node) = handler.field.spatial.node() else {
+						return false;
+					};
+					node.enabled()
+				})
+				// get the unsigned distance to the handler's field (unsigned so giant fields won't always eat input)
+				.map(|handler| {
+					(
+						vec![handler.clone()],
+						hand.compare_distance(&handler.field).abs(),
+					)
+				})
+				// .inspect(|(_, result)| {
+				// 	dbg!(result);
+				// })
+				// now collect all handlers that are same distance if they're the closest
+				.reduce(|(mut handlers_a, distance_a), (handlers_b, distance_b)| {
+					if (distance_a - distance_b).abs() < 0.001 {
+						// distance is basically the same (within 1mm)
+						handlers_a.extend(handlers_b);
+						(handlers_a, distance_a)
+					} else if distance_a < distance_b {
+						(handlers_a, distance_a)
+					} else {
+						(handlers_b, distance_b)
+					}
+				})
+				.map(|(rx, _)| rx)
+				.unwrap_or_default()
+				.iter(),
+		);
+	}
+}
+
+const PINCH_MAX: f32 = 0.11;
+const PINCH_ACTIVACTION_DISTANCE: f32 = 0.01;
+// TODO: handle invalid data
+// based on https://github.com/StereoKit/StereoKit/blob/ca2be7d45f4f4388e8df7542e9a0313bcc45946e/StereoKitC/hands/input_hand.cpp#L375-L394
+fn pinch_activation(joints: &[openxr::HandJointLocation; openxr::HAND_JOINT_COUNT]) -> f32 {
+	let combined_radius =
+		joints[HandBone::ThumbTip as usize].radius + joints[HandBone::IndexTip as usize].radius;
+	let pinch_dist = joints[HandBone::ThumbTip as usize]
+		.pose
+		.position
+		.to_vec3()
+		.distance(joints[HandBone::IndexTip as usize].pose.position.to_vec3())
+		- combined_radius;
+	(1.0 - ((pinch_dist - PINCH_ACTIVACTION_DISTANCE) / (PINCH_MAX - PINCH_ACTIVACTION_DISTANCE)))
+		.clamp(0.0, 1.0)
+}
+
+const GRIP_MAX: f32 = 0.11;
+const GRIP_ACTIVACTION_DISTANCE: f32 = 0.01;
+// TODO: handle invalid data
+// based on https://github.com/StereoKit/StereoKit/blob/ca2be7d45f4f4388e8df7542e9a0313bcc45946e/StereoKitC/hands/input_hand.cpp#L375-L394
+fn grip_activation(joints: &[openxr::HandJointLocation; openxr::HAND_JOINT_COUNT]) -> f32 {
+	let combined_radius = joints[HandBone::RingTip as usize].radius
+		+ joints[HandBone::RingMetacarpal as usize].radius;
+	let grip_dist = joints[HandBone::RingTip as usize]
+		.pose
+		.position
+		.to_vec3()
+		.distance(
+			joints[HandBone::RingMetacarpal as usize]
+				.pose
+				.position
+				.to_vec3(),
+		) - combined_radius;
+	(1.0 - ((grip_dist - GRIP_ACTIVACTION_DISTANCE) / (GRIP_MAX - GRIP_ACTIVACTION_DISTANCE)))
+		.clamp(0.0, 1.0)
+}
+
+fn create_hands(connection: Res<DbusConnection>, session: Res<OxrSession>, mut cmds: Commands) {
+	for handed in [HandSide::Left, HandSide::Right] {
+		let hand = (|| -> color_eyre::Result<_> {
+			let side = match handed {
+				HandSide::Left => "left",
+				HandSide::Right => "right",
+			};
+			let (palm_spatial, palm_object) = SpatialRef::create(
+				&connection,
+				&("/org/stardustxr/Hand/".to_string() + side + "/palm"),
+			);
+			let _node = Node::generate(&INTERNAL_CLIENT, false).add_to_scenegraph_owned()?;
+			Spatial::add_to(&_node.0, None, Mat4::IDENTITY, false);
+			let hand = InputDataType::Hand(Hand {
+				right: matches!(handed, HandSide::Right),
+				..Default::default()
+			});
+			let datamap = Datamap::from_typed(HandDatamap::default())?;
+			let input = InputMethod::add_to(&_node.0, hand, datamap)?;
+
+			let tracker = session.create_hand_tracker(match handed {
+				HandSide::Left => openxr::Hand::LEFT,
+				HandSide::Right => openxr::Hand::RIGHT,
+			})?;
+
+			Ok(SkHand {
+				_node,
+				palm_spatial,
+				palm_object,
+				handed,
+				input,
+				capture: None,
+				datamap: Default::default(),
+				material: OnceCell::new(),
+				vis_entity: OnceCell::new(),
+				hand_tracker: tracker,
+			})
+		})();
+		let hand = match hand {
+			Ok(v) => v,
+			Err(err) => {
+				error!("error while creating hand: {err}");
+				continue;
+			}
+		};
+		cmds.spawn(hand);
+	}
+}
+
+fn convert_joint(joint: openxr::HandJointLocation) -> Joint {
 	Joint {
-		position: Vec3::from(joint.position).into(),
+		position: joint.pose.position.to_vec3().into(),
-		rotation: Quat::from(joint.orientation).into(),
+		rotation: joint.pose.orientation.to_quat().into(),
 		radius: joint.radius,
 		distance: 0.0,
 	}
@@ -36,144 +283,33 @@ struct HandDatamap {
 	grab_strength: f32,
 }
 
+#[derive(Component)]
 pub struct SkHand {
 	_node: OwnedNode,
 	palm_spatial: Arc<Spatial>,
 	palm_object: ObjectHandle<SpatialRef>,
-	handed: Handed,
+	handed: HandSide,
 	input: Arc<InputMethod>,
-	capture_manager: CaptureManager,
+	capture: Option<Arc<InputHandler>>,
 	datamap: HandDatamap,
+	material: OnceCell<Handle<DefaultMaterial>>,
+	vis_entity: OnceCell<Entity>,
+	hand_tracker: openxr::HandTracker,
 }
 impl SkHand {
-	pub fn new(connection: &Connection, handed: Handed) -> Result<Self> {
+	fn compare_distance(&self, field: &Field) -> f32 {
-		let (palm_spatial, palm_object) = SpatialRef::create(
+		let InputDataType::Hand(hand) = &*self.input.data.lock() else {
-			connection,
+			return INFINITY;
-			&("/org/stardustxr/Hand/".to_string()
-				+ match handed {
-					Handed::Left => "left",
-					_ => "right",
-				} + "/palm"),
-		);
-		let _node = Node::generate(&INTERNAL_CLIENT, false).add_to_scenegraph_owned()?;
-		Spatial::add_to(&_node.0, None, Mat4::IDENTITY, false);
-		let hand = InputDataType::Hand(Hand {
-			right: handed == Handed::Right,
-			..Default::default()
-		});
-		let datamap = Datamap::from_typed(HandDatamap::default())?;
-		let input = InputMethod::add_to(&_node.0, hand, datamap)?;
-		Input::hand_visible(handed, true);
-
-		Ok(SkHand {
-			_node,
-			palm_spatial,
-			palm_object,
-			handed,
-			input,
-			capture_manager: CaptureManager::default(),
-			datamap: Default::default(),
-		})
-	}
-	pub fn update(&mut self, sk: &Sk, token: &MainThreadToken, material: &mut Material) {
-		let sk_hand = Input::hand(self.handed);
-		let real_hand = Input::hand_source(self.handed) as u32 == HandSource::Articulated as u32;
-		if let InputDataType::Hand(hand) = &mut *self.input.data.lock() {
-			let input_node = self.input.spatial.node().unwrap();
-			input_node.set_enabled(
-				(real_hand || sk.get_active_display_mode() == DisplayMode::Flatscreen)
-					&& sk_hand.tracked.is_active(),
-			);
-			if input_node.enabled() {
-				hand.thumb.tip = convert_joint(sk_hand.fingers[0][4]);
-				hand.thumb.distal = convert_joint(sk_hand.fingers[0][3]);
-				hand.thumb.proximal = convert_joint(sk_hand.fingers[0][2]);
-				hand.thumb.metacarpal = convert_joint(sk_hand.fingers[0][1]);
-
-				for (finger, mut sk_finger) in [
-					(&mut hand.index, sk_hand.fingers[1]),
-					(&mut hand.middle, sk_hand.fingers[2]),
-					(&mut hand.ring, sk_hand.fingers[3]),
-					(&mut hand.little, sk_hand.fingers[4]),
-				] {
-					sk_finger[4].radius = 0.0;
-					finger.tip = convert_joint(sk_finger[4]);
-					finger.distal = convert_joint(sk_finger[3]);
-					finger.intermediate = convert_joint(sk_finger[2]);
-					finger.proximal = convert_joint(sk_finger[1]);
-					finger.metacarpal = convert_joint(sk_finger[0]);
-				}
-
-				hand.palm.position = Vec3::from(sk_hand.palm.position).into();
-				hand.palm.rotation = Quat::from(sk_hand.palm.orientation).into();
-				hand.palm.radius =
-					(sk_hand.fingers[2][0].radius + sk_hand.fingers[2][1].radius) * 0.5;
-
-				self.palm_spatial
-					.set_local_transform(Mat4::from_rotation_translation(
-						hand.palm.rotation.into(),
-						hand.palm.position.into(),
-					));
-
-				hand.wrist.position = Vec3::from(sk_hand.wrist.position).into();
-				hand.wrist.rotation = Quat::from(sk_hand.wrist.orientation).into();
-				hand.wrist.radius =
-					(sk_hand.fingers[0][0].radius + sk_hand.fingers[4][0].radius) * 0.5;
-
-				hand.elbow = None;
-
-				let hand_color = if self.capture_manager.capture.is_none() {
-					Color128::new_rgb(1.0, 1.0, 1.0)
-				} else {
-					Color128::new_rgb(0.0, 1.0, 0.75)
 		};
-				material.color_tint(hand_color);
+		let spatial = &self.input.spatial;
-			}
+		let thumb_tip_distance = field.distance(spatial, hand.thumb.tip.position.into());
-		}
+		let index_tip_distance = field.distance(spatial, hand.index.tip.position.into());
-		self.datamap.pinch_strength = sk_hand.pinch_activation;
+		let middle_tip_distance = field.distance(spatial, hand.middle.tip.position.into());
-		self.datamap.grab_strength = sk_hand.grip_activation;
+		let ring_tip_distance = field.distance(spatial, hand.ring.tip.position.into());
-		*self.input.datamap.lock() = Datamap::from_typed(&self.datamap).unwrap();
 
-		let distance_calculator = |space: &Arc<Spatial>, data: &InputDataType, field: &Field| {
-			let InputDataType::Hand(hand) = data else {
-				return None;
-			};
-			let thumb_tip_distance = field.distance(space, hand.thumb.tip.position.into());
-			let index_tip_distance = field.distance(space, hand.index.tip.position.into());
-			let middle_tip_distance = field.distance(space, hand.middle.tip.position.into());
-			let ring_tip_distance = field.distance(space, hand.ring.tip.position.into());
-
-			Some(
 		(thumb_tip_distance * 0.3)
 			+ (index_tip_distance * 0.4)
 			+ (middle_tip_distance * 0.15)
-					+ (ring_tip_distance * 0.15),
+			+ (ring_tip_distance * 0.15)
-			)
-		};
-
-		self.capture_manager.update_capture(&self.input);
-		self.capture_manager
-			.set_new_capture(&self.input, distance_calculator);
-		self.capture_manager.apply_capture(&self.input);
-
-		if self.capture_manager.capture.is_some() {
-			return;
-		}
-
-		let sorted_handlers = get_sorted_handlers(&self.input, distance_calculator);
-		self.input.set_handler_order(sorted_handlers.iter());
-	}
-}
-impl Drop for SkHand {
-	fn drop(&mut self) {
-		Input::hand_visible(self.handed, false);
-	}
-}
-
-fn joint_to_line_point(joint: &Joint, color: Color128) -> LinePoint {
-	LinePoint {
-		pt: Vec3::from(joint.position).into(),
-		thickness: joint.radius * 2.0,
-		color: color.into(),
 	}
 }