diff --git a/Cargo.lock b/Cargo.lock index 24e6d92ff8..6cd8ac904d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -167,7 +167,7 @@ dependencies = [ [[package]] name = "alvr_adb" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_filesystem", @@ -179,7 +179,7 @@ dependencies = [ [[package]] name = "alvr_audio" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_session", @@ -195,7 +195,7 @@ dependencies = [ [[package]] name = "alvr_client_core" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_audio", "alvr_common", @@ -231,7 +231,7 @@ dependencies = [ [[package]] name = "alvr_client_mock" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_client_core", "alvr_common", @@ -244,7 +244,7 @@ dependencies = [ [[package]] name = "alvr_client_openxr" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_client_core", "alvr_common", @@ -260,7 +260,7 @@ dependencies = [ [[package]] name = "alvr_common" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "anyhow", "backtrace", @@ -277,7 +277,7 @@ dependencies = [ [[package]] name = "alvr_dashboard" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_adb", "alvr_common", @@ -309,7 +309,7 @@ dependencies = [ [[package]] name = "alvr_events" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_packets", @@ -320,7 +320,7 @@ dependencies = [ [[package]] name = "alvr_filesystem" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "dirs", "once_cell", @@ -328,7 +328,7 @@ dependencies = [ [[package]] name = "alvr_gui_common" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "egui", @@ -336,7 +336,7 @@ dependencies = [ [[package]] name = "alvr_launcher" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_adb", "alvr_common", @@ -359,7 +359,7 @@ dependencies = [ [[package]] name = "alvr_packets" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_session", @@ -369,7 +369,7 @@ dependencies = [ [[package]] name = "alvr_server_core" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_adb", "alvr_audio", @@ -403,7 +403,7 @@ dependencies = [ [[package]] name = "alvr_server_io" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_events", @@ -419,7 +419,7 @@ dependencies = [ [[package]] name = "alvr_server_openvr" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_filesystem", @@ -436,7 +436,7 @@ dependencies = [ [[package]] name = "alvr_session" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_filesystem", @@ -450,7 +450,7 @@ dependencies = [ [[package]] name = "alvr_sockets" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_session", @@ -463,7 +463,7 @@ dependencies = [ [[package]] name = "alvr_system_info" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "jni", @@ -477,7 +477,7 @@ dependencies = [ [[package]] name = "alvr_vrcompositor_wrapper" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_filesystem", @@ -487,7 +487,7 @@ dependencies = [ [[package]] name = "alvr_vulkan_layer" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_common", "alvr_filesystem", @@ -499,7 +499,7 @@ dependencies = [ [[package]] name = "alvr_xtask" -version = "21.0.0-dev01" +version = "21.0.0-dev02" dependencies = [ "alvr_filesystem", "pico-args", diff --git a/Cargo.toml b/Cargo.toml index 502451b834..e8d2b926f8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,7 +3,7 @@ resolver = "2" members = ["alvr/*"] [workspace.package] -version = "21.0.0-dev01" +version = "21.0.0-dev02" edition = "2021" rust-version = "1.81" authors = ["alvr-org"] diff --git a/alvr/client_core/src/connection.rs b/alvr/client_core/src/connection.rs index 8050cac1fd..026e37a42c 100644 --- a/alvr/client_core/src/connection.rs +++ b/alvr/client_core/src/connection.rs @@ -12,7 +12,7 @@ use alvr_common::{ dbg_connection, debug, error, info, parking_lot::{Condvar, Mutex, RwLock}, wait_rwlock, warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState, - Pose, RelaxedAtomic, ALVR_VERSION, + RelaxedAtomic, ALVR_VERSION, }; use alvr_packets::{ ClientConnectionResult, ClientControlPacket, ClientStatistics, Haptics, ServerControlPacket, @@ -65,9 +65,7 @@ pub struct ConnectionContext { pub statistics_sender: Mutex>>, pub statistics_manager: Mutex>, pub decoder_callback: Mutex>>, - pub head_pose_queue: RwLock>, - pub last_good_head_pose: RwLock, - pub view_params: RwLock<[ViewParams; 2]>, + pub view_params_queue: RwLock>, pub uses_multimodal_protocol: RelaxedAtomic, pub velocities_multiplier: RwLock, } @@ -320,7 +318,14 @@ fn connection_pipeline( .map(|callback| callback(header.timestamp, nal)) .unwrap_or(false); - if !submitted { + if submitted { + let view_params_lock = &mut *ctx.view_params_queue.write(); + view_params_lock.push_back((header.timestamp, header.views_params)); + + if view_params_lock.len() > 1024 { + view_params_lock.pop_front(); + } + } else { stream_corrupted = true; if let Some(sender) = &mut *ctx.control_sender.lock() { sender.send(&ClientControlPacket::RequestIdr).ok(); diff --git a/alvr/client_core/src/lib.rs b/alvr/client_core/src/lib.rs index baad14e693..fbcffb1d5e 100644 --- a/alvr/client_core/src/lib.rs +++ b/alvr/client_core/src/lib.rs @@ -23,11 +23,10 @@ use alvr_common::{ glam::{Quat, UVec2, Vec2, Vec3}, parking_lot::{Mutex, RwLock}, warn, ConnectionState, DeviceMotion, LifecycleState, Pose, HAND_LEFT_ID, HAND_RIGHT_ID, - HEAD_ID, }; use alvr_packets::{ BatteryInfo, ButtonEntry, ClientControlPacket, FaceData, ReservedClientControlPacket, - StreamConfig, Tracking, ViewParams, ViewsConfig, + StreamConfig, Tracking, ViewParams, }; use alvr_session::CodecType; use connection::{ConnectionContext, DecoderCallback}; @@ -213,15 +212,8 @@ impl ClientCoreContext { pub fn send_view_params(&self, views: [ViewParams; 2]) { dbg_client_core!("send_view_params"); - *self.connection_context.view_params.write() = views; - if let Some(sender) = &mut *self.connection_context.control_sender.lock() { - sender - .send(&ClientControlPacket::ViewsConfig(ViewsConfig { - fov: [views[0].fov, views[1].fov], - ipd_m: (views[0].pose.position - views[1].pose.position).length(), - })) - .ok(); + sender.send(&ClientControlPacket::ViewsParams(views)).ok(); } } @@ -241,30 +233,13 @@ impl ClientCoreContext { poll_timestamp }; - for (id, motion) in &mut device_motions { + { let velocity_multiplier = *self.connection_context.velocities_multiplier.read(); - motion.linear_velocity *= velocity_multiplier; - motion.angular_velocity *= velocity_multiplier; - - if *id == *HEAD_ID { - *motion = predict_motion(target_timestamp, poll_timestamp, *motion); - - let mut head_pose_queue = self.connection_context.head_pose_queue.write(); - - head_pose_queue.push_back((target_timestamp, motion.pose)); - - while head_pose_queue.len() > 1024 { - head_pose_queue.pop_front(); + if velocity_multiplier != 1.0 { + for (_, motion) in &mut device_motions { + motion.linear_velocity *= velocity_multiplier; + motion.angular_velocity *= velocity_multiplier; } - - // This is done for backward compatibiity for the v20 protocol. Will be removed with the - // tracking rewrite protocol extension. - motion.linear_velocity = Vec3::ZERO; - motion.angular_velocity = Vec3::ZERO; - } else if let Some(stats) = &*self.connection_context.statistics_manager.lock() { - let tracker_timestamp = poll_timestamp + stats.tracker_prediction_offset(); - - *motion = predict_motion(tracker_timestamp, poll_timestamp, *motion); } } @@ -343,25 +318,15 @@ impl ClientCoreContext { stats.report_compositor_start(timestamp); } - let mut head_pose = *self.connection_context.last_good_head_pose.read(); - for (ts, pose) in &*self.connection_context.head_pose_queue.read() { + let mut view_params = [ViewParams::default(); 2]; + for (ts, vp) in &*self.connection_context.view_params_queue.read() { if *ts == timestamp { - head_pose = *pose; + view_params = *vp; break; } } - let view_params = self.connection_context.view_params.read(); - - [ - ViewParams { - pose: head_pose * view_params[0].pose, - fov: view_params[0].fov, - }, - ViewParams { - pose: head_pose * view_params[1].pose, - fov: view_params[1].fov, - }, - ] + + view_params } pub fn report_submit(&self, timestamp: Duration, vsync_queue: Duration) { diff --git a/alvr/packets/src/lib.rs b/alvr/packets/src/lib.rs index 95c4f45271..0fe2057588 100644 --- a/alvr/packets/src/lib.rs +++ b/alvr/packets/src/lib.rs @@ -210,13 +210,6 @@ pub enum ServerControlPacket { ReservedBuffer(Vec), } -#[derive(Serialize, Deserialize, Clone)] -pub struct ViewsConfig { - // Note: the head-to-eye transform is always a translation along the x axis - pub ipd_m: f32, - pub fov: [Fov; 2], -} - #[derive(Serialize, Deserialize, Clone)] pub struct BatteryInfo { pub device_id: u64, @@ -257,7 +250,7 @@ pub enum ClientControlPacket { RequestIdr, KeepAlive, StreamReady, // This flag notifies the server the client streaming socket is ready listening - ViewsConfig(ViewsConfig), + ViewsParams([ViewParams; 2]), // Local frame of reference Battery(BatteryInfo), VideoErrorReport, // legacy Buttons(Vec), @@ -278,6 +271,8 @@ pub struct FaceData { #[derive(Serialize, Deserialize)] pub struct VideoPacketHeader { pub timestamp: Duration, + // These view params are in global reference frame + pub views_params: [ViewParams; 2], pub is_idr: bool, } diff --git a/alvr/server_core/src/c_api.rs b/alvr/server_core/src/c_api.rs index a9f7b59417..3ba4c28ee2 100644 --- a/alvr/server_core/src/c_api.rs +++ b/alvr/server_core/src/c_api.rs @@ -5,12 +5,13 @@ use crate::{ logging_backend, tracking::HandType, ServerCoreContext, ServerCoreEvent, SESSION_MANAGER, }; use alvr_common::{ + glam::Quat, log, once_cell::sync::Lazy, parking_lot::{Mutex, RwLock}, Fov, Pose, }; -use alvr_packets::{ButtonEntry, ButtonValue, Haptics}; +use alvr_packets::{ButtonEntry, ButtonValue, Haptics, ViewParams}; use alvr_session::CodecType; use std::{ collections::{HashMap, VecDeque}, @@ -73,6 +74,48 @@ pub struct AlvrPose { position: [f32; 3], } +fn pose_to_capi(pose: &Pose) -> AlvrPose { + AlvrPose { + orientation: AlvrQuat { + x: pose.orientation.x, + y: pose.orientation.y, + z: pose.orientation.z, + w: pose.orientation.w, + }, + position: pose.position.to_array(), + } +} + +fn capi_to_pose(pose: &AlvrPose) -> Pose { + Pose { + orientation: Quat::from_xyzw( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ), + position: pose.position.into(), + } +} + +fn fov_to_capi(fov: &Fov) -> AlvrFov { + AlvrFov { + left: fov.left, + right: fov.right, + up: fov.up, + down: fov.down, + } +} + +fn capi_to_fov(fov: &AlvrFov) -> Fov { + Fov { + left: fov.left, + right: fov.right, + up: fov.up, + down: fov.down, + } +} + #[repr(C)] pub struct AlvrDeviceMotion { pub pose: AlvrPose, @@ -107,19 +150,19 @@ pub struct AlvrBatteryInfo { pub is_plugged: bool, } +pub struct AlvrViewParams { + pub pose: AlvrPose, + pub fov: AlvrFov, +} + #[repr(u8)] pub enum AlvrEvent { ClientConnected, ClientDisconnected, Battery(AlvrBatteryInfo), PlayspaceSync([f32; 2]), - ViewsConfig { - local_view_transform: [AlvrPose; 2], - fov: [AlvrFov; 2], - }, - TrackingUpdated { - sample_timestamp_ns: u64, - }, + ViewsParams([AlvrViewParams; 2]), + TrackingUpdated { sample_timestamp_ns: u64 }, ButtonsUpdated, RequestIDR, CaptureFrame, @@ -147,27 +190,6 @@ pub struct AlvrDynamicEncoderParams { framerate: f32, } -fn pose_to_capi(pose: &Pose) -> AlvrPose { - AlvrPose { - orientation: AlvrQuat { - x: pose.orientation.x, - y: pose.orientation.y, - z: pose.orientation.z, - w: pose.orientation.w, - }, - position: pose.position.to_array(), - } -} - -fn fov_to_capi(fov: &Fov) -> AlvrFov { - AlvrFov { - left: fov.left, - right: fov.right, - up: fov.up, - down: fov.down, - } -} - fn string_to_c_str(buffer: *mut c_char, value: &str) -> u64 { let cstring = CString::new(value).unwrap(); if !buffer.is_null() { @@ -325,14 +347,24 @@ pub unsafe extern "C" fn alvr_poll_event(out_event: *mut AlvrEvent, timeout_ns: ServerCoreEvent::PlayspaceSync(bounds) => { *out_event = AlvrEvent::PlayspaceSync(bounds.to_array()) } - ServerCoreEvent::ViewsConfig(config) => { - *out_event = AlvrEvent::ViewsConfig { - local_view_transform: [ - pose_to_capi(&config.local_view_transforms[0]), - pose_to_capi(&config.local_view_transforms[1]), - ], - fov: [fov_to_capi(&config.fov[0]), fov_to_capi(&config.fov[1])], - } + ServerCoreEvent::ViewsParams(config) => { + // *out_event = AlvrEvent::ViewsConfig { + // local_view_transform: [ + // pose_to_capi(&config.local_view_transforms[0]), + // pose_to_capi(&config.local_view_transforms[1]), + // ], + // fov: [fov_to_capi(&config.fov[0]), fov_to_capi(&config.fov[1])], + // } + *out_event = AlvrEvent::ViewsParams([ + AlvrViewParams { + pose: pose_to_capi(&config[0].pose), + fov: fov_to_capi(&config[0].fov), + }, + AlvrViewParams { + pose: pose_to_capi(&config[1].pose), + fov: fov_to_capi(&config[1].fov), + }, + ]); } ServerCoreEvent::Tracking { sample_timestamp } => { *out_event = AlvrEvent::TrackingUpdated { @@ -487,13 +519,31 @@ pub unsafe extern "C" fn alvr_set_video_config_nals( #[no_mangle] pub unsafe extern "C" fn alvr_send_video_nal( timestamp_ns: u64, + view_params: *const AlvrViewParams, + is_idr: bool, buffer_ptr: *mut u8, len: i32, - is_idr: bool, ) { if let Some(context) = &*SERVER_CORE_CONTEXT.read() { + let view_params = std::slice::from_raw_parts(view_params, 2); + let view_params = [ + ViewParams { + pose: capi_to_pose(&view_params[0].pose), + fov: capi_to_fov(&view_params[0].fov), + }, + ViewParams { + pose: capi_to_pose(&view_params[1].pose), + fov: capi_to_fov(&view_params[1].fov), + }, + ]; let buffer = std::slice::from_raw_parts(buffer_ptr, len as usize); - context.send_video_nal(Duration::from_nanos(timestamp_ns), buffer.to_vec(), is_idr); + + context.send_video_nal( + Duration::from_nanos(timestamp_ns), + view_params, + is_idr, + buffer.to_vec(), + ); } } diff --git a/alvr/server_core/src/connection.rs b/alvr/server_core/src/connection.rs index 646e19f003..c4af794676 100644 --- a/alvr/server_core/src/connection.rs +++ b/alvr/server_core/src/connection.rs @@ -5,17 +5,17 @@ use crate::{ sockets::WelcomeSocket, statistics::StatisticsManager, tracking::{self, TrackingManager}, - ConnectionContext, ServerCoreEvent, ViewsConfig, FILESYSTEM_LAYOUT, SESSION_MANAGER, + ConnectionContext, ServerCoreEvent, FILESYSTEM_LAYOUT, SESSION_MANAGER, }; use alvr_adb::{WiredConnection, WiredConnectionStatus}; use alvr_common::{ con_bail, dbg_connection, debug, error, - glam::{Quat, UVec2, Vec2, Vec3}, + glam::{UVec2, Vec2}, info, parking_lot::{Condvar, Mutex, RwLock}, settings_schema::Switch, - warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState, Pose, - BUTTON_INFO, CONTROLLER_PROFILE_INFO, QUEST_CONTROLLER_PROFILE_PATH, + warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState, BUTTON_INFO, + CONTROLLER_PROFILE_INFO, QUEST_CONTROLLER_PROFILE_PATH, }; use alvr_events::{AdbEvent, ButtonEvent, EventType}; use alvr_packets::{ @@ -1176,21 +1176,9 @@ fn connection_pipeline( } ctx.events_sender.send(ServerCoreEvent::RequestIDR).ok(); } - ClientControlPacket::ViewsConfig(config) => { + ClientControlPacket::ViewsParams(view_params) => { ctx.events_sender - .send(ServerCoreEvent::ViewsConfig(ViewsConfig { - local_view_transforms: [ - Pose { - position: Vec3::new(-config.ipd_m / 2., 0., 0.), - orientation: Quat::IDENTITY, - }, - Pose { - position: Vec3::new(config.ipd_m / 2., 0., 0.), - orientation: Quat::IDENTITY, - }, - ], - fov: config.fov, - })) + .send(ServerCoreEvent::ViewsParams(view_params)) .ok(); } ClientControlPacket::Battery(packet) => { diff --git a/alvr/server_core/src/lib.rs b/alvr/server_core/src/lib.rs index d78efe2833..3800ab4ae5 100644 --- a/alvr/server_core/src/lib.rs +++ b/alvr/server_core/src/lib.rs @@ -12,7 +12,7 @@ mod web_server; pub use c_api::*; pub use logging_backend::init_logging; -pub use tracking::HandType; +pub use tracking::{predict_motion, HandType}; use crate::connection::VideoPacket; use alvr_common::{ @@ -21,14 +21,13 @@ use alvr_common::{ once_cell::sync::Lazy, parking_lot::{Mutex, RwLock}, settings_schema::Switch, - warn, ConnectionState, DeviceMotion, Fov, LifecycleState, Pose, RelaxedAtomic, - DEVICE_ID_TO_PATH, + warn, ConnectionState, DeviceMotion, LifecycleState, Pose, RelaxedAtomic, DEVICE_ID_TO_PATH, }; use alvr_events::{EventType, HapticsEvent}; use alvr_filesystem as afs; use alvr_packets::{ BatteryInfo, ButtonEntry, ClientListAction, DecoderInitializationConfig, Haptics, - VideoPacketHeader, + VideoPacketHeader, ViewParams, }; use alvr_server_io::ServerSessionManager; use alvr_session::{CodecType, OpenvrProperty, Settings}; @@ -69,13 +68,6 @@ static SESSION_MANAGER: Lazy> = Lazy::new(|| { )) }); -// todo: use this as the network packet -pub struct ViewsConfig { - // transforms relative to the head - pub local_view_transforms: [Pose; 2], - pub fov: [Fov; 2], -} - pub enum ServerCoreEvent { SetOpenvrProperty { device_id: u64, @@ -85,7 +77,7 @@ pub enum ServerCoreEvent { ClientDisconnected, Battery(BatteryInfo), PlayspaceSync(Vec2), - ViewsConfig(ViewsConfig), + ViewsParams([ViewParams; 2]), // local reference frame Tracking { sample_timestamp: Duration, }, @@ -270,6 +262,22 @@ impl ServerCoreContext { .get_device_motion(device_id, sample_timestamp) } + pub fn get_predicted_device_motion( + &self, + device_id: u64, + sample_timestamp: Duration, + target_timestamp: Duration, + ) -> Option { + dbg_server_core!( + "get_predicted_device_motion: dev={device_id} sample_ts={sample_timestamp:?} target_ts={target_timestamp:?}" + ); + + self.connection_context + .tracking_manager + .read() + .get_predicted_device_motion(device_id, sample_timestamp, target_timestamp) + } + pub fn get_hand_skeleton( &self, hand_type: HandType, @@ -287,14 +295,12 @@ impl ServerCoreContext { pub fn get_motion_to_photon_latency(&self) -> Duration { dbg_server_core!("get_total_pipeline_latency"); - // self.connection_context - // .statistics_manager - // .read() - // .as_ref() - // .map(|stats| stats.motion_to_photon_latency_average()) - // .unwrap_or_default() - - Duration::from_millis(0) + self.connection_context + .statistics_manager + .read() + .as_ref() + .map(|stats| stats.motion_to_photon_latency_average()) + .unwrap_or_default() } pub fn get_tracker_pose_time_offset(&self) -> Duration { @@ -361,7 +367,13 @@ impl ServerCoreContext { }); } - pub fn send_video_nal(&self, target_timestamp: Duration, nal_buffer: Vec, is_idr: bool) { + pub fn send_video_nal( + &self, + target_timestamp: Duration, + mut views_params: [ViewParams; 2], + is_idr: bool, + nal_buffer: Vec, + ) { dbg_server_core!("send_video_nal"); // start in the corrupts state, the client didn't receive the initial IDR yet. @@ -415,10 +427,19 @@ impl ServerCoreContext { file.write_all(&nal_buffer).ok(); } + { + let tracking_manager_lock = self.connection_context.tracking_manager.read(); + views_params[0].pose = + tracking_manager_lock.server_to_client_pose(views_params[0].pose); + views_params[1].pose = + tracking_manager_lock.server_to_client_pose(views_params[1].pose); + } + if matches!( sender.try_send(VideoPacket { header: VideoPacketHeader { timestamp: target_timestamp, + views_params, is_idr }, payload: nal_buffer, diff --git a/alvr/server_core/src/tracking/mod.rs b/alvr/server_core/src/tracking/mod.rs index c476ee0963..4d6bae413f 100644 --- a/alvr/server_core/src/tracking/mod.rs +++ b/alvr/server_core/src/tracking/mod.rs @@ -42,6 +42,32 @@ pub enum HandType { Right = 1, } +pub fn predict_motion( + motion: DeviceMotion, + sample_timestamp: Duration, + target_timestamp: Duration, +) -> DeviceMotion { + // There is no simple sub for Duration, this is needed to get signed difference + let delta_time_s = target_timestamp + .saturating_sub(sample_timestamp) + .as_secs_f32() + - sample_timestamp + .saturating_sub(target_timestamp) + .as_secs_f32(); + + let delta_position = motion.linear_velocity * delta_time_s; + let delta_orientation = Quat::from_scaled_axis(motion.angular_velocity * delta_time_s); + + DeviceMotion { + pose: Pose { + orientation: delta_orientation * motion.pose.orientation, + position: motion.pose.position + delta_position, + }, + linear_velocity: motion.linear_velocity, + angular_velocity: motion.angular_velocity, + } +} + // todo: Move this struct to Settings and use it for every tracked device #[derive(Default)] struct MotionConfig { @@ -52,8 +78,8 @@ struct MotionConfig { } pub struct TrackingManager { - last_head_pose: Pose, // client's reference space - inverse_recentering_origin: Pose, // client's reference space + last_head_pose: Pose, // client's reference space + client_to_server_recenter_pose: Pose, // client's reference space device_motions_history: HashMap>, hand_skeletons_history: [VecDeque<(Duration, [Pose; 26])>; 2], last_face_data: FaceData, @@ -63,7 +89,7 @@ impl TrackingManager { pub fn new() -> TrackingManager { TrackingManager { last_head_pose: Pose::default(), - inverse_recentering_origin: Pose::default(), + client_to_server_recenter_pose: Pose::default(), device_motions_history: HashMap::new(), hand_skeletons_history: [VecDeque::new(), VecDeque::new()], last_face_data: FaceData::default(), @@ -103,7 +129,7 @@ impl TrackingManager { RotationRecenteringMode::Tilted => self.last_head_pose.orientation, }; - self.inverse_recentering_origin = Pose { + self.client_to_server_recenter_pose = Pose { position, orientation, } @@ -111,11 +137,15 @@ impl TrackingManager { } pub fn recenter_pose(&self, pose: Pose) -> Pose { - self.inverse_recentering_origin * pose + self.client_to_server_recenter_pose * pose + } + + pub fn server_to_client_pose(&self, pose: Pose) -> Pose { + self.client_to_server_recenter_pose.inverse() * pose } pub fn recenter_motion(&self, motion: DeviceMotion) -> DeviceMotion { - self.inverse_recentering_origin * motion + self.client_to_server_recenter_pose * motion } // Performs all kinds of tracking transformations, driven by settings. @@ -142,34 +172,41 @@ impl TrackingManager { let t = controllers.left_controller_position_offset; let r = controllers.left_controller_rotation_offset; + // NB: we are currently using non-standard order of transforms for the settings (we + // apply first position then orientation, while Pose uses the opposite convention). + // It is converted in here. + let left_orientation = Quat::from_euler( + EulerRot::XYZ, + r[0] * DEG_TO_RAD, + r[1] * DEG_TO_RAD, + r[2] * DEG_TO_RAD, + ); + let left_position = Vec3::new(t[0], t[1], t[2]); device_motion_configs.insert( *HAND_LEFT_ID, MotionConfig { pose_offset: Pose { - orientation: Quat::from_euler( - EulerRot::XYZ, - r[0] * DEG_TO_RAD, - r[1] * DEG_TO_RAD, - r[2] * DEG_TO_RAD, - ), - position: Vec3::new(t[0], t[1], t[2]), + orientation: left_orientation, + position: left_orientation * left_position, }, linear_velocity_cutoff: controllers.linear_velocity_cutoff, angular_velocity_cutoff: controllers.angular_velocity_cutoff * DEG_TO_RAD, }, ); + let right_orientation = Quat::from_euler( + EulerRot::XYZ, + r[0] * DEG_TO_RAD, + -r[1] * DEG_TO_RAD, + -r[2] * DEG_TO_RAD, + ); + let right_position = Vec3::new(-t[0], t[1], t[2]); device_motion_configs.insert( *HAND_RIGHT_ID, MotionConfig { pose_offset: Pose { - orientation: Quat::from_euler( - EulerRot::XYZ, - r[0] * DEG_TO_RAD, - -r[1] * DEG_TO_RAD, - -r[2] * DEG_TO_RAD, - ), - position: Vec3::new(-t[0], t[1], t[2]), + orientation: right_orientation, + position: right_orientation * right_position, }, linear_velocity_cutoff: controllers.linear_velocity_cutoff, angular_velocity_cutoff: controllers.angular_velocity_cutoff * DEG_TO_RAD, @@ -184,18 +221,19 @@ impl TrackingManager { } if let Some(config) = device_motion_configs.get(&device_id) { + let original_motion = motion; + // Recenter - motion = self.recenter_motion(motion); + motion = self.recenter_motion(original_motion); // Apply custom transform - motion.pose.orientation *= config.pose_offset.orientation; - motion.pose.position += motion.pose.orientation * config.pose_offset.position; - - motion.linear_velocity += motion - .angular_velocity - .cross(motion.pose.orientation * config.pose_offset.position); - motion.angular_velocity = - motion.pose.orientation.conjugate() * motion.angular_velocity; + motion.pose = motion.pose * config.pose_offset; + // NB: for linear and angular velocity we need to fix it with an extensive refactoring, + // making platform-specific offsets on the client side, checking correctness by drawing + // velocity vectors in the lobby. + // SteamVR requires specific fixes because apparently it uses a different frame of reference + // for prediction, and possibly it applies rotation/translation in a different order. + // This must be fixed before merging the tracking rewrite. fn cutoff(v: Vec3, threshold: f32) -> Vec3 { if v.length_squared() > threshold * threshold { @@ -241,6 +279,17 @@ impl TrackingManager { }) } + pub fn get_predicted_device_motion( + &self, + device_id: u64, + sample_timestamp: Duration, + target_timestamp: Duration, + ) -> Option { + let motion = self.get_device_motion(device_id, sample_timestamp)?; + + Some(predict_motion(motion, sample_timestamp, target_timestamp)) + } + pub fn report_hand_skeleton( &mut self, hand_type: HandType, diff --git a/alvr/server_openvr/src/lib.rs b/alvr/server_openvr/src/lib.rs index d3a61e42d3..7de54cd6b2 100644 --- a/alvr/server_openvr/src/lib.rs +++ b/alvr/server_openvr/src/lib.rs @@ -19,14 +19,15 @@ use alvr_common::{ once_cell::sync::Lazy, parking_lot::{Mutex, RwLock}, settings_schema::Switch, - warn, BUTTON_INFO, HAND_LEFT_ID, HAND_RIGHT_ID, HAND_TRACKER_LEFT_ID, HAND_TRACKER_RIGHT_ID, - HEAD_ID, + warn, Pose, BUTTON_INFO, HAND_LEFT_ID, HAND_RIGHT_ID, HAND_TRACKER_LEFT_ID, + HAND_TRACKER_RIGHT_ID, HEAD_ID, }; use alvr_filesystem as afs; -use alvr_packets::{ButtonValue, Haptics}; +use alvr_packets::{ButtonValue, Haptics, ViewParams}; use alvr_server_core::{HandType, ServerCoreContext, ServerCoreEvent}; use alvr_session::{CodecType, ControllersConfig}; use std::{ + collections::VecDeque, ffi::{c_char, c_void, CString}, ptr, sync::{mpsc, Once}, @@ -45,6 +46,12 @@ static SERVER_CORE_CONTEXT: Lazy>> = static EVENTS_RECEIVER: Lazy>>> = Lazy::new(|| Mutex::new(None)); +// local frame of reference +static VIEWS_PARAMS: Lazy> = + Lazy::new(|| RwLock::new([ViewParams::default(); 2])); +static HEAD_POSE_QUEUE: Lazy>> = + Lazy::new(|| Mutex::new(VecDeque::new())); + extern "C" fn driver_ready_idle(set_default_chap: bool) { thread::spawn(move || { let events_receiver = EVENTS_RECEIVER.lock().take().unwrap(); @@ -88,25 +95,26 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) { ServerCoreEvent::PlayspaceSync(bounds) => unsafe { SetChaperoneArea(bounds.x, bounds.y) }, - ServerCoreEvent::ViewsConfig(config) => unsafe { + ServerCoreEvent::ViewsParams(params) => unsafe { + *VIEWS_PARAMS.write() = params; + SetViewsConfig(FfiViewsConfig { fov: [ FfiFov { - left: config.fov[0].left, - right: config.fov[0].right, - up: config.fov[0].up, - down: config.fov[0].down, + left: params[0].fov.left, + right: params[0].fov.right, + up: params[0].fov.up, + down: params[0].fov.down, }, FfiFov { - left: config.fov[1].left, - right: config.fov[1].right, - up: config.fov[1].up, - down: config.fov[1].down, + left: params[1].fov.left, + right: params[1].fov.right, + up: params[1].fov.up, + down: params[1].fov.down, }, ], // todo: send full matrix to steamvr - ipd_m: config.local_view_transforms[1].position.x - - config.local_view_transforms[0].position.x, + ipd_m: params[1].pose.position.x - params[0].pose.position.x, }); }, ServerCoreEvent::Tracking { sample_timestamp } => { @@ -121,17 +129,35 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) { .unwrap_or(false); if let Some(context) = &*SERVER_CORE_CONTEXT.read() { + let target_timestamp = + sample_timestamp + context.get_motion_to_photon_latency(); let controllers_pose_time_offset = context.get_tracker_pose_time_offset(); + let controllers_timestamp = + target_timestamp.saturating_sub(controllers_pose_time_offset); + + let predicted_head_motion = context + .get_predicted_device_motion( + *HEAD_ID, + sample_timestamp, + target_timestamp, + ) + .unwrap_or_default(); - let ffi_head_motion = context - .get_device_motion(*HEAD_ID, sample_timestamp) - .map(|m| tracking::to_ffi_motion(*HEAD_ID, m)) - .unwrap_or_else(FfiDeviceMotion::default); + let ffi_head_motion = + tracking::to_ffi_motion(*HEAD_ID, predicted_head_motion); let ffi_left_controller_motion = context - .get_device_motion(*HAND_LEFT_ID, sample_timestamp) + .get_predicted_device_motion( + *HAND_LEFT_ID, + sample_timestamp, + controllers_timestamp, + ) .map(|m| tracking::to_ffi_motion(*HAND_LEFT_ID, m)); let ffi_right_controller_motion = context - .get_device_motion(*HAND_RIGHT_ID, sample_timestamp) + .get_predicted_device_motion( + *HAND_RIGHT_ID, + sample_timestamp, + controllers_timestamp, + ) .map(|m| tracking::to_ffi_motion(*HAND_RIGHT_ID, m)); let ( @@ -235,6 +261,17 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) { ffi_body_tracker_motions.len() as i32, ) }; + + { + let mut head_pose_queue_lock = HEAD_POSE_QUEUE.lock(); + + head_pose_queue_lock + .push_back((sample_timestamp, predicted_head_motion.pose)); + + if head_pose_queue_lock.len() > 1024 { + head_pose_queue_lock.pop_front(); + } + } } } ServerCoreEvent::Buttons(entries) => { @@ -342,7 +379,25 @@ extern "C" fn set_video_config_nals(buffer_ptr: *const u8, len: i32, codec: i32) extern "C" fn send_video(timestamp_ns: u64, buffer_ptr: *mut u8, len: i32, is_idr: bool) { if let Some(context) = &*SERVER_CORE_CONTEXT.read() { let buffer = unsafe { std::slice::from_raw_parts(buffer_ptr, len as usize) }; - context.send_video_nal(Duration::from_nanos(timestamp_ns), buffer.to_vec(), is_idr); + + let mut head_pose = Pose::default(); + for (timestamp, pose) in &*HEAD_POSE_QUEUE.lock() { + if *timestamp == Duration::from_nanos(timestamp_ns) { + head_pose = *pose; + break; + } + } + + let mut view_params = *VIEWS_PARAMS.read(); + view_params[0].pose = head_pose * view_params[0].pose; + view_params[1].pose = head_pose * view_params[1].pose; + + context.send_video_nal( + Duration::from_nanos(timestamp_ns), + view_params, + is_idr, + buffer.to_vec(), + ); } }