avatar_anim/
io.rs

1use binrw::{
2    BinRead, BinResult, Endian,
3    io::{Read, Seek, Write},
4};
5use glam::{EulerRot, Quat, Vec3};
6use std::string::FromUtf8Error;
7
8use crate::{PositionKey, RotationKey};
9
10pub(crate) const KEYFRAME_MOTION_VERSION: u16 = 1;
11pub(crate) const KEYFRAME_MOTION_SUBVERSION: u16 = 0;
12const KEYFRAME_MOTION_OLD_VERSION: u16 = 0;
13const KEYFRAME_MOTION_OLD_SUBVERSION: u16 = 1;
14pub(crate) const MAX_PELVIS_OFFSET: f32 = 5.0;
15
16const OOU16MAX: f32 = 1.0f32 / u16::MAX as f32;
17
18fn clamp(value: f32, lower: f32, upper: f32) -> f32 {
19    value.min(upper).max(lower)
20}
21
22pub(crate) fn f32_to_u16(value: f32, lower: f32, upper: f32) -> u16 {
23    let mut val = clamp(value, lower, upper);
24    val -= lower;
25    val /= upper - lower;
26    (val * u16::MAX as f32).floor() as u16
27}
28
29pub(crate) fn u16_to_f32(value: u16, lower: f32, upper: f32) -> f32 {
30    let mut val = value as f32 * OOU16MAX;
31    let delta = upper - lower;
32    val *= delta;
33    val += lower;
34
35    let max_error = delta * OOU16MAX;
36    if val.abs() < max_error {
37        val = 0.0;
38    }
39
40    val
41}
42
43pub fn read_null_terminated_string<R: Read + Seek>(
44    r: &mut R,
45    _: Endian,
46    _: (),
47) -> BinResult<String> {
48    let mut buf = Vec::new();
49    let mut byte = [0u8; 1];
50    loop {
51        r.read_exact(&mut byte)?;
52        if byte[0] == 0 {
53            break;
54        }
55        buf.push(byte[0]);
56    }
57    String::from_utf8(buf).map_err(|e: FromUtf8Error| binrw::Error::AssertFail {
58        pos: 0,
59        message: format!("Invalid UTF-8 in null-terminated string: {e}"),
60    })
61}
62
63pub fn write_null_terminated_string<W: Write + Seek>(
64    data: &String,
65    w: &mut W,
66    _: Endian,
67    _: (),
68) -> BinResult<()> {
69    w.write_all(data.as_bytes())?;
70    w.write_all(&[0])?;
71    Ok(())
72}
73
74pub fn read_fixed_length_string<R: Read + Seek>(
75    r: &mut R,
76    _: Endian,
77    args: (usize,),
78) -> BinResult<String> {
79    let length = args.0;
80    let mut buf = vec![0u8; length];
81    r.read_exact(&mut buf)?;
82    if let Some(pos) = buf.iter().position(|&b| b == 0) {
83        buf.truncate(pos);
84    }
85    Ok(String::from_utf8(buf).unwrap_or_default())
86}
87
88pub fn write_fixed_length_string<W: Write + Seek>(
89    data: &String,
90    w: &mut W,
91    _: Endian,
92    args: (usize,),
93) -> BinResult<()> {
94    let length = args.0;
95    let mut buf = data.as_bytes().to_vec();
96    buf.resize(length, 0);
97    w.write_all(&buf)?;
98    Ok(())
99}
100
101pub fn read_rot_quat<R: Read + Seek>(reader: &mut R, e: Endian, _: ()) -> BinResult<Quat> {
102    use binrw::BinRead;
103    let x: f32 = u16_to_f32(u16::read_options(reader, e, ())?, -1.0, 1.0);
104    let y: f32 = u16_to_f32(u16::read_options(reader, e, ())?, -1.0, 1.0);
105    let z: f32 = u16_to_f32(u16::read_options(reader, e, ())?, -1.0, 1.0);
106    let sum = x * x + y * y + z * z;
107    let w = if sum <= 1.0 { (1.0 - sum).sqrt() } else { 0.0 };
108    let mut q = Quat::from_xyzw(x, y, z, w);
109    if q.length_squared() > 0.0 {
110        q = q.normalize();
111    }
112    if q.w < 0.0 {
113        q = Quat::from_xyzw(-q.x, -q.y, -q.z, -q.w);
114    }
115    Ok(q)
116}
117
118pub fn write_rot_quat<W: Write + Seek>(
119    value: &Quat,
120    writer: &mut W,
121    e: Endian,
122    _: (),
123) -> BinResult<()> {
124    use binrw::BinWrite;
125    let mut q = if value.length_squared() > 0.0 {
126        value.normalize()
127    } else {
128        *value
129    };
130    // Enforce canonical hemisphere (positive w) for stable roundtrips
131    if q.w < 0.0 {
132        q = Quat::from_xyzw(-q.x, -q.y, -q.z, -q.w);
133    }
134    f32_to_u16(q.x, -1.0, 1.0).write_options(writer, e, ())?;
135    f32_to_u16(q.y, -1.0, 1.0).write_options(writer, e, ())?;
136    f32_to_u16(q.z, -1.0, 1.0).write_options(writer, e, ())
137}
138
139pub fn read_pos_vec3<R: Read + Seek>(reader: &mut R, e: Endian, _: ()) -> BinResult<Vec3> {
140    use binrw::BinRead;
141    let x = u16_to_f32(
142        u16::read_options(reader, e, ())?,
143        -MAX_PELVIS_OFFSET,
144        MAX_PELVIS_OFFSET,
145    );
146    let y = u16_to_f32(
147        u16::read_options(reader, e, ())?,
148        -MAX_PELVIS_OFFSET,
149        MAX_PELVIS_OFFSET,
150    );
151    let z = u16_to_f32(
152        u16::read_options(reader, e, ())?,
153        -MAX_PELVIS_OFFSET,
154        MAX_PELVIS_OFFSET,
155    );
156    Ok(Vec3::new(x, y, z))
157}
158
159pub fn write_pos_vec3<W: Write + Seek>(
160    value: &Vec3,
161    writer: &mut W,
162    e: Endian,
163    _: (),
164) -> BinResult<()> {
165    use binrw::BinWrite;
166    f32_to_u16(value.x, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET).write_options(writer, e, ())?;
167    f32_to_u16(value.y, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET).write_options(writer, e, ())?;
168    f32_to_u16(value.z, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET).write_options(writer, e, ())
169}
170
171// Quantization helper docs:
172// Rotation components are stored as 3 * u16 for x,y,z with range [-1,1]; w is reconstructed.
173// Max component absolute quantization error ~= 1 / 65535 * 2 = 3.05e-5 before normalization.
174// Position components stored as u16 over [-5,5]; max absolute error ~= 10 / 65535 ≈ 1.53e-4.
175
176pub fn quantize_rotation(q: Quat) -> (u16, u16, u16) {
177    let mut qn = if q.length_squared() > 0.0 {
178        q.normalize()
179    } else {
180        q
181    };
182    if qn.w < 0.0 {
183        qn = Quat::from_xyzw(-qn.x, -qn.y, -qn.z, -qn.w);
184    }
185    (
186        f32_to_u16(qn.x, -1.0, 1.0),
187        f32_to_u16(qn.y, -1.0, 1.0),
188        f32_to_u16(qn.z, -1.0, 1.0),
189    )
190}
191
192pub fn quantize_position(v: Vec3) -> (u16, u16, u16) {
193    (
194        f32_to_u16(v.x, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET),
195        f32_to_u16(v.y, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET),
196        f32_to_u16(v.z, -MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET),
197    )
198}
199
200#[derive(Clone, Copy, Debug, PartialEq, Eq)]
201pub enum AnimVersion {
202    Old,
203    Current,
204}
205
206pub(crate) fn classify_anim_version(version: u16, sub_version: u16) -> BinResult<AnimVersion> {
207    if version == KEYFRAME_MOTION_OLD_VERSION && sub_version == KEYFRAME_MOTION_OLD_SUBVERSION {
208        Ok(AnimVersion::Old)
209    } else if version == KEYFRAME_MOTION_VERSION && sub_version == KEYFRAME_MOTION_SUBVERSION {
210        Ok(AnimVersion::Current)
211    } else {
212        Err(binrw::Error::AssertFail {
213            pos: 0,
214            message: format!("Unsupported animation version {version}.{sub_version}"),
215        })
216    }
217}
218
219#[derive(Clone, Copy, Debug)]
220pub struct AnimReadContext {
221    pub version: AnimVersion,
222    pub duration: f32,
223}
224
225fn canonicalize_quat(mut quat: Quat) -> Quat {
226    if quat.length_squared() > 0.0 {
227        quat = quat.normalize();
228    }
229    if quat.w < 0.0 {
230        quat = Quat::from_xyzw(-quat.x, -quat.y, -quat.z, -quat.w);
231    }
232    quat
233}
234
235fn quat_from_degrees_zyx(x_deg: f32, y_deg: f32, z_deg: f32) -> Quat {
236    canonicalize_quat(Quat::from_euler(
237        EulerRot::ZYX,
238        z_deg.to_radians(),
239        y_deg.to_radians(),
240        x_deg.to_radians(),
241    ))
242}
243
244fn quantize_time(time: f32, duration: f32) -> u16 {
245    if duration <= 0.0 {
246        return 0;
247    }
248    let clamped = time.clamp(0.0, duration);
249    f32_to_u16(clamped, 0.0, duration)
250}
251
252pub(crate) fn read_rotation_keys<R: Read + Seek>(
253    reader: &mut R,
254    endian: Endian,
255    count: i32,
256    ctx: AnimReadContext,
257) -> BinResult<Vec<RotationKey>> {
258    if count < 0 {
259        return Err(binrw::Error::AssertFail {
260            pos: 0,
261            message: "num_rot_keys must be non-negative".into(),
262        });
263    }
264    let mut keys = Vec::with_capacity(count as usize);
265    for _ in 0..count {
266        let time = if ctx.version == AnimVersion::Old {
267            let time_seconds: f32 = f32::read_options(reader, endian, ())?;
268            quantize_time(time_seconds, ctx.duration)
269        } else {
270            u16::read_options(reader, endian, ())?
271        };
272
273        let rot = if ctx.version == AnimVersion::Old {
274            let angles: [f32; 3] = <[f32; 3]>::read_options(reader, endian, ())?;
275            quat_from_degrees_zyx(angles[0], angles[1], angles[2])
276        } else {
277            read_rot_quat(reader, endian, ())?
278        };
279
280        keys.push(RotationKey { time, rot });
281    }
282    Ok(keys)
283}
284
285pub(crate) fn read_position_keys<R: Read + Seek>(
286    reader: &mut R,
287    endian: Endian,
288    count: i32,
289    ctx: AnimReadContext,
290) -> BinResult<Vec<PositionKey>> {
291    if count < 0 {
292        return Err(binrw::Error::AssertFail {
293            pos: 0,
294            message: "num_pos_keys must be non-negative".into(),
295        });
296    }
297    let mut keys = Vec::with_capacity(count as usize);
298    for _ in 0..count {
299        let time = if ctx.version == AnimVersion::Old {
300            let time_seconds: f32 = f32::read_options(reader, endian, ())?;
301            quantize_time(time_seconds, ctx.duration)
302        } else {
303            u16::read_options(reader, endian, ())?
304        };
305
306        let pos = if ctx.version == AnimVersion::Old {
307            let mut components: [f32; 3] = <[f32; 3]>::read_options(reader, endian, ())?;
308            for value in &mut components {
309                *value = value.clamp(-MAX_PELVIS_OFFSET, MAX_PELVIS_OFFSET);
310            }
311            Vec3::from_array(components)
312        } else {
313            read_pos_vec3(reader, endian, ())?
314        };
315
316        keys.push(PositionKey { time, pos });
317    }
318    Ok(keys)
319}
avatar_anim/io.rs

avatar_anim/
io.rs
