diff --git a/src/objects/input/eye_pointer.rs b/src/objects/input/eye_pointer.rs index bae4316..c5593a2 100644 --- a/src/objects/input/eye_pointer.rs +++ b/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(); - self.spatial - .set_local_transform(Mat4::from_rotation_translation( - ray.orientation.into(), - ray.position.into(), - )); - { - // Set pointer input datamap - *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 - let rx = 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() - }) - // 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 - .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()); + // let ray = Input::get_eyes(); + // self.spatial + // .set_local_transform(Mat4::from_rotation_translation( + // ray.orientation.into(), + // ray.position.into(), + // )); + // { + // // Set pointer input datamap + // *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 + // let rx = 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() + // }) + // // 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 + // .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()); } } diff --git a/src/objects/input/sk_controller.rs b/src/objects/input/sk_controller.rs index f6fe00b..eb1d75b 100644 --- a/src/objects/input/sk_controller.rs +++ b/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>, - mut query: Query<(&SkController, &mut Transform)>, + mut query: Query<(&mut SkController, &mut Transform)>, time: Res, base_space: Res, session: ResMut, ) { - 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( diff --git a/src/objects/input/sk_hand.rs b/src/objects/input/sk_hand.rs index 5b7090e..2a64d6a 100644 --- a/src/objects/input/sk_hand.rs +++ b/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 stereokit_rust::sk::{DisplayMode, MainThreadToken, Sk}; -use stereokit_rust::system::{HandJoint, HandSource, Handed, Input, LinePoint, Lines}; -use stereokit_rust::util::Color128; +use tracing::error; 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>, + mut query: Query<&mut SkHand>, + time: Res, + base_space: Res, + session: ResMut, +) { + 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, session: Res, 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(), - rotation: Quat::from(joint.orientation).into(), + position: joint.pose.position.to_vec3().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, palm_object: ObjectHandle, - handed: Handed, + handed: HandSide, input: Arc, - capture_manager: CaptureManager, + capture: Option>, datamap: HandDatamap, + material: OnceCell>, + vis_entity: OnceCell, + hand_tracker: openxr::HandTracker, } impl SkHand { - pub fn new(connection: &Connection, handed: Handed) -> Result { - let (palm_spatial, palm_object) = SpatialRef::create( - connection, - &("/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); - } - } - self.datamap.pinch_strength = sk_hand.pinch_activation; - self.datamap.grab_strength = sk_hand.grip_activation; - *self.input.datamap.lock() = Datamap::from_typed(&self.datamap).unwrap(); - - let distance_calculator = |space: &Arc, 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), - ) + fn compare_distance(&self, field: &Field) -> f32 { + let InputDataType::Hand(hand) = &*self.input.data.lock() else { + return INFINITY; }; + 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()); + let middle_tip_distance = field.distance(spatial, hand.middle.tip.position.into()); + let ring_tip_distance = field.distance(spatial, hand.ring.tip.position.into()); - 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(), + (thumb_tip_distance * 0.3) + + (index_tip_distance * 0.4) + + (middle_tip_distance * 0.15) + + (ring_tip_distance * 0.15) } }