diff --git a/src/input.rs b/src/input.rs index 99d1bd43..a31c2059 100644 --- a/src/input.rs +++ b/src/input.rs @@ -11,8 +11,9 @@ mod tests; pub use devices::TrackedDeviceType; pub use profiles::{InteractionProfile, Profiles}; -use devices::{SubactionPaths, TrackedDevice, TrackedDeviceList}; +use devices::{ControllerData, SubactionPaths, TrackedDevice, TrackedDeviceList}; use skeletal::FingerState; +use skeletal::HandSkeletonBone; use skeletal::SkeletalInputActionData; use crate::{ @@ -55,7 +56,9 @@ pub struct Input { set_map: RwLock>, loaded_actions_path: OnceLock, legacy_state: legacy::LegacyState, - skeletal_tracking_level: RwLock, + skeletal_tracking_level: RwLock<[vr::EVRSkeletalTrackingLevel; 2]>, + skeletal_bone_cache: + [Mutex>; 2], estimated_finger_state: [Mutex; 2], subaction_paths: SubactionPaths, events: Mutex>, @@ -125,7 +128,8 @@ impl Input { left_hand_key, right_hand_key, legacy_state: Default::default(), - skeletal_tracking_level: RwLock::new(vr::EVRSkeletalTrackingLevel::Estimated), + skeletal_tracking_level: RwLock::new([vr::EVRSkeletalTrackingLevel::Estimated; 2]), + skeletal_bone_cache: Default::default(), estimated_finger_state: [ Mutex::new(FingerState::new()), Mutex::new(FingerState::new()), @@ -232,7 +236,6 @@ impl Input { } } -#[derive(Default)] pub struct InputSessionData { actions: OnceLock, estimated_skeleton_actions: OnceLock, @@ -241,6 +244,15 @@ pub struct InputSessionData { } impl InputSessionData { + pub(crate) fn new(session: &xr::Session) -> Self { + Self { + actions: OnceLock::new(), + estimated_skeleton_actions: OnceLock::new(), + pose_data: OnceLock::new(), + devices: RwLock::new(TrackedDeviceList::new(session)), + } + } + #[inline] fn get_loaded_actions(&self) -> Option<&ManifestLoadedActions> { match self.actions.get()? { @@ -555,7 +567,7 @@ impl vr::IVRInput010_Interface for Input { if transform_array_count < skeletal::HandSkeletonBone::Count as u32 { return vr::EVRInputError::BufferTooSmall; } - let transforms = unsafe { + let out_transforms = unsafe { std::slice::from_raw_parts_mut(transform_array, transform_array_count as usize) }; @@ -564,7 +576,12 @@ impl vr::IVRInput010_Interface for Input { return vr::EVRInputError::WrongType; }; - self.get_bones_from_hand_tracking(&session_data, transform_space, *hand, transforms); + let Some(transforms) = + self.get_bones_from_hand_tracking(&session_data, transform_space, *hand) + else { + return vr::EVRInputError::NoData; + }; + out_transforms.copy_from_slice(&transforms[0..out_transforms.len()]); vr::EVRInputError::None } fn GetSkeletalTrackingLevel( @@ -592,7 +609,7 @@ impl vr::IVRInput010_Interface for Input { if controller_type.as_deref() == Some(c"knuckles") { *level = vr::EVRSkeletalTrackingLevel::Partial; } else { - *level = *self.skeletal_tracking_level.read().unwrap(); + *level = self.skeletal_tracking_level.read().unwrap()[*hand as usize - 1]; } } vr::EVRInputError::None @@ -673,25 +690,23 @@ impl vr::IVRInput010_Interface for Input { // std::mem::size_of::() //); - let data = self.openxr.session_data.get(); - let Some(loaded) = data.input_data.get_loaded_actions() else { - return vr::EVRInputError::InvalidHandle; + get_action_from_handle!(self, action, session_data, action); + let ActionData::Skeleton(hand) = action else { + return vr::EVRInputError::WrongType; }; - let origin = match loaded.try_get_action(action) { - Ok(ActionData::Skeleton(hand)) => match hand { - Hand::Left => self.left_hand_key.data().as_ffi(), - Hand::Right => self.right_hand_key.data().as_ffi(), - }, - Ok(_) => return vr::EVRInputError::WrongType, - Err(e) => return e, + + let origin = match hand { + Hand::Left => self.left_hand_key.data().as_ffi(), + Hand::Right => self.right_hand_key.data().as_ffi(), }; - let pose_data = data.input_data.pose_data.get().unwrap(); unsafe { std::ptr::addr_of_mut!((*action_data).bActive).write( - pose_data - .grip - .is_active(&data.session, xr::Path::NULL) - .unwrap(), + self.get_bones_from_hand_tracking( + &session_data, + vr::EVRSkeletalTransformSpace::Parent, + *hand, + ) + .is_some(), ); std::ptr::addr_of_mut!((*action_data).activeOrigin).write(origin); } @@ -777,7 +792,25 @@ impl vr::IVRInput010_Interface for Input { } } - no_data!() + // We may have no interaction profile with hand tracking, try to get synthesised pose + let pose = hand.and_then(|hand| { + devices + .get_controller(hand) + .and_then(|d| d.get_pose(&self.openxr, &data, origin)) + }); + + if let Some(pose) = pose { + unsafe { + action_data.write(vr::InputPoseActionData_t { + bActive: true, + activeOrigin: 0, + pose, + }); + } + return vr::EVRInputError::None; + } else { + no_data!() + } }; let origin = hand.is_some().then_some(restrict_to_device); @@ -1371,16 +1404,17 @@ impl Input { xr::sys::Result::ERROR_EXTENSION_NOT_PRESENT | xr::sys::Result::ERROR_FEATURE_UNSUPPORTED ) { - log::warn!("Failed to create hand tracker for hand {hand:?}: {e}"); + log::debug!("Failed to create hand tracker for hand {hand:?}: {e}"); } }) .ok(); devices_to_create.push(( - TrackedDeviceType::Controller { + TrackedDeviceType::Controller(ControllerData { hand, hand_tracker, skeleton_cache: Mutex::new(Default::default()), - }, + pose_is_synthesised: false.into(), + }), Some(profile_path), Some(p), )); @@ -1419,6 +1453,10 @@ impl Input { let data = self.openxr.session_data.get(); let devices = data.input_data.devices.read().unwrap(); + for cache in self.skeletal_bone_cache.as_ref() { + std::mem::take(&mut *cache.lock().unwrap()); + } + for device in devices.iter() { device.clear_pose_cache(); } @@ -1433,8 +1471,8 @@ impl Input { // don't actually call UpdateActionState if no controllers are reported as connected, // and interaction profiles are only updated after xrSyncActions is called. So here, we // do an action sync to try and get the runtime to update the interaction profile. - if (left_hand.is_none_or(|hand| !hand.connected)) - && (right_hand.is_none_or(|hand| !hand.connected)) + if (left_hand.is_none_or(|hand| !hand.is_connected(&self.openxr, &data))) + && (right_hand.is_none_or(|hand| !hand.is_connected(&self.openxr, &data))) { debug!("no controllers connected - syncing info set"); data.session diff --git a/src/input/devices.rs b/src/input/devices.rs index dc8841c7..bdacecdd 100644 --- a/src/input/devices.rs +++ b/src/input/devices.rs @@ -1,6 +1,11 @@ +use glam::{Mat4, Quat, Vec3}; use std::collections::HashMap; +use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; use std::ffi::{CStr, CString}; -use std::sync::Mutex; +use std::sync::{ + Mutex, + atomic::{AtomicBool, Ordering}, +}; use openvr as vr; use openxr as xr; @@ -14,15 +19,22 @@ use crate::openxr_data::{self, Hand, OpenXrData, SessionData}; use crate::tracy_span; use log::trace; -use super::{Input, InteractionProfile, profiles::MainAxisType}; +use super::{ + Input, InteractionProfile, + profiles::{MainAxisType, vrlink_hand::VRLinkHand}, +}; + +pub struct ControllerData { + pub hand: Hand, + pub hand_tracker: Option, + pub skeleton_cache: + Mutex>>, + pub pose_is_synthesised: AtomicBool, +} pub enum TrackedDeviceType { Hmd, - Controller { - hand: Hand, - hand_tracker: Option, - skeleton_cache: Mutex>>, - }, + Controller(ControllerData), #[cfg(feature = "monado")] GenericTracker { space: xr::Space, @@ -34,7 +46,7 @@ impl std::fmt::Debug for TrackedDeviceType { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { match self { TrackedDeviceType::Hmd => write!(f, "HMD"), - TrackedDeviceType::Controller { hand, .. } => write!(f, "Controller ({:?})", hand), + TrackedDeviceType::Controller(data) => write!(f, "Controller ({:?})", data.hand), #[cfg(feature = "monado")] TrackedDeviceType::GenericTracker { serial, .. } => { write!(f, "Generic Tracker ({})", serial.to_string_lossy()) @@ -74,11 +86,90 @@ fn get_controller_pose( xr_data: &OpenXrData, session_data: &SessionData, controller: &TrackedDevice, + data: &ControllerData, origin: vr::ETrackingUniverseOrigin, ) -> Option { + // Try to synthesise a grip pose from palm joint + let pose_from_hand_tracking = || { + if let Some((joints, velocities)) = + controller.get_hand_skeleton(xr_data, session_data.tracking_space()) + { + let bone = joints[xr::HandJointEXT::PALM]; + let velocity = velocities[xr::HandJointEXT::PALM]; + if !bone + .location_flags + .contains(xr::SpaceLocationFlags::ORIENTATION_VALID) + { + trace!("Palm bone is untracked, returning empty pose"); + data.pose_is_synthesised.store(false, Ordering::Relaxed); + return (xr::SpaceLocation::default(), xr::SpaceVelocity::default()); + } + + let pose = { + let position = bone.pose.position; + let orientation = bone.pose.orientation; + let mut pose = Mat4::from_rotation_translation( + Quat::from_xyzw(orientation.x, orientation.y, orientation.z, orientation.w), + Vec3::from_array([position.x, position.y, position.z]), + ); + + let mut offset = Mat4::from_translation(match data.hand { + Hand::Left => Vec3::from_array([0.09, -0.03, -0.09]), + Hand::Right => Vec3::from_array([-0.09, -0.03, -0.09]), + }); + offset *= Mat4::from_euler( + glam::EulerRot::XYZ, + 0.0, + if data.hand == Hand::Left { + -FRAC_PI_4 + } else { + FRAC_PI_4 + }, + if data.hand == Hand::Left { + -FRAC_PI_2 + } else { + FRAC_PI_2 + }, + ); + + pose *= offset; + + let (_, rot, pos) = pose.to_scale_rotation_translation(); + xr::Posef { + position: xr::Vector3f { + x: pos.x, + y: pos.y, + z: pos.z, + }, + orientation: xr::Quaternionf { + x: rot.x, + y: rot.y, + z: rot.z, + w: rot.w, + }, + } + }; + + data.pose_is_synthesised.store(true, Ordering::Relaxed); + ( + xr::SpaceLocation { + location_flags: bone.location_flags, + pose, + }, + xr::SpaceVelocity { + velocity_flags: velocity.velocity_flags, + linear_velocity: velocity.linear_velocity, + angular_velocity: velocity.angular_velocity, + }, + ) + } else { + trace!("No hand tracking available, returning empty pose"); + (xr::SpaceLocation::default(), xr::SpaceVelocity::default()) + } + }; let pose_data = session_data.input_data.pose_data.get()?; - let spaces = match controller.get_controller_hand().unwrap() { + let spaces = match data.hand { Hand::Left => &pose_data.left_space, Hand::Right => &pose_data.right_space, }; @@ -86,14 +177,25 @@ fn get_controller_pose( let (location, velocity) = if let Some(raw) = spaces.try_get_or_init_raw(&controller.interaction_profile, session_data, pose_data) { - raw.relate( - session_data.get_space_for_origin(origin), - xr_data.display_time.get(), - ) - .ok()? + let pose = raw + .relate( + session_data.get_space_for_origin(origin), + xr_data.display_time.get(), + ) + .ok(); + + pose.filter(|(loc, _vel)| { + loc.location_flags + .contains(xr::SpaceLocationFlags::ORIENTATION_VALID) + }) + .inspect(|_| { + data.pose_is_synthesised.store(false, Ordering::Relaxed); + }) + .unwrap_or_else(pose_from_hand_tracking) } else { - trace!("Failed to get raw space, returning empty pose"); - (xr::SpaceLocation::default(), xr::SpaceVelocity::default()) + trace!("Failed to get raw space, trying hand tracking"); + + pose_from_hand_tracking() }; Some(vr::space_relation_to_openvr_pose(location, velocity)) @@ -147,10 +249,10 @@ impl TrackedDevice { return Some(pose); } - *pose_cache = match self.device_type { + *pose_cache = match &self.device_type { TrackedDeviceType::Hmd => get_hmd_pose(xr_data, session_data, origin), - TrackedDeviceType::Controller { .. } => { - get_controller_pose(xr_data, session_data, self, origin) + TrackedDeviceType::Controller(data) => { + get_controller_pose(xr_data, session_data, self, data, origin) } #[cfg(feature = "monado")] TrackedDeviceType::GenericTracker { .. } => { @@ -165,22 +267,17 @@ impl TrackedDevice { &self, xr_data: &OpenXrData, base: &xr::Space, - ) -> Option { - let TrackedDeviceType::Controller { - hand_tracker, - skeleton_cache, - .. - } = self.get_type() - else { + ) -> Option<(xr::HandJointLocations, xr::HandJointVelocities)> { + let TrackedDeviceType::Controller(data) = self.get_type() else { return None; }; - let mut skeleton_cache = skeleton_cache.lock().unwrap(); + let mut skeleton_cache = data.skeleton_cache.lock().unwrap(); if let Some(skeleton) = skeleton_cache.get(&base.as_raw().into_raw()) { return *skeleton; } let joints = base - .locate_hand_joints(hand_tracker.as_ref()?, xr_data.display_time.get()) + .relate_hand_joints(data.hand_tracker.as_ref()?, xr_data.display_time.get()) .unwrap_or_default(); skeleton_cache.insert(base.as_raw().into_raw(), joints); joints @@ -188,8 +285,24 @@ impl TrackedDevice { pub fn clear_pose_cache(&self) { std::mem::take(&mut *self.pose_cache.lock().unwrap()); - if let TrackedDeviceType::Controller { skeleton_cache, .. } = self.get_type() { - skeleton_cache.lock().unwrap().clear(); + if let TrackedDeviceType::Controller(data) = self.get_type() { + data.skeleton_cache.lock().unwrap().clear(); + } + } + + pub fn is_connected( + &self, + xr_data: &OpenXrData, + session_data: &SessionData, + ) -> bool { + match self.get_type() { + TrackedDeviceType::Controller { .. } => { + self.connected + || self + .get_hand_skeleton(xr_data, session_data.tracking_space()) + .is_some() + } + _ => self.connected, } } @@ -206,20 +319,45 @@ impl TrackedDevice { &self.device_type } + pub fn get_interaction_profile( + &self, + xr_data: &OpenXrData, + session_data: &SessionData, + ) -> Option<&'static dyn InteractionProfile> { + self.interaction_profile.or_else(|| { + if matches!(self.device_type, TrackedDeviceType::Controller { .. }) + && self + .get_hand_skeleton(xr_data, session_data.tracking_space()) + .is_some() + { + Some(&VRLinkHand) + } else { + None + } + }) + } + pub fn get_controller_hand(&self) -> Option { - match self.device_type { - TrackedDeviceType::Controller { hand, .. } => Some(hand), + match &self.device_type { + TrackedDeviceType::Controller(data) => Some(data.hand), _ => None, } } - fn get_string_property(&self, property: vr::ETrackedDeviceProperty) -> Option<&CStr> { - let hand = match self.device_type { - TrackedDeviceType::Controller { hand, .. } => hand, + fn get_string_property( + &self, + xr_data: &OpenXrData, + session_data: &SessionData, + property: vr::ETrackedDeviceProperty, + ) -> Option<&CStr> { + let hand = match &self.device_type { + TrackedDeviceType::Controller(data) => data.hand, _ => Hand::Left, }; - let data = self.interaction_profile?.properties(); + let data = self + .get_interaction_profile(xr_data, session_data)? + .properties(); match property { // Audica likes to apply controller specific tweaks via this property @@ -250,10 +388,17 @@ impl TrackedDevice { } } - fn get_int_property(&self, property: vr::ETrackedDeviceProperty) -> Option { + fn get_int_property( + &self, + xr_data: &OpenXrData, + session_data: &SessionData, + property: vr::ETrackedDeviceProperty, + ) -> Option { match self.device_type { TrackedDeviceType::Controller { .. } => { - let data = self.interaction_profile?.properties(); + let data = self + .get_interaction_profile(xr_data, session_data)? + .properties(); match property { vr::ETrackedDeviceProperty::Axis0Type_Int32 => match data.main_axis { @@ -279,10 +424,17 @@ impl TrackedDevice { } } - fn get_uint_property(&self, property: vr::ETrackedDeviceProperty) -> Option { + fn get_uint_property( + &self, + xr_data: &OpenXrData, + session_data: &SessionData, + property: vr::ETrackedDeviceProperty, + ) -> Option { match self.device_type { TrackedDeviceType::Controller { .. } => { - let data = self.interaction_profile?.properties(); + let data = self + .get_interaction_profile(xr_data, session_data)? + .properties(); match property { vr::ETrackedDeviceProperty::SupportedButtons_Uint64 => { @@ -318,10 +470,47 @@ pub struct TrackedDeviceList { devices: Vec, } -impl Default for TrackedDeviceList { - fn default() -> Self { +impl TrackedDeviceList { + pub fn new(session: &xr::Session) -> Self { + let create_hand_tracker = |hand: Hand| { + session + .create_hand_tracker(hand.into()) + .inspect_err(|e| { + if !matches!( + *e, + xr::sys::Result::ERROR_EXTENSION_NOT_PRESENT + | xr::sys::Result::ERROR_FEATURE_UNSUPPORTED + ) { + log::debug!("Failed to create hand tracker for hand {hand:?}: {e}"); + } + }) + .ok() + }; + Self { - devices: vec![TrackedDevice::new(TrackedDeviceType::Hmd, None, None)], + devices: vec![ + TrackedDevice::new(TrackedDeviceType::Hmd, None, None), + TrackedDevice::new( + TrackedDeviceType::Controller(ControllerData { + hand: Hand::Left, + hand_tracker: create_hand_tracker(Hand::Left), + skeleton_cache: Mutex::new(Default::default()), + pose_is_synthesised: false.into(), + }), + None, + None, + ), + TrackedDevice::new( + TrackedDeviceType::Controller(ControllerData { + hand: Hand::Right, + hand_tracker: create_hand_tracker(Hand::Right), + skeleton_cache: Mutex::new(Default::default()), + pose_is_synthesised: false.into(), + }), + None, + None, + ), + ], } } } @@ -372,7 +561,7 @@ impl TrackedDeviceList { #[cfg(feature = "monado")] pub(super) fn create_monado_generic_trackers( &mut self, - xr_data: &OpenXrData, + xr_data: &OpenXrData, session_data: &SessionData, ) -> xr::Result<()> { if !xr_data @@ -498,7 +687,9 @@ impl Input { let session_data = self.openxr.session_data.get(); let devices = session_data.input_data.devices.read().unwrap(); - devices.get_device(index).is_some_and(|d| d.connected) + devices + .get_device(index) + .is_some_and(|d| d.is_connected(&self.openxr, &session_data)) } pub fn device_index_to_tracked_device_class( @@ -543,7 +734,9 @@ impl Input { let devices = session_data.input_data.devices.read().unwrap(); let device = devices.get_device(index)?; - device.get_string_property(property).map(|s| s.to_owned()) + device + .get_string_property(&self.openxr, &session_data, property) + .map(|s| s.to_owned()) } pub fn get_device_int_tracked_property( @@ -555,7 +748,7 @@ impl Input { let devices = session_data.input_data.devices.read().unwrap(); let device = devices.get_device(index)?; - device.get_int_property(property) + device.get_int_property(&self.openxr, &session_data, property) } pub fn get_device_uint_tracked_property( @@ -567,7 +760,7 @@ impl Input { let devices = session_data.input_data.devices.read().unwrap(); let device = devices.get_device(index)?; - device.get_uint_property(property) + device.get_uint_property(&self.openxr, &session_data, property) } } @@ -591,16 +784,16 @@ mod tests { // we need to wait two frames for the tracker to be connected. frame(); - assert!(f.input.device_index_to_tracked_device_class(2).is_none()); + assert!(f.input.device_index_to_tracked_device_class(3).is_none()); frame(); assert!( - f.input.device_index_to_tracked_device_class(2) + f.input.device_index_to_tracked_device_class(3) == Some(vr::ETrackedDeviceClass::GenericTracker) ); let pose2 = f .input - .get_device_pose(2, Some(vr::ETrackingUniverseOrigin::Seated)); + .get_device_pose(3, Some(vr::ETrackingUniverseOrigin::Seated)); assert!(pose2.is_some()); } @@ -619,16 +812,16 @@ mod tests { // we need to wait two frames for the tracker to be connected. frame(); - assert!(f.input.device_index_to_tracked_device_class(2).is_none()); + assert!(f.input.device_index_to_tracked_device_class(3).is_none()); frame(); assert!( - f.input.device_index_to_tracked_device_class(2) + f.input.device_index_to_tracked_device_class(3) == Some(vr::ETrackedDeviceClass::GenericTracker) ); let serial = f .input - .get_device_string_tracked_property(2, vr::ETrackedDeviceProperty::SerialNumber_String) + .get_device_string_tracked_property(3, vr::ETrackedDeviceProperty::SerialNumber_String) .unwrap(); assert_eq!(serial.to_str().unwrap(), "FAKEXR-SERIAL"); } diff --git a/src/input/profiles.rs b/src/input/profiles.rs index c9b44bfe..d3c836cd 100644 --- a/src/input/profiles.rs +++ b/src/input/profiles.rs @@ -5,6 +5,7 @@ pub mod vive_controller; pub mod vive_focus3; #[cfg(feature = "monado")] pub mod vive_tracker; +pub mod vrlink_hand; use super::{ action_manifest::ControllerType, legacy::LegacyBindings, skeletal::SkeletalInputBindings, }; diff --git a/src/input/profiles/vrlink_hand.rs b/src/input/profiles/vrlink_hand.rs new file mode 100644 index 00000000..18920ba5 --- /dev/null +++ b/src/input/profiles/vrlink_hand.rs @@ -0,0 +1,75 @@ +use super::{ + InteractionProfile, MainAxisType, PathTranslation, ProfileProperties, Property, + SkeletalInputBindings, StringToPath, +}; +use crate::button_mask_from_ids; +use crate::input::legacy::{LegacyBindings, button_mask_from_id}; +use crate::openxr_data::Hand; +use glam::Mat4; +use openvr::EVRButtonId; + +pub struct VRLinkHand; + +impl InteractionProfile for VRLinkHand { + fn profile_path(&self) -> &'static str { + "/interaction_profiles/ext/hand_interaction_ext" + } + fn has_required_extensions(&self, _: &openxr::ExtensionSet) -> bool { + unimplemented!() + } + fn properties(&self) -> &'static ProfileProperties { + static DEVICE_PROPERTIES: ProfileProperties = ProfileProperties { + model: Property::PerHand { + left: c"VRLink Hand Tracker (Left Hand)", + right: c"VRLink Hand Tracker (Right Hand)", + }, + openvr_controller_type: c"svl_hand_interaction_augmented", + render_model_name: Property::BothHands(c"{vrlink}/rendermodels/shuttlecock"), + main_axis: MainAxisType::Thumbstick, + registered_device_type: Property::PerHand { + left: c"vrlink/VRLINKQ_HandTracker_Left", + right: c"vrlink/VRLINKQ_HandTracker_Right", + }, + serial_number: Property::PerHand { + left: c"VRLINKQ_Hand_Left", + right: c"VRLINKQ_Hand_Right", + }, + tracking_system_name: c"vrlink", + manufacturer_name: c"VRLink", + legacy_buttons_mask: button_mask_from_ids!( + EVRButtonId::System, + EVRButtonId::ApplicationMenu, + EVRButtonId::Grip, + EVRButtonId::A, + EVRButtonId::Axis0, + EVRButtonId::Axis1, + EVRButtonId::Axis2 + ), + }; + &DEVICE_PROPERTIES + } + fn translate_map(&self) -> &'static [PathTranslation] { + &[] + } + + fn legal_paths(&self) -> Box<[String]> { + [].into() + } + + fn legacy_bindings(&self, _: &dyn StringToPath) -> LegacyBindings { + unimplemented!(); + } + + fn skeletal_input_bindings(&self, _: &dyn StringToPath) -> SkeletalInputBindings { + SkeletalInputBindings { + thumb_touch: Vec::new(), + index_touch: Vec::new(), + index_curl: Vec::new(), + rest_curl: Vec::new(), + } + } + + fn offset_grip_pose(&self, _: Hand) -> Mat4 { + Mat4::IDENTITY + } +} diff --git a/src/input/skeletal.rs b/src/input/skeletal.rs index ffc1995e..ea242846 100644 --- a/src/input/skeletal.rs +++ b/src/input/skeletal.rs @@ -2,6 +2,7 @@ mod generated; use super::Input; +use crate::input::TrackedDeviceType; use crate::openxr_data::{self, Hand, SessionData}; use HandSkeletonBone::*; use glam::{Affine3A, Quat, Vec3}; @@ -11,6 +12,7 @@ use openxr::{self as xr}; use paste::paste; use std::cell::RefCell; use std::f32::consts::{FRAC_PI_2, PI}; +use std::sync::atomic::Ordering; use std::time::Instant; impl Input { @@ -19,41 +21,66 @@ impl Input { session_data: &SessionData, space: vr::EVRSkeletalTransformSpace, hand: Hand, - transforms: &mut [vr::VRBoneTransform_t], - ) { + ) -> Option<[vr::VRBoneTransform_t; Count as usize]> { use HandSkeletonBone::*; - let pose_data = session_data.input_data.pose_data.get().unwrap(); + let mut bone_cache = self.skeletal_bone_cache[hand as usize - 1].lock().unwrap(); + if let Some(mut bones) = *bone_cache { + finalize_transforms(&mut bones, space); + return Some(bones); + } + + let mut transforms: [vr::VRBoneTransform_t; Count as usize] = Default::default(); + let devices = session_data.input_data.devices.read().unwrap(); - let Some(controller) = devices.get_controller(hand) else { - self.get_estimated_bones(session_data, space, hand, transforms); - return; + let controller = devices.get_controller(hand)?; + let TrackedDeviceType::Controller(data) = controller.get_type() else { + unreachable!() }; - let Some(raw) = match hand { + let grip: xr::Posef = controller + .get_pose(&self.openxr, session_data, session_data.current_origin) + .unwrap_or_default() + .mDeviceToAbsoluteTracking + .into(); + let is_synthesised = data.pose_is_synthesised.load(Ordering::Relaxed); + + let pose_data = session_data.input_data.pose_data.get().unwrap(); + + let raw = match hand { Hand::Left => &pose_data.left_space, Hand::Right => &pose_data.right_space, } - .try_get_or_init_raw(&controller.interaction_profile, session_data, pose_data) else { - self.get_estimated_bones(session_data, space, hand, transforms); - return; + .try_get_or_init_raw(&controller.interaction_profile, session_data, pose_data); + + let base = if is_synthesised { + session_data.tracking_space() + } else { + &raw? }; - let Some(joints) = controller.get_hand_skeleton(&self.openxr, &raw) else { - self.get_estimated_bones(session_data, space, hand, transforms); - return; + let Some((joints, _)) = controller.get_hand_skeleton(&self.openxr, base) else { + drop(bone_cache); + return Some(self.get_estimated_bones(session_data, space, hand)); }; let mut joints: Box<[_]> = joints .into_iter() .map(|joint_location| { - let position = joint_location.pose.position; - let orientation = joint_location.pose.orientation; - Affine3A::from_rotation_translation( - Quat::from_xyzw(orientation.x, orientation.y, orientation.z, orientation.w), - Vec3::from_array([position.x, position.y, position.z]), - ) + let xr::Posef { + position, + orientation, + } = joint_location.pose; + let mut position = Vec3::from_array([position.x, position.y, position.z]); + if is_synthesised { + position -= + Vec3::from_array([grip.position.x, grip.position.y, grip.position.z]); + } + let orientation = + Quat::from_xyzw(orientation.x, orientation.y, orientation.z, orientation.w); + + Affine3A::from_rotation_translation(orientation, position) }) .collect(); @@ -126,16 +153,23 @@ impl Input { // applied to the wrist in the conversion method. transforms[Root as usize] = Affine3A::IDENTITY.into(); - // Currently as is, the hands will point down - // This rotation corrects them so they are pointing the correct direction - // Note that it is hand specific. - joints[xr::HandJoint::WRIST] *= match hand { - Hand::Left => { - Affine3A::from_quat(Quat::from_euler(glam::EulerRot::YZXEx, FRAC_PI_2, PI, 0.0)) - } - Hand::Right => Affine3A::from_rotation_y(-FRAC_PI_2), - }; - transforms[Wrist as usize] = joints[xr::HandJoint::WRIST].into(); + if is_synthesised { + transforms[Wrist as usize] = match hand { + Hand::Left => generated::left_hand::OPENHAND[Wrist as usize], + Hand::Right => generated::right_hand::OPENHAND[Wrist as usize], + }; + } else { + // Currently as is, the hands will point down + // This rotation corrects them so they are pointing the correct direction + // Note that it is hand specific. + joints[xr::HandJoint::WRIST] *= match hand { + Hand::Left => { + Affine3A::from_quat(Quat::from_euler(glam::EulerRot::YZXEx, FRAC_PI_2, PI, 0.0)) + } + Hand::Right => Affine3A::from_rotation_y(-FRAC_PI_2), + }; + transforms[Wrist as usize] = joints[xr::HandJoint::WRIST].into(); + } for (joint, bone) in JOINTS_TO_BONES[1..] .iter() @@ -145,21 +179,13 @@ impl Input { xr_joint_to_vr_bone(&joints[joint], &mut transforms[bone as usize]) } - // Convert back to model space if needed - // it is unnecessary to convert back and forth, but it works and it's easy - if space == vr::EVRSkeletalTransformSpace::Model { - let bone_data: Vec<(Vec3, Quat)> = parent_to_model_space_bone_data( - transforms.iter().map(|t| bone_transform_to_glam(*t)), - ) - .collect(); + *bone_cache = Some(transforms); - for (transform, (pos, rot)) in transforms.iter_mut().zip(bone_data) { - transform.position = pos.into(); - transform.orientation = rot.into(); - } - } + finalize_transforms(&mut transforms, space); + self.skeletal_tracking_level.write().unwrap()[hand as usize - 1] = + vr::EVRSkeletalTrackingLevel::Full; - *self.skeletal_tracking_level.write().unwrap() = vr::EVRSkeletalTrackingLevel::Full; + Some(transforms) } pub(super) fn get_bone_summary_from_hand_tracking( @@ -186,7 +212,7 @@ impl Input { return; }; - let Some(joints) = controller.get_hand_skeleton(&self.openxr, &raw) else { + let Some((joints, _)) = controller.get_hand_skeleton(&self.openxr, &raw) else { self.get_estimated_bone_summary(session_data, summary_type, summary_data, hand); return; }; @@ -288,8 +314,15 @@ impl Input { session_data: &SessionData, space: vr::EVRSkeletalTransformSpace, hand: Hand, - transforms: &mut [vr::VRBoneTransform_t], - ) { + ) -> [vr::VRBoneTransform_t; Count as usize] { + let mut bone_cache = self.skeletal_bone_cache[hand as usize - 1].lock().unwrap(); + if let Some(mut bones) = *bone_cache { + finalize_transforms(&mut bones, space); + return bones; + } + + let mut transforms: [vr::VRBoneTransform_t; Count as usize] = Default::default(); + let finger_state = self.get_finger_state(session_data, hand); let (open, fist) = match hand { Hand::Left => (&generated::left_hand::OPENHAND, &generated::left_hand::FIST), @@ -299,35 +332,27 @@ impl Input { ), }; - const fn constrain<'a, F, G>(f: F) -> F - where - F: Fn(&'a [vr::VRBoneTransform_t], f32) -> G, - G: Fn(usize) -> (Vec3, Quat) + 'a, - { - f - } - let bone_transform_map = constrain(|start_data: &[vr::VRBoneTransform_t], state| { - move |idx| { - let (start_pos, start_rot) = bone_transform_to_glam(start_data[idx]); - let (closed_pos, closed_rot) = bone_transform_to_glam(fist[idx]); + // lerp to target pose + for idx in HandSkeletonBone::Root as usize..HandSkeletonBone::Count as usize { + let curl_state = finger_state + .get_bone_state(unsafe { std::mem::transmute::(idx) }); - let pos = start_pos.lerp(closed_pos, state); - let rot = start_rot.slerp(closed_rot, state); + let (start_pos, start_rot) = bone_transform_to_glam(open[idx]); + let (closed_pos, closed_rot) = bone_transform_to_glam(fist[idx]); - (pos, rot) - } - }); + transforms[idx] = vr::VRBoneTransform_t { + position: start_pos.lerp(closed_pos, curl_state).into(), + orientation: start_rot.slerp(closed_rot, curl_state).into(), + }; + } - let bone_it = (0..HandSkeletonBone::Count as usize).map(|idx| { - let bone = unsafe { std::mem::transmute::(idx) }; - let curl_state = finger_state.get_bone_state(bone); + *bone_cache = Some(transforms); - let map_fn = bone_transform_map(open, curl_state); - map_fn(idx) - }); + finalize_transforms(&mut transforms, space); + self.skeletal_tracking_level.write().unwrap()[hand as usize - 1] = + vr::EVRSkeletalTrackingLevel::Estimated; - finalize_transforms(bone_it, space, transforms); - *self.skeletal_tracking_level.write().unwrap() = vr::EVRSkeletalTrackingLevel::Estimated; + transforms } pub(super) fn get_estimated_bone_summary( @@ -424,27 +449,25 @@ impl Input { hand: Hand, space: vr::EVRSkeletalTransformSpace, pose: vr::EVRSkeletalReferencePose, - transforms: &mut [vr::VRBoneTransform_t], + out_transforms: &mut [vr::VRBoneTransform_t], ) { - let skeleton = match hand { + let mut skeleton = match hand { Hand::Left => match pose { - vr::EVRSkeletalReferencePose::BindPose => &generated::left_hand::BINDPOSE, - vr::EVRSkeletalReferencePose::OpenHand => &generated::left_hand::OPENHAND, - vr::EVRSkeletalReferencePose::Fist => &generated::left_hand::FIST, - vr::EVRSkeletalReferencePose::GripLimit => &generated::left_hand::GRIPLIMIT, + vr::EVRSkeletalReferencePose::BindPose => generated::left_hand::BINDPOSE, + vr::EVRSkeletalReferencePose::OpenHand => generated::left_hand::OPENHAND, + vr::EVRSkeletalReferencePose::Fist => generated::left_hand::FIST, + vr::EVRSkeletalReferencePose::GripLimit => generated::left_hand::GRIPLIMIT, }, Hand::Right => match pose { - vr::EVRSkeletalReferencePose::BindPose => &generated::right_hand::BINDPOSE, - vr::EVRSkeletalReferencePose::OpenHand => &generated::right_hand::OPENHAND, - vr::EVRSkeletalReferencePose::Fist => &generated::right_hand::FIST, - vr::EVRSkeletalReferencePose::GripLimit => &generated::right_hand::GRIPLIMIT, + vr::EVRSkeletalReferencePose::BindPose => generated::right_hand::BINDPOSE, + vr::EVRSkeletalReferencePose::OpenHand => generated::right_hand::OPENHAND, + vr::EVRSkeletalReferencePose::Fist => generated::right_hand::FIST, + vr::EVRSkeletalReferencePose::GripLimit => generated::right_hand::GRIPLIMIT, }, }; - let bone_it = - (0..HandSkeletonBone::Count as usize).map(|idx| bone_transform_to_glam(skeleton[idx])); - - finalize_transforms(bone_it, space, transforms); + finalize_transforms(&mut skeleton, space); + out_transforms.copy_from_slice(&skeleton[0..out_transforms.len()]); } } @@ -504,38 +527,21 @@ fn bone_transform_to_glam(transform: vr::VRBoneTransform_t) -> (Vec3, Quat) { } fn finalize_transforms( - bone_iterator: impl PoseIterator, - space: vr::EVRSkeletalTransformSpace, transforms: &mut [vr::VRBoneTransform_t], + space: vr::EVRSkeletalTransformSpace, ) { - // If we need to convert our iterator to model space, it will become a different type - - // this is the only reason for this enum existing - enum TransformedIt { - Parent(T), - Model(U), - } - let mut full_it = TransformedIt::Parent(bone_iterator); - + // Convert back to model space if needed + // it is unnecessary to convert back and forth, but it works and it's easy if space == vr::EVRSkeletalTransformSpace::Model { - let TransformedIt::Parent(it) = full_it else { - unreachable!(); - }; - full_it = TransformedIt::Model(parent_to_model_space_bone_data(it)); - } + let bone_data: Vec<(Vec3, Quat)> = + parent_to_model_space_bone_data(transforms.iter().map(|t| bone_transform_to_glam(*t))) + .collect(); - fn convert(it: impl PoseIterator, transforms: &mut [vr::VRBoneTransform_t]) { - for ((pos, rot), transform) in it.zip(transforms) { - *transform = vr::VRBoneTransform_t { - position: pos.into(), - orientation: rot.into(), - }; + for (transform, (pos, rot)) in transforms.iter_mut().zip(bone_data) { + transform.position = pos.into(); + transform.orientation = rot.into(); } } - - match full_it { - TransformedIt::Parent(it) => convert(it, transforms), - TransformedIt::Model(it) => convert(it, transforms), - } } macro_rules! joints_for_finger { diff --git a/src/input/tests.rs b/src/input/tests.rs index 948819cf..14dedd77 100644 --- a/src/input/tests.rs +++ b/src/input/tests.rs @@ -517,18 +517,16 @@ fn raw_pose_waitgetposes_and_skeletal_pose_identical() { }; // we need to wait two frames for the controller to be connected. - frame(); - assert!( - f.input - .get_controller_device_index(super::Hand::Left) - .is_none() - ); - frame(); - assert!( - f.input + { + let idx = f + .input .get_controller_device_index(super::Hand::Left) - .is_some() - ); + .unwrap(); + frame(); + assert!(!f.input.is_device_connected(idx)); + frame(); + assert!(f.input.is_device_connected(idx)); + } let rot = Quat::from_rotation_x(-FRAC_PI_4); let pose = xr::Posef { @@ -1011,18 +1009,18 @@ fn detect_controller_after_manifest_load() { input.openxr.poll_events(); input.frame_start_update(); }; + let index = f.input.get_controller_device_index(Hand::Left).unwrap(); frame(); - assert!(f.input.get_controller_device_index(Hand::Left).is_none()); + assert!(!f.input.is_device_connected(index)); f.set_interaction_profile(&Knuckles, fakexr::UserPath::LeftHand); frame(); // Profile won't be set for this frame - we call sync after events have already been polled - assert!(f.input.get_controller_device_index(Hand::Left).is_none()); + assert!(!f.input.is_device_connected(index)); frame(); - let index = f.input.get_controller_device_index(Hand::Left); - assert!(index.is_some_and(|i| f.input.is_device_connected(i))); + assert!(f.input.is_device_connected(index)); } #[test] diff --git a/src/openxr_data.rs b/src/openxr_data.rs index ab498044..2571b08e 100644 --- a/src/openxr_data.rs +++ b/src/openxr_data.rs @@ -1,6 +1,7 @@ use crate::{ clientcore::{Injected, Injector}, graphics_backends::{GraphicsBackend, VulkanData, supported_apis_enum}, + input::InputSessionData, }; use derive_more::Deref; use glam::f32::{Quat, Vec3}; @@ -429,7 +430,7 @@ pub struct SessionData { stage_space_adjusted: xr::Space, pub current_origin: vr::ETrackingUniverseOrigin, - pub input_data: crate::input::InputSessionData, + pub input_data: InputSessionData, pub comp_data: crate::compositor::CompositorSessionData, pub overlay_data: crate::overlay::OverlaySessionData, /// OpenXR requires graphics information before creating a session, but OpenVR clients don't @@ -553,6 +554,8 @@ impl SessionData { .map_err(SessionCreationError::BeginSessionFailed)?; info!("Began OpenXR session."); + let input_data = InputSessionData::new(&session); + Ok(( SessionData { temp_vulkan, @@ -564,7 +567,7 @@ impl SessionData { local_space_adjusted, stage_space_reference, stage_space_adjusted, - input_data: Default::default(), + input_data, comp_data: Default::default(), overlay_data: Default::default(), current_origin,