zendo_protocol/
encode.rs

1//! Pure encoding of typed frames back into binary form.
2//!
3//! These mirror [`decode`](crate::decode()) exactly and are used to build test
4//! fixtures and round-trip tests. They never allocate: each returns a
5//! fixed-size array.
6
7use crate::constants::{
8    BODY_ISB_ANGLE_COUNT, BODY_JOINT_COUNT, BODY_LANDMARK_COUNT, F64_BYTES, HAND_JOINT_COUNT,
9    HAND_LANDMARK_COUNT, MSG_BODY_ISB_ANGLES, MSG_BODY_LANDMARK, MSG_BODY_QUATERNION,
10    MSG_HAND_LANDMARK, MSG_HAND_QUATERNION, MSG_HELLO, TUPLE_BYTES,
11};
12use crate::frames::{
13    BodyIsbAnglesFrame, BodyLandmarkFrame, BodyQuaternionFrame, HandLandmarkFrame,
14    HandQuaternionFrame,
15};
16use crate::types::{HandSide, Landmark, Quaternion};
17
18/// Encoded size of a body-quaternion frame, in bytes.
19pub const ENCODED_BODY_QUATERNION_LEN: usize = 1 + BODY_JOINT_COUNT * TUPLE_BYTES;
20
21/// Encoded size of a body-landmark frame, in bytes.
22pub const ENCODED_BODY_LANDMARK_LEN: usize = 1 + BODY_LANDMARK_COUNT * TUPLE_BYTES;
23
24/// Encoded size of a hand-quaternion frame, in bytes.
25pub const ENCODED_HAND_QUATERNION_LEN: usize = 2 + HAND_JOINT_COUNT * TUPLE_BYTES;
26
27/// Encoded size of a hand-landmark frame, in bytes.
28pub const ENCODED_HAND_LANDMARK_LEN: usize = 2 + HAND_LANDMARK_COUNT * TUPLE_BYTES;
29
30/// Encoded size of a body ISB-angles frame, in bytes.
31pub const ENCODED_BODY_ISB_ANGLES_LEN: usize = 1 + BODY_ISB_ANGLE_COUNT * F64_BYTES;
32
33/// Encoded size of a hello frame, in bytes (tag + `u16` version).
34pub const ENCODED_HELLO_LEN: usize = 1 + 2;
35
36/// Encodes a body-quaternion frame (`0x02`).
37pub fn encode_body_quaternions(frame: &BodyQuaternionFrame) -> [u8; ENCODED_BODY_QUATERNION_LEN] {
38    let mut out = [0u8; ENCODED_BODY_QUATERNION_LEN];
39    out[0] = MSG_BODY_QUATERNION;
40    write_quaternions(&mut out[1..], &frame.to_array());
41    out
42}
43
44/// Encodes a body-landmark frame (`0x03`).
45pub fn encode_body_landmarks(frame: &BodyLandmarkFrame) -> [u8; ENCODED_BODY_LANDMARK_LEN] {
46    let mut out = [0u8; ENCODED_BODY_LANDMARK_LEN];
47    out[0] = MSG_BODY_LANDMARK;
48    write_landmarks(&mut out[1..], &frame.to_array());
49    out
50}
51
52/// Encodes a hand-quaternion frame (`0x04`).
53pub fn encode_hand_quaternions(
54    side: HandSide,
55    frame: &HandQuaternionFrame,
56) -> [u8; ENCODED_HAND_QUATERNION_LEN] {
57    let mut out = [0u8; ENCODED_HAND_QUATERNION_LEN];
58    out[0] = MSG_HAND_QUATERNION;
59    out[1] = side.as_byte();
60    write_quaternions(&mut out[2..], &frame.to_array());
61    out
62}
63
64/// Encodes a hand-landmark frame (`0x05`).
65pub fn encode_hand_landmarks(
66    side: HandSide,
67    frame: &HandLandmarkFrame,
68) -> [u8; ENCODED_HAND_LANDMARK_LEN] {
69    let mut out = [0u8; ENCODED_HAND_LANDMARK_LEN];
70    out[0] = MSG_HAND_LANDMARK;
71    out[1] = side.as_byte();
72    write_landmarks(&mut out[2..], &frame.to_array());
73    out
74}
75
76/// Encodes a body ISB-angles frame (`0x06`).
77pub fn encode_body_isb_angles(frame: &BodyIsbAnglesFrame) -> [u8; ENCODED_BODY_ISB_ANGLES_LEN] {
78    let mut out = [0u8; ENCODED_BODY_ISB_ANGLES_LEN];
79    out[0] = MSG_BODY_ISB_ANGLES;
80    for (i, &val) in frame.to_array().iter().enumerate() {
81        write_f64(&mut out[1..], i * F64_BYTES, val);
82    }
83    out
84}
85
86/// Encodes a hello frame (`0x01`) announcing the protocol version.
87pub fn encode_hello(protocol_version: u16) -> [u8; ENCODED_HELLO_LEN] {
88    let mut out = [0u8; ENCODED_HELLO_LEN];
89    out[0] = MSG_HELLO;
90    out[1..].copy_from_slice(&protocol_version.to_le_bytes());
91    out
92}
93
94fn write_quaternions(dst: &mut [u8], quats: &[Quaternion]) {
95    for (i, q) in quats.iter().enumerate() {
96        let base = i * TUPLE_BYTES;
97        write_f64(dst, base, q.w);
98        write_f64(dst, base + F64_BYTES, q.x);
99        write_f64(dst, base + 2 * F64_BYTES, q.y);
100        write_f64(dst, base + 3 * F64_BYTES, q.z);
101    }
102}
103
104fn write_landmarks(dst: &mut [u8], landmarks: &[Landmark]) {
105    for (i, lm) in landmarks.iter().enumerate() {
106        let base = i * TUPLE_BYTES;
107        write_f64(dst, base, lm.x);
108        write_f64(dst, base + F64_BYTES, lm.y);
109        write_f64(dst, base + 2 * F64_BYTES, lm.z);
110        write_f64(dst, base + 3 * F64_BYTES, lm.confidence);
111    }
112}
113
114fn write_f64(dst: &mut [u8], offset: usize, value: f64) {
115    dst[offset..offset + F64_BYTES].copy_from_slice(&value.to_le_bytes());
116}
117
118#[cfg(test)]
119mod tests {
120    use super::*;
121    use crate::constants::BODY_ISB_ANGLE_COUNT;
122    use crate::decode::{decode, decode_hello, Message};
123
124    fn sample_quaternion_frame() -> BodyQuaternionFrame {
125        let mut quats = [Quaternion::default(); BODY_JOINT_COUNT];
126        for (i, q) in quats.iter_mut().enumerate() {
127            let f = i as f64;
128            *q = Quaternion {
129                w: f,
130                x: f + 0.1,
131                y: f + 0.2,
132                z: f + 0.3,
133            };
134        }
135        BodyQuaternionFrame::from_array(quats)
136    }
137
138    fn sample_landmark_frame() -> HandLandmarkFrame {
139        let mut landmarks = [Landmark::default(); HAND_LANDMARK_COUNT];
140        for (i, lm) in landmarks.iter_mut().enumerate() {
141            let f = i as f64;
142            *lm = Landmark {
143                x: f,
144                y: -f,
145                z: f * 2.0,
146                confidence: 0.5,
147            };
148        }
149        HandLandmarkFrame::from_array(landmarks)
150    }
151
152    #[test]
153    fn body_quaternions_round_trip() {
154        // Arrange
155        let frame = sample_quaternion_frame();
156
157        // Act
158        let encoded = encode_body_quaternions(&frame);
159        let decoded = decode(&encoded).expect("round-trips");
160
161        // Assert
162        assert_eq!(decoded, Message::BodyQuaternions(frame));
163    }
164
165    #[test]
166    fn hand_landmarks_round_trip() {
167        // Arrange
168        let frame = sample_landmark_frame();
169
170        // Act
171        let encoded = encode_hand_landmarks(HandSide::Left, &frame);
172        let decoded = decode(&encoded).expect("round-trips");
173
174        // Assert
175        assert_eq!(
176            decoded,
177            Message::HandLandmarks {
178                side: HandSide::Left,
179                frame
180            }
181        );
182    }
183
184    #[test]
185    fn hello_round_trips() {
186        // Arrange / Act
187        let encoded = encode_hello(7);
188        let version = decode_hello(&encoded).expect("round-trips");
189
190        // Assert
191        assert_eq!(version, 7);
192    }
193
194    #[test]
195    fn body_isb_angles_round_trip() {
196        // Arrange
197        let mut values = [0.0f64; BODY_ISB_ANGLE_COUNT];
198        for (i, v) in values.iter_mut().enumerate() {
199            *v = i as f64 * 0.1;
200        }
201        let frame = BodyIsbAnglesFrame::from_array(values);
202
203        // Act
204        let encoded = encode_body_isb_angles(&frame);
205        let decoded = decode(&encoded).expect("round-trips");
206
207        // Assert
208        assert_eq!(decoded, Message::BodyIsbAngles(frame));
209    }
210
211    #[test]
212    fn body_isb_angles_preserves_nan() {
213        // Arrange — shoulder plane_of_elevation carries NaN on singularity
214        let mut values = [0.0f64; BODY_ISB_ANGLE_COUNT];
215        values[5] = f64::NAN; // right_shoulder_plane_of_elevation
216        values[7] = f64::NAN; // left_shoulder_plane_of_elevation
217        let frame = BodyIsbAnglesFrame::from_array(values);
218
219        // Act
220        let encoded = encode_body_isb_angles(&frame);
221        let decoded = decode(&encoded).expect("round-trips even with NaN");
222
223        // Assert
224        match decoded {
225            Message::BodyIsbAngles(f) => {
226                assert!(f.right_shoulder_plane_of_elevation.is_nan());
227                assert!(f.left_shoulder_plane_of_elevation.is_nan());
228                assert_eq!(f.thorax_lateral_bend, 0.0);
229            }
230            other => panic!("expected BodyIsbAngles, got {other:?}"),
231        }
232    }
233
234    #[test]
235    fn encoded_lengths_are_correct() {
236        // Arrange / Act / Assert
237        assert_eq!(ENCODED_BODY_QUATERNION_LEN, 417);
238        assert_eq!(ENCODED_BODY_LANDMARK_LEN, 609);
239        assert_eq!(ENCODED_HAND_QUATERNION_LEN, 514);
240        assert_eq!(ENCODED_HAND_LANDMARK_LEN, 674);
241        assert_eq!(ENCODED_HELLO_LEN, 3);
242        assert_eq!(ENCODED_BODY_ISB_ANGLES_LEN, 169);
243    }
244}
zendo_protocol/encode.rs

zendo_protocol/
encode.rs
