oxihuman_viewer/
motion_sample.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Motion sample ring buffer for temporal effects.
6
7use std::f32::consts::FRAC_PI_4;
8
9pub const MOTION_SAMPLE_CAPACITY: usize = 16;
10
11#[allow(dead_code)]
12#[derive(Debug, Clone, Copy)]
13pub struct MotionSampleEntry {
14    pub velocity: [f32; 2],
15    pub timestamp: f32,
16}
17
18#[allow(dead_code)]
19#[derive(Debug, Clone)]
20pub struct MotionSampleBuffer {
21    pub entries: [Option<MotionSampleEntry>; MOTION_SAMPLE_CAPACITY],
22    pub head: usize,
23    pub len: usize,
24}
25
26#[allow(dead_code)]
27pub fn new_motion_sample_buffer() -> MotionSampleBuffer {
28    MotionSampleBuffer {
29        entries: [None; MOTION_SAMPLE_CAPACITY],
30        head: 0,
31        len: 0,
32    }
33}
34
35#[allow(dead_code)]
36pub fn msb_push(buf: &mut MotionSampleBuffer, velocity: [f32; 2], timestamp: f32) {
37    let idx = buf.head % MOTION_SAMPLE_CAPACITY;
38    buf.entries[idx] = Some(MotionSampleEntry {
39        velocity,
40        timestamp,
41    });
42    buf.head = (buf.head + 1) % MOTION_SAMPLE_CAPACITY;
43    if buf.len < MOTION_SAMPLE_CAPACITY {
44        buf.len += 1;
45    }
46}
47
48#[allow(dead_code)]
49pub fn msb_clear(buf: &mut MotionSampleBuffer) {
50    buf.entries = [None; MOTION_SAMPLE_CAPACITY];
51    buf.head = 0;
52    buf.len = 0;
53}
54
55#[allow(dead_code)]
56pub fn msb_count(buf: &MotionSampleBuffer) -> usize {
57    buf.len
58}
59
60#[allow(dead_code)]
61pub fn msb_is_empty(buf: &MotionSampleBuffer) -> bool {
62    buf.len == 0
63}
64
65#[allow(dead_code)]
66pub fn msb_average_velocity(buf: &MotionSampleBuffer) -> [f32; 2] {
67    if buf.len == 0 {
68        return [0.0, 0.0];
69    }
70    let mut sx = 0.0f32;
71    let mut sy = 0.0f32;
72    for e in buf.entries.iter().flatten() {
73        sx += e.velocity[0];
74        sy += e.velocity[1];
75    }
76    [sx / buf.len as f32, sy / buf.len as f32]
77}
78
79#[allow(dead_code)]
80pub fn msb_peak_speed(buf: &MotionSampleBuffer) -> f32 {
81    buf.entries
82        .iter()
83        .flatten()
84        .map(|e| (e.velocity[0] * e.velocity[0] + e.velocity[1] * e.velocity[1]).sqrt())
85        .fold(0.0f32, f32::max)
86}
87
88#[allow(dead_code)]
89pub fn msb_speed_angle_rad(buf: &MotionSampleBuffer) -> f32 {
90    let s = msb_peak_speed(buf);
91    if s > 0.0 {
92        (1.0 / s).atan().min(FRAC_PI_4)
93    } else {
94        0.0
95    }
96}
97
98#[allow(dead_code)]
99pub fn msb_to_json(buf: &MotionSampleBuffer) -> String {
100    let avg = msb_average_velocity(buf);
101    format!(
102        "{{\"count\":{},\"avg_vel\":[{:.4},{:.4}]}}",
103        msb_count(buf),
104        avg[0],
105        avg[1]
106    )
107}
108
109#[cfg(test)]
110mod tests {
111    use super::*;
112    #[test]
113    fn new_is_empty() {
114        assert!(msb_is_empty(&new_motion_sample_buffer()));
115    }
116    #[test]
117    fn push_increments_count() {
118        let mut b = new_motion_sample_buffer();
119        msb_push(&mut b, [1.0, 0.0], 0.0);
120        assert_eq!(msb_count(&b), 1);
121    }
122    #[test]
123    fn clear_empties() {
124        let mut b = new_motion_sample_buffer();
125        msb_push(&mut b, [1.0, 0.0], 0.0);
126        msb_clear(&mut b);
127        assert!(msb_is_empty(&b));
128    }
129    #[test]
130    fn wraps_at_capacity() {
131        let mut b = new_motion_sample_buffer();
132        for i in 0..MOTION_SAMPLE_CAPACITY + 2 {
133            msb_push(&mut b, [i as f32, 0.0], i as f32);
134        }
135        assert_eq!(msb_count(&b), MOTION_SAMPLE_CAPACITY);
136    }
137    #[test]
138    fn average_velocity_empty_zero() {
139        let b = new_motion_sample_buffer();
140        let avg = msb_average_velocity(&b);
141        assert!(avg[0].abs() < 1e-6 && avg[1].abs() < 1e-6);
142    }
143    #[test]
144    fn average_velocity_one_entry() {
145        let mut b = new_motion_sample_buffer();
146        msb_push(&mut b, [4.0, 2.0], 0.0);
147        let avg = msb_average_velocity(&b);
148        assert!((avg[0] - 4.0).abs() < 1e-5);
149    }
150    #[test]
151    fn peak_speed_correct() {
152        let mut b = new_motion_sample_buffer();
153        msb_push(&mut b, [3.0, 4.0], 0.0);
154        assert!((msb_peak_speed(&b) - 5.0).abs() < 1e-4);
155    }
156    #[test]
157    fn speed_angle_nonneg() {
158        assert!(msb_speed_angle_rad(&new_motion_sample_buffer()) >= 0.0);
159    }
160    #[test]
161    fn to_json_has_count() {
162        assert!(msb_to_json(&new_motion_sample_buffer()).contains("\"count\""));
163    }
164    #[test]
165    fn capacity_is_sixteen() {
166        assert_eq!(MOTION_SAMPLE_CAPACITY, 16);
167    }
168}
oxihuman_viewer/motion_sample.rs

oxihuman_viewer/
motion_sample.rs
