diff --git a/alvr/server_core/src/lib.rs b/alvr/server_core/src/lib.rs index 29783c602f..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::{ diff --git a/alvr/server_core/src/tracking/mod.rs b/alvr/server_core/src/tracking/mod.rs index ea43796e43..eaecd99e0d 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 { @@ -253,25 +279,7 @@ impl TrackingManager { ) -> Option { let motion = self.get_device_motion(device_id, sample_timestamp)?; - // 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); - - Some(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, - }) + Some(predict_motion(motion, sample_timestamp, target_timestamp)) } pub fn report_hand_skeleton(