sim_lib_view_spatial/
pose_view.rs1#[derive(Clone, Debug, PartialEq)]
9pub struct PoseView {
10 pub sample_seq: u64,
12 pub age_ms: u64,
14 pub predict_ns: u64,
16 pub translation_m: [f64; 3],
18 pub yaw_deg: f64,
20 pub pitch_deg: f64,
22 pub roll_deg: f64,
24 pub inter_eye_m: f64,
26}
27
28impl Default for PoseView {
29 fn default() -> Self {
30 Self::identity(0)
31 }
32}
33
34impl PoseView {
35 pub fn identity(sample_seq: u64) -> Self {
37 Self {
38 sample_seq,
39 age_ms: 0,
40 predict_ns: 0,
41 translation_m: [0.0, 0.0, 0.0],
42 yaw_deg: 0.0,
43 pitch_deg: 0.0,
44 roll_deg: 0.0,
45 inter_eye_m: 0.064,
46 }
47 }
48
49 pub fn with_timing(mut self, age_ms: u64, predict_ns: u64) -> Self {
51 self.age_ms = age_ms;
52 self.predict_ns = predict_ns;
53 self
54 }
55
56 pub fn with_translation(mut self, translation_m: [f64; 3]) -> Self {
58 self.translation_m = translation_m;
59 self
60 }
61
62 pub fn with_angles(mut self, yaw_deg: f64, pitch_deg: f64, roll_deg: f64) -> Self {
64 self.yaw_deg = yaw_deg;
65 self.pitch_deg = pitch_deg;
66 self.roll_deg = roll_deg;
67 self
68 }
69
70 pub fn clamped_predict_ms(&self, max_predict_ms: u64) -> u64 {
72 (self.predict_ns / 1_000_000).min(max_predict_ms)
73 }
74
75 pub(crate) fn clamped_yaw_rad(&self, max_predict_ms: u64) -> f64 {
76 let requested_ms = self.predict_ns as f64 / 1_000_000.0;
77 let scale = if requested_ms <= f64::EPSILON {
78 0.0
79 } else {
80 self.clamped_predict_ms(max_predict_ms) as f64 / requested_ms
81 };
82 self.yaw_deg.to_radians() * scale
83 }
84}