mocopi_parser/
lib.rs

1use nom::bytes::complete::take;
2use nom::error::Error;
3use nom::number::complete::le_u32;
4use serde::{Deserialize, Serialize};
5use std::error;
6
7pub type BoneId = u16;
8pub type TransVal = f32;
9
10#[derive(Debug, PartialEq, Serialize, Deserialize)]
11pub struct SkeletonPacket {
12    pub head: Head,
13    pub info: Info,
14    pub skeleton: Skeleton,
15}
16
17#[derive(Debug, PartialEq, Serialize, Deserialize)]
18pub struct Head {
19    pub format: String,
20    pub ver: u8,
21}
22
23#[derive(Debug, PartialEq, Serialize, Deserialize)]
24pub struct Info {
25    pub addr: u64,
26    pub port: u16,
27}
28
29#[derive(Debug, PartialEq, Serialize, Deserialize)]
30pub struct Skeleton {
31    pub bones: Vec<Bone>,
32}
33
34#[derive(Debug, PartialEq, Serialize, Deserialize)]
35pub struct Bone {
36    pub id: BoneId,
37    pub parent: BoneId,
38    pub trans: Transform,
39}
40
41#[derive(Debug, PartialEq, Serialize, Deserialize)]
42pub struct FramePacket {
43    pub head: Head,
44    pub info: Info,
45    pub frame: Frame,
46}
47
48#[derive(Debug, PartialEq, Serialize, Deserialize)]
49pub struct Frame {
50    pub num: u32,
51    pub time: u32,
52    pub bones: Vec<BoneTrans>,
53}
54
55#[derive(Debug, PartialEq, Serialize, Deserialize)]
56pub struct BoneTrans {
57    pub id: BoneId,
58    pub trans: Transform,
59}
60
61#[derive(Debug, PartialEq, Serialize, Deserialize)]
62pub struct Transform {
63    pub rot: Rotation,
64    pub pos: Position,
65}
66
67#[derive(Debug, PartialEq, Serialize, Deserialize)]
68pub struct Rotation {
69    pub x: TransVal,
70    pub y: TransVal,
71    pub z: TransVal,
72    pub w: TransVal,
73}
74
75#[derive(Debug, PartialEq, Serialize, Deserialize)]
76pub struct Position {
77    pub x: TransVal,
78    pub y: TransVal,
79    pub z: TransVal,
80}
81
82#[derive(Debug, PartialEq, Serialize, Deserialize)]
83pub struct Data<'a> {
84    pub len: u32,
85    pub name: String,
86    pub data: &'a [u8],
87    pub rem: &'a [u8],
88}
89
90pub enum SkeletonOrFrame {
91    Skeleton(SkeletonPacket),
92    Frame(FramePacket),
93}
94
95/// Parse the values.
96fn parse_value(data: &[u8]) -> Result<Data, Box<dyn error::Error + '_>> {
97    // lengthの長さは4bytesで固定
98    let (data, length) = le_u32::<_, Error<_>>(data)? as (&[u8], u32);
99
100    // nameは4bytesの文字列
101    let (data, name) = take::<_, _, Error<_>>(4usize)(data)?;
102    let name_str = String::from_utf8(name.to_vec())?;
103
104    // valueの長さはlengthの値による
105    let (rem, data) = take::<_, _, Error<_>>(length)(data)?;
106
107    Ok(Data {
108        len: length,
109        name: name_str,
110        data,
111        rem,
112    })
113}
114
115fn parse_head(data: &[u8]) -> Result<(u32, Head), Box<dyn error::Error + '_>> {
116    let data = parse_value(data)?;
117    let len = data.len;
118
119    // ftyp
120    let data = parse_value(data.data)?;
121    let format = String::from_utf8(data.data.to_vec())?;
122
123    // vrsn
124    let data = parse_value(data.rem)?;
125    let ver = data.data[0];
126
127    Ok((len, Head { format, ver }))
128}
129
130fn parse_info(data: &[u8]) -> Result<(u32, Info), Box<dyn error::Error + '_>> {
131    let data = parse_value(data)?;
132    let len = data.len;
133
134    // ipad
135    let data = parse_value(data.data)?;
136    let addr = u64::from_le_bytes(data.data.try_into()?);
137
138    // rcvp
139    let data = parse_value(data.rem)?;
140    let port = u16::from_le_bytes(data.data.try_into()?);
141
142    Ok((len, Info { addr, port }))
143}
144
145fn parse_skeleton(data: &[u8]) -> Result<(u32, Skeleton), Box<dyn error::Error + '_>> {
146    // skdf
147    let data = parse_value(data)?;
148    let len = data.len;
149
150    // bons
151    let (_, bones) = parse_bones(data.data)?;
152
153    Ok((len, Skeleton { bones: *bones }))
154}
155
156fn parse_frame(data: &[u8]) -> Result<(u32, Frame), Box<dyn error::Error + '_>> {
157    // fram
158    let data = parse_value(data)?;
159    let len = data.len;
160
161    // fnum
162    let data = parse_value(data.data)?;
163    let num = u32::from_le_bytes(data.data.try_into()?);
164
165    // time
166    let data = parse_value(data.rem)?;
167    let time = u32::from_le_bytes(data.data.try_into()?);
168
169    // btrs
170    let (_, bones) = parse_bone_trans(data.rem)?;
171
172    Ok((
173        len,
174        Frame {
175            num,
176            time,
177            bones: *bones,
178        },
179    ))
180}
181
182fn parse_bone_trans(data: &[u8]) -> Result<(u32, Box<Vec<BoneTrans>>), Box<dyn error::Error + '_>> {
183    // btrs
184    let btrs_data = parse_value(data)?;
185    let btrs_len = btrs_data.len;
186
187    // btrsの下にあるbtdtをparseしていく
188    let mut bones: Vec<BoneTrans> = Vec::new();
189    let mut read_bytes: u32 = 0;
190    loop {
191        let part = &btrs_data.data[(read_bytes as usize)..];
192
193        // btdt
194        let data = parse_value(part)?;
195        let len = data.len;
196
197        // bnid
198        let data = parse_value(data.data)?;
199        let id = u16::from_le_bytes(data.data.try_into()?);
200
201        // tran
202        let (_, trans) = parse_trans(data.rem)?;
203
204        bones.push(BoneTrans { id, trans });
205
206        read_bytes += len + 8;
207        if read_bytes == btrs_len {
208            break;
209        }
210    }
211
212    Ok((btrs_len, Box::new(bones)))
213}
214
215fn parse_bones(data: &[u8]) -> Result<(u32, Box<Vec<Bone>>), Box<dyn error::Error + '_>> {
216    // bons
217    let bons_data = parse_value(data)?;
218    let bons_len = bons_data.len;
219
220    // bonsの下にあるbndtをparseしていく
221    let mut bones: Vec<Bone> = Vec::new();
222    let mut read_bytes: u32 = 0;
223    loop {
224        let part = &bons_data.data[(read_bytes as usize)..];
225
226        // bndt
227        let data = parse_value(part)?;
228        let len = data.len;
229
230        // bnid
231        let data = parse_value(data.data)?;
232        let id = u16::from_le_bytes(data.data.try_into()?);
233
234        // pbid
235        let data = parse_value(data.rem)?;
236        let parent = u16::from_le_bytes(data.data.try_into()?);
237
238        // tran
239        let (_, trans) = parse_trans(part)?;
240
241        bones.push(Bone { id, parent, trans });
242
243        read_bytes += len + 8;
244        if read_bytes == bons_len {
245            break;
246        }
247    }
248
249    Ok((bons_len, Box::new(bones)))
250}
251
252fn parse_trans(data: &[u8]) -> Result<(u32, Transform), Box<dyn error::Error + '_>> {
253    // tran
254    let data = parse_value(data)?;
255
256    // 28bytesのデータを4bytesごとに取り出す
257    let mut values = [0.0; 7];
258    for (i, v) in values.iter_mut().enumerate() {
259        let b = &data.data[i * 4..(i * 4 + 4)];
260        *v = f32::from_le_bytes(b.try_into()?);
261    }
262
263    Ok((
264        data.len,
265        Transform {
266            rot: Rotation {
267                x: values[0],
268                y: values[1],
269                z: values[2],
270                w: values[3],
271            },
272            pos: Position {
273                x: values[4],
274                y: values[5],
275                z: values[6],
276            },
277        },
278    ))
279}
280
281/// Parse the streamed data from mocopi.
282///
283/// # Examples
284///
285/// ```
286/// use std::net::UdpSocket;
287///
288/// let socket = UdpSocket::bind("192.168.10.1:12351").unwrap();
289/// let mut buf = [0; 1024];
290///
291/// loop {
292///     socket.recv_from(&mut buf).unwrap();
293///     let packet = mocopi_parser::parse(&mut buf).unwrap();
294///
295///     match packet {
296///         mocopi_parser::SkeletonOrFrame::Skeleton(skeleton) => { dbg!(skeleton); },
297///         mocopi_parser::SkeletonOrFrame::Frame(frame) => { dbg!(frame); },
298///     }
299/// }
300/// ```
301pub fn parse(data: &mut [u8]) -> Result<SkeletonOrFrame, Box<dyn error::Error + '_>> {
302    let (len, head) = parse_head(data)?;
303    let mut remain = &data[((len + 8) as usize)..];
304
305    let (len, info) = parse_info(remain)?;
306    remain = &remain[((len + 8) as usize)..];
307
308    let name = parse_value(data)?.name;
309
310    if name == "skdf" {
311        let (_, skeleton) = parse_skeleton(remain)?;
312        Ok(SkeletonOrFrame::Skeleton(SkeletonPacket {
313            head,
314            info,
315            skeleton,
316        }))
317    } else {
318        let (_, frame) = parse_frame(remain)?;
319        Ok(SkeletonOrFrame::Frame(FramePacket { head, info, frame }))
320    }
321}
322
323#[cfg(test)]
324mod tests {
325    use super::*;
326
327    #[test]
328    fn test_parse_value() {
329        let raw = [
330            0x04, 0x00, 0x00, 0x00,
331            0x62, 0x6e, 0x64, 0x74,
332            0x02, 0x00, 0x00, 0x00,
333            0x01, 0x00, 0x00, 0x00
334        ];
335
336        let data = parse_value(&raw).unwrap();
337
338        assert_eq!(data.len, 4);
339        assert_eq!(data.name, "bndt");
340        assert_eq!(data.data, [0x02, 0x00, 0x00, 0x00]);
341        assert_eq!(data.rem, [0x01, 0x00, 0x00, 0x00]);
342    }
343
344    #[test]
345    fn test_parse_trans() {
346        let raw = [
347            0x1c, 0x00, 0x00, 0x00,
348
349            0x74, 0x72, 0x61, 0x6e,
350
351            0x00, 0x00, 0x9c, 0xa2,
352            0x00, 0xc0, 0xfe, 0xa4,
353            0x00, 0x00, 0xd0, 0xa3,
354            0x00, 0x00, 0x80, 0x3f,
355
356            0x17, 0x56, 0x03, 0xbc,
357            0x7c, 0x48, 0xd0, 0xbd,
358            0x0c, 0xa8, 0x03, 0x3e,
359        ];
360
361        let (len, data) = parse_trans(&raw).unwrap();
362
363        assert_eq!(len, 28);
364
365        assert_eq!(data.rot.x, -4.22838847e-18);
366        assert_eq!(data.rot.y, -1.104802e-16);
367        assert_eq!(data.rot.z, -2.25514052e-17);
368        assert_eq!(data.rot.w, 1.0);
369
370        assert_eq!(data.pos.x, -0.008016131);
371        assert_eq!(data.pos.y, -0.101700753);
372        assert_eq!(data.pos.z, 0.128570735);
373    }
374}
mocopi_parser/lib.rs

mocopi_parser/
lib.rs
