use anyhow::Result;
use std::collections::HashMap;
use std::cell::RefCell;
use std::fs::File;
use std::io;
use std::io::prelude::*;
use std::io::Cursor;
use std::io::SeekFrom;
use std::mem;
use std::rc::Rc;
use std::slice;
use thiserror::Error;
#[derive(Error, Debug)]
pub enum ParserError {
#[error("magic word not unmatched, might not be a c3d file")]
UnmatchMagic,
#[error("unable to parse paramter block")]
ParseParameterError,
#[error("io error")]
IoError(#[from] io::Error),
#[error("missing header/parameter")]
MissingField,
}
pub struct C3dAdapter<T: Read + Seek> {
pub header: Option<HeaderBlock>,
pub parameter: Option<ParameterBlock>,
handle: Rc<RefCell<T>>,
}
impl<T: Read + Seek> C3dAdapter<T> {
pub fn new(mut file: T) -> Result<Self, ParserError> {
file.seek(SeekFrom::Start(0))?;
let handle = Rc::new(RefCell::new(file));
Ok(C3dAdapter {
header: None,
parameter: None,
handle,
})
}
pub fn construct(mut self) -> Result<Self, ParserError> {
let header = HeaderBlock::from_reader(&mut *self.handle.borrow_mut());
let parameter = ParameterBlock::from_reader(&mut *self.handle.borrow_mut());
if header.magic_word != 0x50 {
return Err(ParserError::UnmatchMagic);
}
if parameter.header.magic_word != 0x50 + 4 {
dbg!(parameter.header.magic_word);
return Err(ParserError::UnmatchMagic);
}
self.header.replace(header);
self.parameter.replace(parameter);
Ok(self)
}
}
pub struct C3dReader<'a, R: Read + Seek> {
header: &'a HeaderBlock,
parameter: &'a ParameterBlock,
handle: std::cell::RefMut<'a, R>,
points_buffer: Vec<u8>,
analog_buffer: Vec<u8>,
frame_idx: u16,
analog_unsigned: bool,
analog_offset: Option<Vec<f32>>,
analog_scale: Option<Vec<f32>>,
analog_gen_scale: Option<f32>,
}
impl<'a, R: Read + Seek> C3dReader<'a, R> {
pub fn new(
header: &'a HeaderBlock,
parameter: &'a ParameterBlock,
mut handle: std::cell::RefMut<'a, R>,
) -> Result<Self, ParserError> {
(*handle).seek(SeekFrom::Start((header.data_start as u64 - 1) * 512))?;
let points_buffer: Vec<u8> = vec![];
let analog_buffer: Vec<u8> = vec![];
let analog_unsigned = parameter
.get("ANALOG:FORMAT")
.map(|param| {
param
.parameter_data
.values
.iter()
.filter_map(|v| v.as_char())
.filter(|c| !c.is_whitespace())
.collect::<String>()
})
.and(Some("UNSIGNED"))
.is_some();
let analog_offset = parameter.get("ANALOG:OFFSET").map(|v| {
v.parameter_data
.values
.iter()
.filter_map(|arr| arr.as_i16())
.map(|a| *a as f32)
.collect::<Vec<f32>>()
});
let analog_scale = parameter.get("ANALOG:SCALE").map(|v| {
v.parameter_data
.values
.iter()
.filter_map(|arr| arr.as_f32())
.map(|a| *a)
.collect::<Vec<f32>>()
});
let analog_gen_scale = parameter
.get("ANALOG:GEN_SCALE")
.map(|v| {
v.parameter_data
.values
.iter()
.filter_map(|arr| arr.as_f32())
.map(|a| *a)
.next()
})
.flatten();
log::debug!("analog offsets: {:?}", analog_offset);
log::debug!("scale factors: {:?}", analog_scale);
log::debug!("genral scale factor: {:?}", analog_gen_scale);
Ok(C3dReader {
header,
parameter,
handle,
points_buffer,
analog_buffer,
frame_idx: header.frame_first,
analog_unsigned,
analog_offset,
analog_scale,
analog_gen_scale,
})
}
}
impl<'a, R: Read + Seek> Iterator for C3dReader<'a, R> {
type Item = (u16, PointData, Option<AnalogData>);
fn next(&mut self) -> Option<Self::Item> {
if self.frame_idx > self.header.frame_last {
return None;
}
let point_scale = self.header.scale;
let is_float = point_scale <= 0.0;
let point_data_length = if is_float { 4 } else { 2 };
let point_scale = if is_float { 1.0 } else { point_scale.abs() };
let analog_data_length = if is_float { 4 } else { 2 };
let points_n = 4 * self.header.point_counts;
let analog_n = self.header.analog_counts;
self.points_buffer
.resize((points_n * point_data_length) as usize, 0_u8);
if self.handle.read_exact(&mut self.points_buffer[..]).is_err() {
return None;
}
let values = self
.points_buffer
.chunks_exact(4 * point_data_length as usize)
.map(|arr| {
let mut points_vec = [0_f32; 5];
let raw_vec = arr
.chunks_exact(point_data_length as usize)
.filter_map(|arr| {
Some(if is_float {
let mut buf = [0_u8; 4];
arr.clone().read_exact(&mut buf).unwrap();
f32::from_le_bytes(buf)
} else {
let mut buf = [0_u8; 2];
arr.clone().read_exact(&mut buf).unwrap();
i16::from_le_bytes(buf) as f32 * point_scale
})
})
.collect::<Vec<f32>>();
points_vec[..4].copy_from_slice(&raw_vec);
points_vec
})
.map(|mut arr| {
let fourth = &arr[3];
if *fourth <= -0.01_f32 {
arr[3..5].iter_mut().for_each(|v| {
*v = -0.01_f32;
});
} else {
let err = arr[3] as i16;
arr[3] = (err & 0xff) as f32 * self.header.scale.abs();
arr[4] = (8..17)
.map(|idx| {
let mask = idx << 8;
(err & mask) as f32
})
.sum();
}
arr
})
.collect::<Vec<_>>();
let point_data = PointData { values };
let analog_data = if analog_n > 0 {
self.analog_buffer
.resize((analog_n * analog_data_length) as usize, 0_u8);
if self.handle.read_exact(&mut self.analog_buffer[..]).is_err() {
return None;
}
let mut values: Vec<f32> = self
.analog_buffer
.chunks_exact(analog_data_length as usize)
.map(|arr| {
if is_float {
let mut buf = [0_u8; 4];
(arr.clone()).read_exact(&mut buf).unwrap();
f32::from_le_bytes(buf)
} else {
let mut buf = [0_u8; 2];
if self.analog_unsigned {
(arr.clone()).read_exact(&mut buf).unwrap();
u16::from_le_bytes(buf) as f32
} else {
(arr.clone()).read_exact(&mut buf).unwrap();
i16::from_le_bytes(buf) as f32
}
}
})
.collect();
if let Some(offsets) = self.analog_offset.as_ref() {
values.iter_mut().zip(offsets.iter()).for_each(|(v, off)| {
*v -= *off;
});
}
if let Some(scales) = self.analog_scale.as_ref() {
values.iter_mut().zip(scales.iter()).for_each(|(v, off)| {
*v *= *off;
});
}
if let Some(gen_scale) = self.analog_gen_scale.as_ref() {
values.iter_mut().for_each(|v| {
*v *= *gen_scale;
});
}
Some(AnalogData { values })
} else {
None
};
let frame_idx = self.frame_idx;
self.frame_idx += 1;
Some((frame_idx, point_data, analog_data))
}
}
impl<T: Read + Seek> C3dAdapter<T> {
pub fn get_point_labels(&self) -> Option<Vec<String>> {
let mut rv = None;
if let Some(parameter) = self.parameter.as_ref() {
rv = parameter.get("POINT:LABELS").map(|param| {
param
.parameter_data
.values
.chunks_exact(param.dimensions[0] as usize)
.map(|arr| {
arr.iter()
.filter_map(|v| v.as_char())
.filter(|c| !c.is_whitespace())
.collect::<String>()
})
.collect::<Vec<String>>()
});
}
rv
}
pub fn get_analog_labels(&self) -> Option<Vec<String>> {
let mut rv = None;
if let Some(parameter) = self.parameter.as_ref() {
if let Some(analog) = parameter.groups.get("ANALOG") {
let mut keys = analog
.params
.keys()
.filter_map(|v| if v.contains("LABEL") { Some(v) } else { None })
.collect::<Vec<_>>();
keys.sort();
rv = Some(
keys.iter()
.filter_map(|v| {
parameter.get(&format!("ANALOG:{}", v)).map(|param| {
param
.parameter_data
.values
.chunks_exact(param.dimensions[0] as usize)
.map(|arr| {
arr.iter()
.filter_map(|c| c.as_char())
.filter(|c| !c.is_whitespace())
.collect::<String>()
})
.collect::<Vec<String>>()
})
})
.flatten()
.collect::<Vec<_>>(),
);
}
}
rv
}
pub fn reader<'a>(&'a self) -> Result<C3dReader<'a, T>, ParserError> {
if let Some(header) = self.header.as_ref() {
if let Some(parameter) = self.parameter.as_ref() {
let handle = self.handle.borrow_mut();
return C3dReader::new(header, parameter, handle);
}
}
Err(ParserError::MissingField)
}
}
#[repr(C, packed)]
#[derive(Copy, Clone)]
pub struct HeaderBlock {
parameter_start: u8,
magic_word: u8,
pub point_counts: u16,
pub analog_counts: u16,
pub frame_first: u16,
pub frame_last: u16,
pub max_gap: u16,
pub scale: f32,
data_start: u16,
pub analog_per_frame: u16,
pub frame_rate: f32,
reserved: [u8; 274],
event_lables_long: u16,
event_counts: u16,
reserved_two: u16,
event_times: [f32; 18],
event_display_flags: [u8; 18],
reserved_three: u16,
event_labels: [u8; 72],
reserved_four: [u8; 44],
}
trait FromReader {
fn from_reader<R: Read + Seek>(r: &mut R) -> Self;
}
impl FromReader for HeaderBlock {
fn from_reader<R: Read + Seek>(r: &mut R) -> Self {
let mut header: HeaderBlock = unsafe { mem::zeroed() };
let header_size = mem::size_of::<HeaderBlock>();
unsafe {
let header_slice =
slice::from_raw_parts_mut(&mut header as *mut _ as *mut u8, header_size);
r.read_exact(header_slice).unwrap();
}
header
}
}
impl FromReader for ParameterBlockHeader {
fn from_reader<R: Read + Seek>(r: &mut R) -> Self {
let mut parameter: ParameterBlockHeader = unsafe { mem::zeroed() };
unsafe {
let parameter_slice = slice::from_raw_parts_mut(&mut parameter as *mut _ as *mut u8, 4);
r.read_exact(parameter_slice).unwrap();
}
parameter
}
}
impl FromReader for ParameterBlock {
fn from_reader<R: Read + Seek>(r: &mut R) -> Self {
let header = ParameterBlockHeader::from_reader(r);
let mut u8_buffer = [0_u8];
let mut i8_buffer = [0_u8];
let mut i16_buffer = [0_u8; 2];
let mut string_buffer: Vec<u8> = vec![];
let mut parameter_buf: Vec<u8> = vec![];
parameter_buf.resize(header.parameter_block_counts as usize * 512 - 4, 0);
r.read_exact(&mut parameter_buf).unwrap();
let mut parameter_block_cursor = Cursor::new(¶meter_buf[..]);
let mut groups = HashMap::<u8, GroupFormat>::new();
loop {
parameter_block_cursor.read_exact(&mut i8_buffer).unwrap();
let name_chars_size = i8::from_le_bytes(i8_buffer);
let locked = name_chars_size < 0;
let name_chars_size = name_chars_size.abs() as usize;
parameter_block_cursor.read_exact(&mut i8_buffer).unwrap();
let id: i8 = i8::from_le_bytes(i8_buffer);
if id == 0 || name_chars_size == 0 {
break;
}
string_buffer.resize(name_chars_size, 0_u8);
parameter_block_cursor
.read_exact(&mut string_buffer)
.unwrap();
let name = String::from_utf8(string_buffer.clone()).unwrap();
parameter_block_cursor.read_exact(&mut i16_buffer).unwrap();
let offset = i16::from_le_bytes(i16_buffer);
let is_param = id > 0;
if is_param {
parameter_block_cursor.read_exact(&mut i8_buffer).unwrap();
let data_length = i8::from_le_bytes(i8_buffer);
parameter_block_cursor.read_exact(&mut u8_buffer).unwrap();
let num_dimensions = u8_buffer[0];
let mut num_elements = 1_i32;
let mut buf = [0_u8; 1];
let mut dimensions = vec![];
for _ in 0..num_dimensions {
parameter_block_cursor.read_exact(&mut buf).unwrap();
dimensions.push(buf[0]);
num_elements *= buf[0] as i32;
}
let total_data_length = num_elements as i16 * data_length.abs() as i16;
let mut data_buffer = vec![0_u8; total_data_length as usize];
parameter_block_cursor.read_exact(&mut data_buffer).unwrap();
let datas: Vec<Box<dyn ParamValue>> = data_buffer
.chunks_exact(data_length.abs() as usize)
.filter_map(|arr| match data_length {
1 => Some(Box::new(arr[0] as u8) as Box<dyn ParamValue>),
2 => {
let mut buf = [0_u8; 2];
(arr.clone()).read_exact(&mut buf).unwrap();
let val = i16::from_le_bytes(buf);
Some(Box::new(val) as Box<dyn ParamValue>)
}
4 => {
let mut buf = [0_u8; 4];
(arr.clone()).read_exact(&mut buf).unwrap();
let val = f32::from_le_bytes(buf);
Some(Box::new(val))
}
-1 => Some(Box::new(arr[0] as char) as Box<dyn ParamValue>),
_ => None,
})
.collect();
let param_data = ParamData { values: datas };
parameter_block_cursor.read_exact(&mut u8_buffer).unwrap();
let desc_chars_size = u8_buffer[0];
string_buffer.resize(desc_chars_size as usize, 0);
let desc = String::from_utf8(string_buffer.clone()).unwrap();
let param = ParameterFormat {
id,
name_chars_size: name_chars_size as u8,
name: name.clone(),
offset,
data_length,
num_dimensions,
dimensions,
parameter_data: param_data,
desc_chars_size,
description: desc,
locked,
};
let group_id = id as u8;
if let Some(group) = groups.get_mut(&group_id) {
group.params.insert(name, param);
} else {
let mut group = GroupFormat::default();
group.params.insert(name, param);
groups.insert(group_id, group);
}
} else {
let group_id = id.abs() as u8;
parameter_block_cursor.read_exact(&mut u8_buffer).unwrap();
let desc_chars_size = u8_buffer[0];
string_buffer.resize(desc_chars_size as usize, 0);
let desc = String::from_utf8(string_buffer.clone()).unwrap();
if let Some(group) = groups.get_mut(&group_id) {
group.name = name;
group.description = desc;
} else {
let new_group = GroupFormat {
name,
description: desc,
locked,
..Default::default()
};
groups.insert(group_id, new_group);
}
}
parameter_buf = parameter_buf.split_off(2 + name_chars_size as usize + offset as usize);
parameter_block_cursor = Cursor::new(¶meter_buf[..]);
}
let groups: HashMap<String, GroupFormat> = groups
.into_iter()
.map(|(_, v)| (v.name.clone(), v))
.collect();
let parameter_block = ParameterBlock { header, groups };
parameter_block
}
}
#[derive(Debug)]
pub struct ParameterBlock {
header: ParameterBlockHeader,
pub groups: HashMap<String, GroupFormat>,
}
impl ParameterBlock {
pub fn get(&self, key: &str) -> Option<&ParameterFormat> {
let split_key: &'static str;
if key.contains(".") {
split_key = ".";
} else if key.contains(":") {
split_key = ":";
} else {
return None;
}
let mut iter = key.split(split_key);
let group_key = iter.next().unwrap();
let param_key = iter.next().unwrap();
if let Some(group) = self.groups.get(group_key) {
if let Some(param) = group.params.get(param_key) {
return Some(¶m);
}
}
None
}
}
#[repr(C, packed)]
#[derive(Copy, Clone, Debug)]
struct ParameterBlockHeader {
reserved_one: u8,
reserved_two: u8,
parameter_block_counts: u8,
magic_word: u8,
}
#[derive(Debug)]
pub struct ParameterFormat {
name_chars_size: u8,
id: i8,
pub name: String,
offset: i16,
pub data_length: i8,
pub num_dimensions: u8,
pub dimensions: Vec<u8>,
pub parameter_data: ParamData,
desc_chars_size: u8,
pub description: String,
pub locked: bool,
}
#[derive(Default, Debug)]
pub struct GroupFormat {
pub name: String,
pub description: String,
pub locked: bool,
pub params: HashMap<String, ParameterFormat>,
}
#[derive(Debug)]
pub struct ParamData {
pub values: Vec<Box<dyn ParamValue>>,
}
#[derive(Debug)]
pub struct PointData {
pub values: Vec<[f32; 5]>,
}
#[derive(Debug)]
pub struct AnalogData {
pub values: Vec<f32>,
}
pub trait ParamValue: std::fmt::Debug + ParamClone {
fn as_char(&self) -> Option<&char> {
None
}
fn as_f32(&self) -> Option<&f32> {
None
}
fn as_i16(&self) -> Option<&i16> {
None
}
fn as_u8(&self) -> Option<&u8> {
None
}
}
pub trait ParamClone {
fn clone_box(&self) -> Box<dyn ParamValue>;
}
impl<T> ParamClone for T
where
T: 'static + ParamValue + Clone,
{
fn clone_box(&self) -> Box<dyn ParamValue> {
Box::new(self.clone())
}
}
impl ParamValue for char {
fn as_char(&self) -> Option<&char> {
Some(self)
}
}
impl ParamValue for f32 {
fn as_f32(&self) -> Option<&f32> {
Some(self)
}
}
impl ParamValue for i16 {
fn as_i16(&self) -> Option<&i16> {
Some(&self)
}
}
impl ParamValue for u8 {
fn as_u8(&self) -> Option<&u8> {
Some(&self)
}
}
#[cfg(test)]
mod tests {
use super::*;
fn set_logger() {
femme::with_level(log::LevelFilter::Debug);
}
#[test]
fn test_parser() -> Result<()> {
set_logger();
let mut file = File::open("test_data/vicon_trial.c3d")?;
let mut buf: Vec<u8> = vec![];
file.read_to_end(&mut buf)?;
let mut cursor = Cursor::new(&buf[..]);
let adapter = C3dAdapter::new(&mut cursor)?.construct()?;
for (i, p, a) in adapter.reader()?.into_iter() {
dbg!(i, p, a);
}
adapter.get_point_labels().unwrap();
adapter.get_analog_labels().unwrap();
let mut file = File::open("test_data/motion_shadow.c3d")?;
let adapter = C3dAdapter::new(&mut file)?.construct()?;
for (i, p, a) in adapter.reader()?.into_iter() {
dbg!(i, p, a);
}
Ok(())
}
}