Skip to content

Commit 81549f7

Browse files
zmerpzmerp
committedFeb 13, 2025·
tracking rewrite
1 parent 57d7283 commit 81549f7

File tree

10 files changed

+307
-216
lines changed

10 files changed

+307
-216
lines changed
 

‎Cargo.lock

+22-22
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎Cargo.toml

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ resolver = "2"
33
members = ["alvr/*"]
44

55
[workspace.package]
6-
version = "21.0.0-dev01"
6+
version = "21.0.0-dev02"
77
edition = "2021"
88
rust-version = "1.81"
99
authors = ["alvr-org"]

‎alvr/client_core/src/connection.rs

+10-5
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ use alvr_common::{
1212
dbg_connection, debug, error, info,
1313
parking_lot::{Condvar, Mutex, RwLock},
1414
wait_rwlock, warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState,
15-
Pose, RelaxedAtomic, ALVR_VERSION,
15+
RelaxedAtomic, ALVR_VERSION,
1616
};
1717
use alvr_packets::{
1818
ClientConnectionResult, ClientControlPacket, ClientStatistics, Haptics, ServerControlPacket,
@@ -65,9 +65,7 @@ pub struct ConnectionContext {
6565
pub statistics_sender: Mutex<Option<StreamSender<ClientStatistics>>>,
6666
pub statistics_manager: Mutex<Option<StatisticsManager>>,
6767
pub decoder_callback: Mutex<Option<Box<DecoderCallback>>>,
68-
pub head_pose_queue: RwLock<VecDeque<(Duration, Pose)>>,
69-
pub last_good_head_pose: RwLock<Pose>,
70-
pub view_params: RwLock<[ViewParams; 2]>,
68+
pub view_params_queue: RwLock<VecDeque<(Duration, [ViewParams; 2])>>,
7169
pub uses_multimodal_protocol: RelaxedAtomic,
7270
pub velocities_multiplier: RwLock<f32>,
7371
pub max_prediction: RwLock<Duration>,
@@ -322,7 +320,14 @@ fn connection_pipeline(
322320
.map(|callback| callback(header.timestamp, nal))
323321
.unwrap_or(false);
324322

325-
if !submitted {
323+
if submitted {
324+
let view_params_lock = &mut *ctx.view_params_queue.write();
325+
view_params_lock.push_back((header.timestamp, header.views_params));
326+
327+
if view_params_lock.len() > 1024 {
328+
view_params_lock.pop_front();
329+
}
330+
} else {
326331
stream_corrupted = true;
327332
if let Some(sender) = &mut *ctx.control_sender.lock() {
328333
sender.send(&ClientControlPacket::RequestIdr).ok();

‎alvr/client_core/src/lib.rs

+14-53
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,10 @@ use alvr_common::{
2222
glam::{UVec2, Vec2, Vec3},
2323
parking_lot::{Mutex, RwLock},
2424
warn, ConnectionState, DeviceMotion, LifecycleState, Pose, HAND_LEFT_ID, HAND_RIGHT_ID,
25-
HEAD_ID,
2625
};
2726
use alvr_packets::{
2827
BatteryInfo, ButtonEntry, ClientControlPacket, FaceData, RealTimeConfig,
29-
ReservedClientControlPacket, StreamConfig, Tracking, ViewParams, ViewsConfig,
28+
ReservedClientControlPacket, StreamConfig, Tracking, ViewParams,
3029
};
3130
use alvr_session::CodecType;
3231
use connection::{ConnectionContext, DecoderCallback};
@@ -212,15 +211,8 @@ impl ClientCoreContext {
212211
pub fn send_view_params(&self, views: [ViewParams; 2]) {
213212
dbg_client_core!("send_view_params");
214213

215-
*self.connection_context.view_params.write() = views;
216-
217214
if let Some(sender) = &mut *self.connection_context.control_sender.lock() {
218-
sender
219-
.send(&ClientControlPacket::ViewsConfig(ViewsConfig {
220-
fov: [views[0].fov, views[1].fov],
221-
ipd_m: (views[0].pose.position - views[1].pose.position).length(),
222-
}))
223-
.ok();
215+
sender.send(&ClientControlPacket::ViewsParams(views)).ok();
224216
}
225217
}
226218

@@ -243,34 +235,13 @@ impl ClientCoreContext {
243235
poll_timestamp
244236
};
245237

246-
// Guarantee that sent timestamps never go backwards by sending the poll time
247-
let reported_timestamp = poll_timestamp;
248-
249-
for (id, motion) in &mut device_motions {
238+
{
250239
let velocity_multiplier = *self.connection_context.velocities_multiplier.read();
251-
motion.linear_velocity *= velocity_multiplier;
252-
motion.angular_velocity *= velocity_multiplier;
253-
254-
if *id == *HEAD_ID {
255-
*motion = motion.predict(poll_timestamp, target_timestamp);
256-
257-
let mut head_pose_queue = self.connection_context.head_pose_queue.write();
258-
259-
head_pose_queue.push_back((reported_timestamp, motion.pose));
260-
261-
while head_pose_queue.len() > 1024 {
262-
head_pose_queue.pop_front();
240+
if velocity_multiplier != 1.0 {
241+
for (_, motion) in &mut device_motions {
242+
motion.linear_velocity *= velocity_multiplier;
243+
motion.angular_velocity *= velocity_multiplier;
263244
}
264-
265-
// This is done for backward compatibiity for the v20 protocol. Will be removed with the
266-
// tracking rewrite protocol extension.
267-
motion.linear_velocity = Vec3::ZERO;
268-
motion.angular_velocity = Vec3::ZERO;
269-
} else if let Some(stats) = &*self.connection_context.statistics_manager.lock() {
270-
let tracker_timestamp = poll_timestamp
271-
+ Duration::min(stats.tracker_prediction_offset(), max_prediction);
272-
273-
*motion = motion.predict(poll_timestamp, tracker_timestamp);
274245
}
275246
}
276247

@@ -303,15 +274,15 @@ impl ClientCoreContext {
303274
if let Some(sender) = &mut *self.connection_context.tracking_sender.lock() {
304275
sender
305276
.send_header(&Tracking {
306-
target_timestamp: reported_timestamp,
277+
target_timestamp,
307278
device_motions,
308279
hand_skeletons,
309280
face_data,
310281
})
311282
.ok();
312283

313284
if let Some(stats) = &mut *self.connection_context.statistics_manager.lock() {
314-
stats.report_input_acquired(reported_timestamp);
285+
stats.report_input_acquired(target_timestamp);
315286
}
316287
}
317288
}
@@ -359,25 +330,15 @@ impl ClientCoreContext {
359330
stats.report_compositor_start(timestamp);
360331
}
361332

362-
let mut head_pose = *self.connection_context.last_good_head_pose.read();
363-
for (ts, pose) in &*self.connection_context.head_pose_queue.read() {
333+
let mut view_params = [ViewParams::default(); 2];
334+
for (ts, vp) in &*self.connection_context.view_params_queue.read() {
364335
if *ts == timestamp {
365-
head_pose = *pose;
336+
view_params = *vp;
366337
break;
367338
}
368339
}
369-
let view_params = self.connection_context.view_params.read();
370-
371-
[
372-
ViewParams {
373-
pose: head_pose * view_params[0].pose,
374-
fov: view_params[0].fov,
375-
},
376-
ViewParams {
377-
pose: head_pose * view_params[1].pose,
378-
fov: view_params[1].fov,
379-
},
380-
]
340+
341+
view_params
381342
}
382343

383344
pub fn report_submit(&self, timestamp: Duration, vsync_queue: Duration) {

‎alvr/packets/src/lib.rs

+3-8
Original file line numberDiff line numberDiff line change
@@ -214,13 +214,6 @@ pub enum ServerControlPacket {
214214
ReservedBuffer(Vec<u8>),
215215
}
216216

217-
#[derive(Serialize, Deserialize, Clone)]
218-
pub struct ViewsConfig {
219-
// Note: the head-to-eye transform is always a translation along the x axis
220-
pub ipd_m: f32,
221-
pub fov: [Fov; 2],
222-
}
223-
224217
#[derive(Serialize, Deserialize, Clone)]
225218
pub struct BatteryInfo {
226219
pub device_id: u64,
@@ -261,7 +254,7 @@ pub enum ClientControlPacket {
261254
RequestIdr,
262255
KeepAlive,
263256
StreamReady, // This flag notifies the server the client streaming socket is ready listening
264-
ViewsConfig(ViewsConfig),
257+
ViewsParams([ViewParams; 2]), // Local frame of reference
265258
Battery(BatteryInfo),
266259
VideoErrorReport, // legacy
267260
Buttons(Vec<ButtonEntry>),
@@ -282,6 +275,8 @@ pub struct FaceData {
282275
#[derive(Serialize, Deserialize)]
283276
pub struct VideoPacketHeader {
284277
pub timestamp: Duration,
278+
// These view params are in global reference frame
279+
pub views_params: [ViewParams; 2],
285280
pub is_idr: bool,
286281
}
287282

‎alvr/server_core/src/c_api.rs

+82-39
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,13 @@ use crate::{
55
logging_backend, tracking::HandType, ServerCoreContext, ServerCoreEvent, SESSION_MANAGER,
66
};
77
use alvr_common::{
8+
glam::Quat,
89
log,
910
once_cell::sync::Lazy,
1011
parking_lot::{Mutex, RwLock},
1112
Fov, Pose,
1213
};
13-
use alvr_packets::{ButtonEntry, ButtonValue, Haptics};
14+
use alvr_packets::{ButtonEntry, ButtonValue, Haptics, ViewParams};
1415
use alvr_session::CodecType;
1516
use std::{
1617
collections::{HashMap, VecDeque},
@@ -73,6 +74,48 @@ pub struct AlvrPose {
7374
position: [f32; 3],
7475
}
7576

77+
fn pose_to_capi(pose: &Pose) -> AlvrPose {
78+
AlvrPose {
79+
orientation: AlvrQuat {
80+
x: pose.orientation.x,
81+
y: pose.orientation.y,
82+
z: pose.orientation.z,
83+
w: pose.orientation.w,
84+
},
85+
position: pose.position.to_array(),
86+
}
87+
}
88+
89+
fn capi_to_pose(pose: &AlvrPose) -> Pose {
90+
Pose {
91+
orientation: Quat::from_xyzw(
92+
pose.orientation.x,
93+
pose.orientation.y,
94+
pose.orientation.z,
95+
pose.orientation.w,
96+
),
97+
position: pose.position.into(),
98+
}
99+
}
100+
101+
fn fov_to_capi(fov: &Fov) -> AlvrFov {
102+
AlvrFov {
103+
left: fov.left,
104+
right: fov.right,
105+
up: fov.up,
106+
down: fov.down,
107+
}
108+
}
109+
110+
fn capi_to_fov(fov: &AlvrFov) -> Fov {
111+
Fov {
112+
left: fov.left,
113+
right: fov.right,
114+
up: fov.up,
115+
down: fov.down,
116+
}
117+
}
118+
76119
#[repr(C)]
77120
pub struct AlvrDeviceMotion {
78121
pub pose: AlvrPose,
@@ -107,19 +150,19 @@ pub struct AlvrBatteryInfo {
107150
pub is_plugged: bool,
108151
}
109152

153+
pub struct AlvrViewParams {
154+
pub pose: AlvrPose,
155+
pub fov: AlvrFov,
156+
}
157+
110158
#[repr(u8)]
111159
pub enum AlvrEvent {
112160
ClientConnected,
113161
ClientDisconnected,
114162
Battery(AlvrBatteryInfo),
115163
PlayspaceSync([f32; 2]),
116-
ViewsConfig {
117-
local_view_transform: [AlvrPose; 2],
118-
fov: [AlvrFov; 2],
119-
},
120-
TrackingUpdated {
121-
sample_timestamp_ns: u64,
122-
},
164+
ViewsParams([AlvrViewParams; 2]),
165+
TrackingUpdated { sample_timestamp_ns: u64 },
123166
ButtonsUpdated,
124167
RequestIDR,
125168
CaptureFrame,
@@ -147,27 +190,6 @@ pub struct AlvrDynamicEncoderParams {
147190
framerate: f32,
148191
}
149192

150-
fn pose_to_capi(pose: &Pose) -> AlvrPose {
151-
AlvrPose {
152-
orientation: AlvrQuat {
153-
x: pose.orientation.x,
154-
y: pose.orientation.y,
155-
z: pose.orientation.z,
156-
w: pose.orientation.w,
157-
},
158-
position: pose.position.to_array(),
159-
}
160-
}
161-
162-
fn fov_to_capi(fov: &Fov) -> AlvrFov {
163-
AlvrFov {
164-
left: fov.left,
165-
right: fov.right,
166-
up: fov.up,
167-
down: fov.down,
168-
}
169-
}
170-
171193
fn string_to_c_str(buffer: *mut c_char, value: &str) -> u64 {
172194
let cstring = CString::new(value).unwrap();
173195
if !buffer.is_null() {
@@ -325,14 +347,17 @@ pub unsafe extern "C" fn alvr_poll_event(out_event: *mut AlvrEvent, timeout_ns:
325347
ServerCoreEvent::PlayspaceSync(bounds) => {
326348
*out_event = AlvrEvent::PlayspaceSync(bounds.to_array())
327349
}
328-
ServerCoreEvent::ViewsConfig(config) => {
329-
*out_event = AlvrEvent::ViewsConfig {
330-
local_view_transform: [
331-
pose_to_capi(&config.local_view_transforms[0]),
332-
pose_to_capi(&config.local_view_transforms[1]),
333-
],
334-
fov: [fov_to_capi(&config.fov[0]), fov_to_capi(&config.fov[1])],
335-
}
350+
ServerCoreEvent::ViewsParams(config) => {
351+
*out_event = AlvrEvent::ViewsParams([
352+
AlvrViewParams {
353+
pose: pose_to_capi(&config[0].pose),
354+
fov: fov_to_capi(&config[0].fov),
355+
},
356+
AlvrViewParams {
357+
pose: pose_to_capi(&config[1].pose),
358+
fov: fov_to_capi(&config[1].fov),
359+
},
360+
]);
336361
}
337362
ServerCoreEvent::Tracking { sample_timestamp } => {
338363
*out_event = AlvrEvent::TrackingUpdated {
@@ -487,13 +512,31 @@ pub unsafe extern "C" fn alvr_set_video_config_nals(
487512
#[no_mangle]
488513
pub unsafe extern "C" fn alvr_send_video_nal(
489514
timestamp_ns: u64,
515+
view_params: *const AlvrViewParams,
516+
is_idr: bool,
490517
buffer_ptr: *mut u8,
491518
len: i32,
492-
is_idr: bool,
493519
) {
494520
if let Some(context) = &*SERVER_CORE_CONTEXT.read() {
521+
let view_params = std::slice::from_raw_parts(view_params, 2);
522+
let view_params = [
523+
ViewParams {
524+
pose: capi_to_pose(&view_params[0].pose),
525+
fov: capi_to_fov(&view_params[0].fov),
526+
},
527+
ViewParams {
528+
pose: capi_to_pose(&view_params[1].pose),
529+
fov: capi_to_fov(&view_params[1].fov),
530+
},
531+
];
495532
let buffer = std::slice::from_raw_parts(buffer_ptr, len as usize);
496-
context.send_video_nal(Duration::from_nanos(timestamp_ns), buffer.to_vec(), is_idr);
533+
534+
context.send_video_nal(
535+
Duration::from_nanos(timestamp_ns),
536+
view_params,
537+
is_idr,
538+
buffer.to_vec(),
539+
);
497540
}
498541
}
499542

‎alvr/server_core/src/connection.rs

+6-18
Original file line numberDiff line numberDiff line change
@@ -5,17 +5,17 @@ use crate::{
55
sockets::WelcomeSocket,
66
statistics::StatisticsManager,
77
tracking::{self, TrackingManager},
8-
ConnectionContext, ServerCoreEvent, ViewsConfig, FILESYSTEM_LAYOUT, SESSION_MANAGER,
8+
ConnectionContext, ServerCoreEvent, FILESYSTEM_LAYOUT, SESSION_MANAGER,
99
};
1010
use alvr_adb::{WiredConnection, WiredConnectionStatus};
1111
use alvr_common::{
1212
con_bail, dbg_connection, debug, error,
13-
glam::{Quat, UVec2, Vec2, Vec3},
13+
glam::{UVec2, Vec2},
1414
info,
1515
parking_lot::{Condvar, Mutex, RwLock},
1616
settings_schema::Switch,
17-
warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState, Pose,
18-
BUTTON_INFO, CONTROLLER_PROFILE_INFO, QUEST_CONTROLLER_PROFILE_PATH,
17+
warn, AnyhowToCon, ConResult, ConnectionError, ConnectionState, LifecycleState, BUTTON_INFO,
18+
CONTROLLER_PROFILE_INFO, QUEST_CONTROLLER_PROFILE_PATH,
1919
};
2020
use alvr_events::{AdbEvent, ButtonEvent, EventType};
2121
use alvr_packets::{
@@ -1202,21 +1202,9 @@ fn connection_pipeline(
12021202
}
12031203
ctx.events_sender.send(ServerCoreEvent::RequestIDR).ok();
12041204
}
1205-
ClientControlPacket::ViewsConfig(config) => {
1205+
ClientControlPacket::ViewsParams(view_params) => {
12061206
ctx.events_sender
1207-
.send(ServerCoreEvent::ViewsConfig(ViewsConfig {
1208-
local_view_transforms: [
1209-
Pose {
1210-
position: Vec3::new(-config.ipd_m / 2., 0., 0.),
1211-
orientation: Quat::IDENTITY,
1212-
},
1213-
Pose {
1214-
position: Vec3::new(config.ipd_m / 2., 0., 0.),
1215-
orientation: Quat::IDENTITY,
1216-
},
1217-
],
1218-
fov: config.fov,
1219-
}))
1207+
.send(ServerCoreEvent::ViewsParams(view_params))
12201208
.ok();
12211209
}
12221210
ClientControlPacket::Battery(packet) => {

‎alvr/server_core/src/lib.rs

+41-20
Original file line numberDiff line numberDiff line change
@@ -21,14 +21,13 @@ use alvr_common::{
2121
once_cell::sync::Lazy,
2222
parking_lot::{Mutex, RwLock},
2323
settings_schema::Switch,
24-
warn, ConnectionState, DeviceMotion, Fov, LifecycleState, Pose, RelaxedAtomic,
25-
DEVICE_ID_TO_PATH,
24+
warn, ConnectionState, DeviceMotion, LifecycleState, Pose, RelaxedAtomic, DEVICE_ID_TO_PATH,
2625
};
2726
use alvr_events::{EventType, HapticsEvent};
2827
use alvr_filesystem as afs;
2928
use alvr_packets::{
3029
BatteryInfo, ButtonEntry, ClientListAction, DecoderInitializationConfig, Haptics,
31-
VideoPacketHeader,
30+
VideoPacketHeader, ViewParams,
3231
};
3332
use alvr_server_io::ServerSessionManager;
3433
use alvr_session::{CodecType, OpenvrProperty, Settings};
@@ -69,13 +68,6 @@ static SESSION_MANAGER: Lazy<RwLock<ServerSessionManager>> = Lazy::new(|| {
6968
))
7069
});
7170

72-
// todo: use this as the network packet
73-
pub struct ViewsConfig {
74-
// transforms relative to the head
75-
pub local_view_transforms: [Pose; 2],
76-
pub fov: [Fov; 2],
77-
}
78-
7971
pub enum ServerCoreEvent {
8072
SetOpenvrProperty {
8173
device_id: u64,
@@ -85,7 +77,7 @@ pub enum ServerCoreEvent {
8577
ClientDisconnected,
8678
Battery(BatteryInfo),
8779
PlayspaceSync(Vec2),
88-
ViewsConfig(ViewsConfig),
80+
ViewsParams([ViewParams; 2]), // local reference frame
8981
Tracking {
9082
sample_timestamp: Duration,
9183
},
@@ -272,6 +264,22 @@ impl ServerCoreContext {
272264
.get_device_motion(device_id, sample_timestamp)
273265
}
274266

267+
pub fn get_predicted_device_motion(
268+
&self,
269+
device_id: u64,
270+
sample_timestamp: Duration,
271+
target_timestamp: Duration,
272+
) -> Option<DeviceMotion> {
273+
dbg_server_core!(
274+
"get_predicted_device_motion: dev={device_id} sample_ts={sample_timestamp:?} target_ts={target_timestamp:?}"
275+
);
276+
277+
self.connection_context
278+
.tracking_manager
279+
.read()
280+
.get_predicted_device_motion(device_id, sample_timestamp, target_timestamp)
281+
}
282+
275283
pub fn get_hand_skeleton(
276284
&self,
277285
hand_type: HandType,
@@ -289,14 +297,12 @@ impl ServerCoreContext {
289297
pub fn get_motion_to_photon_latency(&self) -> Duration {
290298
dbg_server_core!("get_total_pipeline_latency");
291299

292-
// self.connection_context
293-
// .statistics_manager
294-
// .read()
295-
// .as_ref()
296-
// .map(|stats| stats.motion_to_photon_latency_average())
297-
// .unwrap_or_default()
298-
299-
Duration::from_millis(0)
300+
self.connection_context
301+
.statistics_manager
302+
.read()
303+
.as_ref()
304+
.map(|stats| stats.motion_to_photon_latency_average())
305+
.unwrap_or_default()
300306
}
301307

302308
pub fn get_tracker_pose_time_offset(&self) -> Duration {
@@ -363,7 +369,13 @@ impl ServerCoreContext {
363369
});
364370
}
365371

366-
pub fn send_video_nal(&self, target_timestamp: Duration, nal_buffer: Vec<u8>, is_idr: bool) {
372+
pub fn send_video_nal(
373+
&self,
374+
target_timestamp: Duration,
375+
mut views_params: [ViewParams; 2],
376+
is_idr: bool,
377+
nal_buffer: Vec<u8>,
378+
) {
367379
dbg_server_core!("send_video_nal");
368380

369381
// start in the corrupts state, the client didn't receive the initial IDR yet.
@@ -417,10 +429,19 @@ impl ServerCoreContext {
417429
file.write_all(&nal_buffer).ok();
418430
}
419431

432+
{
433+
let tracking_manager_lock = self.connection_context.tracking_manager.read();
434+
views_params[0].pose =
435+
tracking_manager_lock.server_to_client_pose(views_params[0].pose);
436+
views_params[1].pose =
437+
tracking_manager_lock.server_to_client_pose(views_params[1].pose);
438+
}
439+
420440
if matches!(
421441
sender.try_send(VideoPacket {
422442
header: VideoPacketHeader {
423443
timestamp: target_timestamp,
444+
views_params,
424445
is_idr
425446
},
426447
payload: nal_buffer,

‎alvr/server_core/src/tracking/mod.rs

+52-29
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,8 @@ struct MotionConfig {
5252
}
5353

5454
pub struct TrackingManager {
55-
last_head_pose: Pose, // client's reference space
56-
inverse_recentering_origin: Pose, // client's reference space
55+
last_head_pose: Pose, // client's reference space
56+
client_to_server_recenter_pose: Pose, // client's reference space
5757
device_motions_history: HashMap<u64, VecDeque<(Duration, DeviceMotion)>>,
5858
hand_skeletons_history: [VecDeque<(Duration, [Pose; 26])>; 2],
5959
last_face_data: FaceData,
@@ -64,7 +64,7 @@ impl TrackingManager {
6464
pub fn new(max_history_size: usize) -> TrackingManager {
6565
TrackingManager {
6666
last_head_pose: Pose::default(),
67-
inverse_recentering_origin: Pose::default(),
67+
client_to_server_recenter_pose: Pose::default(),
6868
device_motions_history: HashMap::new(),
6969
hand_skeletons_history: [VecDeque::new(), VecDeque::new()],
7070
last_face_data: FaceData::default(),
@@ -105,19 +105,23 @@ impl TrackingManager {
105105
RotationRecenteringMode::Tilted => self.last_head_pose.orientation,
106106
};
107107

108-
self.inverse_recentering_origin = Pose {
108+
self.client_to_server_recenter_pose = Pose {
109109
position,
110110
orientation,
111111
}
112112
.inverse();
113113
}
114114

115115
pub fn recenter_pose(&self, pose: Pose) -> Pose {
116-
self.inverse_recentering_origin * pose
116+
self.client_to_server_recenter_pose * pose
117+
}
118+
119+
pub fn server_to_client_pose(&self, pose: Pose) -> Pose {
120+
self.client_to_server_recenter_pose.inverse() * pose
117121
}
118122

119123
pub fn recenter_motion(&self, motion: DeviceMotion) -> DeviceMotion {
120-
self.inverse_recentering_origin * motion
124+
self.client_to_server_recenter_pose * motion
121125
}
122126

123127
// Performs all kinds of tracking transformations, driven by settings.
@@ -144,34 +148,41 @@ impl TrackingManager {
144148
let t = controllers.left_controller_position_offset;
145149
let r = controllers.left_controller_rotation_offset;
146150

151+
// NB: we are currently using non-standard order of transforms for the settings (we
152+
// apply first position then orientation, while Pose uses the opposite convention).
153+
// It is converted in here.
154+
let left_orientation = Quat::from_euler(
155+
EulerRot::XYZ,
156+
r[0] * DEG_TO_RAD,
157+
r[1] * DEG_TO_RAD,
158+
r[2] * DEG_TO_RAD,
159+
);
160+
let left_position = Vec3::new(t[0], t[1], t[2]);
147161
device_motion_configs.insert(
148162
*HAND_LEFT_ID,
149163
MotionConfig {
150164
pose_offset: Pose {
151-
orientation: Quat::from_euler(
152-
EulerRot::XYZ,
153-
r[0] * DEG_TO_RAD,
154-
r[1] * DEG_TO_RAD,
155-
r[2] * DEG_TO_RAD,
156-
),
157-
position: Vec3::new(t[0], t[1], t[2]),
165+
orientation: left_orientation,
166+
position: left_orientation * left_position,
158167
},
159168
linear_velocity_cutoff: controllers.linear_velocity_cutoff,
160169
angular_velocity_cutoff: controllers.angular_velocity_cutoff * DEG_TO_RAD,
161170
},
162171
);
163172

173+
let right_orientation = Quat::from_euler(
174+
EulerRot::XYZ,
175+
r[0] * DEG_TO_RAD,
176+
-r[1] * DEG_TO_RAD,
177+
-r[2] * DEG_TO_RAD,
178+
);
179+
let right_position = Vec3::new(-t[0], t[1], t[2]);
164180
device_motion_configs.insert(
165181
*HAND_RIGHT_ID,
166182
MotionConfig {
167183
pose_offset: Pose {
168-
orientation: Quat::from_euler(
169-
EulerRot::XYZ,
170-
r[0] * DEG_TO_RAD,
171-
-r[1] * DEG_TO_RAD,
172-
-r[2] * DEG_TO_RAD,
173-
),
174-
position: Vec3::new(-t[0], t[1], t[2]),
184+
orientation: right_orientation,
185+
position: right_orientation * right_position,
175186
},
176187
linear_velocity_cutoff: controllers.linear_velocity_cutoff,
177188
angular_velocity_cutoff: controllers.angular_velocity_cutoff * DEG_TO_RAD,
@@ -186,18 +197,19 @@ impl TrackingManager {
186197
}
187198

188199
if let Some(config) = device_motion_configs.get(&device_id) {
200+
let original_motion = motion;
201+
189202
// Recenter
190-
motion = self.recenter_motion(motion);
203+
motion = self.recenter_motion(original_motion);
191204

192205
// Apply custom transform
193-
motion.pose.orientation *= config.pose_offset.orientation;
194-
motion.pose.position += motion.pose.orientation * config.pose_offset.position;
195-
196-
motion.linear_velocity += motion
197-
.angular_velocity
198-
.cross(motion.pose.orientation * config.pose_offset.position);
199-
motion.angular_velocity =
200-
motion.pose.orientation.conjugate() * motion.angular_velocity;
206+
motion.pose = motion.pose * config.pose_offset;
207+
// NB: for linear and angular velocity we need to fix it with an extensive refactoring,
208+
// making platform-specific offsets on the client side, checking correctness by drawing
209+
// velocity vectors in the lobby.
210+
// SteamVR requires specific fixes because apparently it uses a different frame of reference
211+
// for prediction, and possibly it applies rotation/translation in a different order.
212+
// This must be fixed before merging the tracking rewrite.
201213

202214
fn cutoff(v: Vec3, threshold: f32) -> Vec3 {
203215
if v.length_squared() > threshold * threshold {
@@ -265,6 +277,17 @@ impl TrackingManager {
265277
})
266278
}
267279

280+
pub fn get_predicted_device_motion(
281+
&self,
282+
device_id: u64,
283+
sample_timestamp: Duration,
284+
target_timestamp: Duration,
285+
) -> Option<DeviceMotion> {
286+
let motion = self.get_device_motion(device_id, sample_timestamp)?;
287+
288+
Some(motion.predict(sample_timestamp, target_timestamp))
289+
}
290+
268291
pub fn report_hand_skeleton(
269292
&mut self,
270293
hand_type: HandType,

‎alvr/server_openvr/src/lib.rs

+76-21
Original file line numberDiff line numberDiff line change
@@ -19,14 +19,15 @@ use alvr_common::{
1919
once_cell::sync::Lazy,
2020
parking_lot::{Mutex, RwLock},
2121
settings_schema::Switch,
22-
warn, BUTTON_INFO, HAND_LEFT_ID, HAND_RIGHT_ID, HAND_TRACKER_LEFT_ID, HAND_TRACKER_RIGHT_ID,
23-
HEAD_ID,
22+
warn, Pose, BUTTON_INFO, HAND_LEFT_ID, HAND_RIGHT_ID, HAND_TRACKER_LEFT_ID,
23+
HAND_TRACKER_RIGHT_ID, HEAD_ID,
2424
};
2525
use alvr_filesystem as afs;
26-
use alvr_packets::{ButtonValue, Haptics};
26+
use alvr_packets::{ButtonValue, Haptics, ViewParams};
2727
use alvr_server_core::{HandType, ServerCoreContext, ServerCoreEvent};
2828
use alvr_session::{CodecType, ControllersConfig};
2929
use std::{
30+
collections::VecDeque,
3031
ffi::{c_char, c_void, CString},
3132
ptr,
3233
sync::{mpsc, Once},
@@ -45,6 +46,12 @@ static SERVER_CORE_CONTEXT: Lazy<RwLock<Option<ServerCoreContext>>> =
4546
static EVENTS_RECEIVER: Lazy<Mutex<Option<mpsc::Receiver<ServerCoreEvent>>>> =
4647
Lazy::new(|| Mutex::new(None));
4748

49+
// local frame of reference
50+
static VIEWS_PARAMS: Lazy<RwLock<[ViewParams; 2]>> =
51+
Lazy::new(|| RwLock::new([ViewParams::default(); 2]));
52+
static HEAD_POSE_QUEUE: Lazy<Mutex<VecDeque<(Duration, Pose)>>> =
53+
Lazy::new(|| Mutex::new(VecDeque::new()));
54+
4855
extern "C" fn driver_ready_idle(set_default_chap: bool) {
4956
thread::spawn(move || {
5057
let events_receiver = EVENTS_RECEIVER.lock().take().unwrap();
@@ -88,25 +95,26 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) {
8895
ServerCoreEvent::PlayspaceSync(bounds) => unsafe {
8996
SetChaperoneArea(bounds.x, bounds.y)
9097
},
91-
ServerCoreEvent::ViewsConfig(config) => unsafe {
98+
ServerCoreEvent::ViewsParams(params) => unsafe {
99+
*VIEWS_PARAMS.write() = params;
100+
92101
SetViewsConfig(FfiViewsConfig {
93102
fov: [
94103
FfiFov {
95-
left: config.fov[0].left,
96-
right: config.fov[0].right,
97-
up: config.fov[0].up,
98-
down: config.fov[0].down,
104+
left: params[0].fov.left,
105+
right: params[0].fov.right,
106+
up: params[0].fov.up,
107+
down: params[0].fov.down,
99108
},
100109
FfiFov {
101-
left: config.fov[1].left,
102-
right: config.fov[1].right,
103-
up: config.fov[1].up,
104-
down: config.fov[1].down,
110+
left: params[1].fov.left,
111+
right: params[1].fov.right,
112+
up: params[1].fov.up,
113+
down: params[1].fov.down,
105114
},
106115
],
107116
// todo: send full matrix to steamvr
108-
ipd_m: config.local_view_transforms[1].position.x
109-
- config.local_view_transforms[0].position.x,
117+
ipd_m: params[1].pose.position.x - params[0].pose.position.x,
110118
});
111119
},
112120
ServerCoreEvent::Tracking { sample_timestamp } => {
@@ -121,18 +129,36 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) {
121129
.unwrap_or(false);
122130

123131
if let Some(context) = &*SERVER_CORE_CONTEXT.read() {
132+
let target_timestamp =
133+
sample_timestamp + context.get_motion_to_photon_latency();
124134
let controllers_pose_time_offset = context.get_tracker_pose_time_offset();
135+
let controllers_timestamp =
136+
target_timestamp.saturating_sub(controllers_pose_time_offset);
137+
138+
let predicted_head_motion = context
139+
.get_predicted_device_motion(
140+
*HEAD_ID,
141+
sample_timestamp,
142+
target_timestamp,
143+
)
144+
.unwrap_or_default();
125145

126-
let ffi_head_motion = context
127-
.get_device_motion(*HEAD_ID, sample_timestamp)
128-
.map(|m| tracking::to_ffi_motion(*HEAD_ID, m))
129-
.unwrap_or_else(FfiDeviceMotion::default);
146+
let ffi_head_motion =
147+
tracking::to_ffi_motion(*HEAD_ID, predicted_head_motion);
130148
let ffi_left_controller_motion = context
131-
.get_device_motion(*HAND_LEFT_ID, sample_timestamp)
149+
.get_predicted_device_motion(
150+
*HAND_LEFT_ID,
151+
sample_timestamp,
152+
controllers_timestamp,
153+
)
132154
.map(|m| tracking::to_ffi_motion(*HAND_LEFT_ID, m))
133155
.filter(|_| tracked);
134156
let ffi_right_controller_motion = context
135-
.get_device_motion(*HAND_RIGHT_ID, sample_timestamp)
157+
.get_predicted_device_motion(
158+
*HAND_RIGHT_ID,
159+
sample_timestamp,
160+
controllers_timestamp,
161+
)
136162
.map(|m| tracking::to_ffi_motion(*HAND_RIGHT_ID, m))
137163
.filter(|_| tracked);
138164

@@ -237,6 +263,17 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) {
237263
ffi_body_tracker_motions.len() as i32,
238264
)
239265
};
266+
267+
{
268+
let mut head_pose_queue_lock = HEAD_POSE_QUEUE.lock();
269+
270+
head_pose_queue_lock
271+
.push_back((sample_timestamp, predicted_head_motion.pose));
272+
273+
if head_pose_queue_lock.len() > 1024 {
274+
head_pose_queue_lock.pop_front();
275+
}
276+
}
240277
}
241278
}
242279
ServerCoreEvent::Buttons(entries) => {
@@ -344,7 +381,25 @@ extern "C" fn set_video_config_nals(buffer_ptr: *const u8, len: i32, codec: i32)
344381
extern "C" fn send_video(timestamp_ns: u64, buffer_ptr: *mut u8, len: i32, is_idr: bool) {
345382
if let Some(context) = &*SERVER_CORE_CONTEXT.read() {
346383
let buffer = unsafe { std::slice::from_raw_parts(buffer_ptr, len as usize) };
347-
context.send_video_nal(Duration::from_nanos(timestamp_ns), buffer.to_vec(), is_idr);
384+
385+
let mut head_pose = Pose::default();
386+
for (timestamp, pose) in &*HEAD_POSE_QUEUE.lock() {
387+
if *timestamp == Duration::from_nanos(timestamp_ns) {
388+
head_pose = *pose;
389+
break;
390+
}
391+
}
392+
393+
let mut view_params = *VIEWS_PARAMS.read();
394+
view_params[0].pose = head_pose * view_params[0].pose;
395+
view_params[1].pose = head_pose * view_params[1].pose;
396+
397+
context.send_video_nal(
398+
Duration::from_nanos(timestamp_ns),
399+
view_params,
400+
is_idr,
401+
buffer.to_vec(),
402+
);
348403
}
349404
}
350405

0 commit comments

Comments
 (0)
Please sign in to comment.