repame_shell/
motion.rs

1use std::collections::HashMap;
2
3use glam::Vec3;
4use repame_input::{MotionSample, MotionSource, SensorKind};
5use repose_core::input::SensorKind as ReposeSensorKind;
6use repose_core::input::{GamepadEvent, GamepadId};
7use repose_platform::sensor::{SensorBackend, SensorReading, create_backend};
8
9const DEG_TO_RAD: f32 = std::f32::consts::PI / 180.0;
10const G_TO_MS2: f32 = 9.806_65;
11
12/// One pad's motion, ready for [`MotionTracker`](repame_input::MotionTracker).
13/// Holds its own copy of the reading so it stays a plain value the sim can
14/// carry between frames.
15#[derive(Clone, Copy, Debug, Default, PartialEq)]
16pub struct PadMotion {
17    kind: SensorKind,
18    sample: MotionSample,
19    reported: bool,
20}
21
22impl PadMotion {
23    /// Whether this pad has ever reported motion.
24    pub fn reported(&self) -> bool {
25        self.reported
26    }
27}
28
29impl MotionSource for PadMotion {
30    fn kind(&self) -> SensorKind {
31        self.kind
32    }
33
34    fn sample(&self) -> Option<MotionSample> {
35        self.reported.then_some(self.sample)
36    }
37}
38
39/// Motion-sensor polling on top of the shared platform backend. Sensor nodes
40/// are separate evdev nodes from the pad, so readings are routed back to pads
41/// by name; feed [`MotionPoller::feed_gamepad`] the same events
42/// [`GamepadPoller`](crate::GamepadPoller) drains.
43pub struct MotionPoller {
44    backend: Option<Box<dyn SensorBackend>>,
45    pads: HashMap<u32, String>,
46    gyroscope: HashMap<u32, [f32; 3]>,
47    accelerometer: HashMap<u32, [f32; 3]>,
48}
49
50impl MotionPoller {
51    pub fn new() -> Self {
52        Self {
53            backend: create_backend().map(|b| Box::new(b) as Box<dyn SensorBackend>),
54            pads: HashMap::new(),
55            gyroscope: HashMap::new(),
56            accelerometer: HashMap::new(),
57        }
58    }
59
60    /// Track pad names so sensor devices can be routed to them.
61    pub fn feed_gamepad(&mut self, events: &[GamepadEvent]) {
62        for event in events {
63            match event {
64                GamepadEvent::Connected { id, name, .. } => {
65                    self.pads.insert(id.0, name.clone());
66                }
67                GamepadEvent::Disconnected { id } => {
68                    self.pads.remove(&id.0);
69                    self.gyroscope.remove(&id.0);
70                    self.accelerometer.remove(&id.0);
71                }
72                _ => {}
73            }
74        }
75    }
76
77    /// Drain sensor readings since the last call. Devices with no matching
78    /// pad are dropped rather than invented.
79    pub fn poll(&mut self) {
80        let Some(backend) = &mut self.backend else {
81            return;
82        };
83        let readings = backend.poll_sensors();
84        self.feed_readings(readings);
85    }
86
87    /// Route readings to pads by device name, latest wins.
88    pub fn feed_readings(&mut self, readings: Vec<SensorReading>) {
89        for reading in readings {
90            let Some(id) = self.resolve(&reading.device) else {
91                log::debug!(
92                    "motion: no pad named {:?}, dropping {:?}",
93                    reading.device,
94                    reading.sample.kind
95                );
96                continue;
97            };
98            match reading.sample.kind {
99                ReposeSensorKind::Gyroscope => {
100                    self.gyroscope.insert(id, reading.sample.data);
101                }
102                ReposeSensorKind::Accelerometer => {
103                    self.accelerometer.insert(id, reading.sample.data);
104                }
105            }
106        }
107    }
108
109    /// Current motion for `id`, in repame's units: angular velocity in
110    /// radians per second and acceleration in m/s^2, both device-frame.
111    pub fn source(&self, id: GamepadId) -> PadMotion {
112        let gyroscope = self.gyroscope.get(&id.0).copied();
113        let accelerometer = self.accelerometer.get(&id.0).copied();
114        let kind = match (gyroscope.is_some(), accelerometer.is_some()) {
115            (true, true) => SensorKind::GyroscopeAccelerometer,
116            (true, false) => SensorKind::Gyroscope,
117            (false, true) => SensorKind::Accelerometer,
118            (false, false) => SensorKind::None,
119        };
120        let angular_velocity = gyroscope
121            .map(|d| Vec3::new(d[0], d[1], d[2]) * DEG_TO_RAD)
122            .unwrap_or(Vec3::ZERO);
123        let acceleration = accelerometer
124            .map(|d| Vec3::new(d[0], d[1], d[2]) * G_TO_MS2)
125            .unwrap_or(Vec3::ZERO);
126        PadMotion {
127            kind,
128            sample: MotionSample {
129                acceleration,
130                angular_velocity,
131            },
132            reported: kind != SensorKind::None,
133        }
134    }
135
136    /// Pads that have reported motion, id-sorted.
137    pub fn motion_ids(&self) -> Vec<GamepadId> {
138        let mut ids: Vec<u32> = self
139            .gyroscope
140            .keys()
141            .chain(self.accelerometer.keys())
142            .copied()
143            .collect();
144        ids.sort_unstable();
145        ids.dedup();
146        ids.into_iter().map(GamepadId).collect()
147    }
148
149    fn resolve(&self, device: &str) -> Option<u32> {
150        if let Some((id, _)) = self.pads.iter().find(|(_, name)| name.as_str() == device) {
151            return Some(*id);
152        }
153        let needle = repose_core::input::normalize_device_name(device);
154        let mut matches = self
155            .pads
156            .iter()
157            .filter(|(_, name)| repose_core::input::normalize_device_name(name) == needle)
158            .map(|(id, _)| *id);
159        let first = matches.next()?;
160        matches.next().is_none().then_some(first)
161    }
162}
163
164impl Default for MotionPoller {
165    fn default() -> Self {
166        Self::new()
167    }
168}
repame_shell/motion.rs

repame_shell/
motion.rs
