qualia_vision/biosense/pose/
pack_pose_landmarks.rs1use crate::cv::error::CvError;
4
5pub const MAX_POSE_LANDMARKS: usize = 33;
7
8#[derive(Debug, Clone, Copy, Default, PartialEq)]
9pub struct PoseLandmark {
10 pub x: f32,
11 pub y: f32,
12 pub visibility: f32,
13}
14
15pub fn pack_pose_landmarks_xy(
17 xy: &[f32],
18 has_z: bool,
19 vis: Option<&[f32]>,
20 count: usize,
21 out: &mut [PoseLandmark],
22) -> Result<usize, CvError> {
23 let n = count.min(MAX_POSE_LANDMARKS).min(out.len());
24 let stride = if has_z { 3 } else { 2 };
25 if xy.len() < n * stride {
26 return Err(CvError::BufferTooSmall);
27 }
28 for i in 0..n {
29 let base = i * stride;
30 out[i] = PoseLandmark {
31 x: xy[base],
32 y: xy[base + 1],
33 visibility: vis.map(|v| v.get(i).copied().unwrap_or(1.0)).unwrap_or(1.0),
34 };
35 }
36 Ok(n)
37}
38
39#[cfg(test)]
40mod tests {
41 use super::*;
42
43 #[test]
44 fn packs_xy_ignores_z() {
45 let mut xy = [0.0f32; 33 * 3];
46 xy[0] = 0.5;
47 xy[1] = 0.25;
48 xy[2] = 9.9; let mut out = [PoseLandmark::default(); MAX_POSE_LANDMARKS];
50 let n = pack_pose_landmarks_xy(&xy, true, None, 33, &mut out).unwrap();
51 assert_eq!(n, 33);
52 assert!((out[0].x - 0.5).abs() < 1e-6);
53 assert!((out[0].y - 0.25).abs() < 1e-6);
54 }
55}