steel_utils/codec/
lp_vec3.rs

1use std::io::{Cursor, Result, Write};
2
3use glam::DVec3;
4
5use crate::{
6    codec::VarInt,
7    serial::{ReadFrom, WriteTo},
8};
9
10const DATA_BITS_MASK: u64 = 32_767;
11const MAX_QUANTIZED_VALUE: f64 = 32_766.0;
12const CONTINUATION_FLAG: u8 = 4;
13const ABS_MAX_VALUE: f64 = 1.717_986_918_3E10;
14const ABS_MIN_VALUE: f64 = 3.051_944_088_384_301E-5;
15
16/// Vanilla `LpVec3` packed vector codec.
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct LpVec3(pub DVec3);
19
20impl ReadFrom for LpVec3 {
21    fn read(data: &mut Cursor<&[u8]>) -> Result<Self> {
22        let lowest = u8::read(data)?;
23        if lowest == 0 {
24            return Ok(Self(DVec3::ZERO));
25        }
26
27        let middle = u8::read(data)?;
28        let highest = u32::read(data)?;
29        let buffer = (u64::from(highest) << 16) | (u64::from(middle) << 8) | u64::from(lowest);
30        let mut scale = u64::from(lowest & 3);
31        if has_continuation_bit(lowest) {
32            scale |= u64::from(VarInt::read(data)?.0 as u32) << 2;
33        }
34        let scale = scale as f64;
35
36        Ok(Self(DVec3::new(
37            unpack(buffer >> 3) * scale,
38            unpack(buffer >> 18) * scale,
39            unpack(buffer >> 33) * scale,
40        )))
41    }
42}
43
44impl WriteTo for LpVec3 {
45    fn write(&self, writer: &mut impl Write) -> Result<()> {
46        let x = sanitize(self.0.x);
47        let y = sanitize(self.0.y);
48        let z = sanitize(self.0.z);
49        let chessboard_length = x.abs().max(y.abs().max(z.abs()));
50
51        if chessboard_length < ABS_MIN_VALUE {
52            return writer.write_all(&[0]);
53        }
54
55        let scale = chessboard_length.ceil() as i64;
56        let is_partial = (scale & 3) != scale;
57        let markers = if is_partial {
58            (scale & 3) | i64::from(CONTINUATION_FLAG)
59        } else {
60            scale
61        };
62        let buffer = markers
63            | (pack(x / scale as f64) << 3)
64            | (pack(y / scale as f64) << 18)
65            | (pack(z / scale as f64) << 33);
66
67        writer.write_all(&[buffer as u8])?;
68        writer.write_all(&[(buffer >> 8) as u8])?;
69        writer.write_all(&((buffer >> 16) as u32).to_be_bytes())?;
70        if is_partial {
71            VarInt((scale >> 2) as i32).write(writer)?;
72        }
73        Ok(())
74    }
75}
76
77const fn has_continuation_bit(value: u8) -> bool {
78    (value & CONTINUATION_FLAG) == CONTINUATION_FLAG
79}
80
81fn sanitize(value: f64) -> f64 {
82    if value.is_nan() {
83        0.0
84    } else {
85        value.clamp(-ABS_MAX_VALUE, ABS_MAX_VALUE)
86    }
87}
88
89fn pack(value: f64) -> i64 {
90    ((value * 0.5 + 0.5) * MAX_QUANTIZED_VALUE).round() as i64
91}
92
93fn unpack(value: u64) -> f64 {
94    (value & DATA_BITS_MASK).min(MAX_QUANTIZED_VALUE as u64) as f64 * 2.0 / MAX_QUANTIZED_VALUE
95        - 1.0
96}
97
98#[cfg(test)]
99mod tests {
100    use super::*;
101
102    fn round_trip(value: DVec3) -> Result<DVec3> {
103        let mut bytes = Vec::new();
104        LpVec3(value).write(&mut bytes)?;
105        Ok(LpVec3::read(&mut Cursor::new(&bytes))?.0)
106    }
107
108    #[test]
109    fn zero_vector_is_single_zero_byte() -> Result<()> {
110        let mut bytes = Vec::new();
111
112        LpVec3(DVec3::ZERO).write(&mut bytes)?;
113
114        assert_eq!(bytes, vec![0]);
115        assert_eq!(LpVec3::read(&mut Cursor::new(&bytes))?.0, DVec3::ZERO);
116        Ok(())
117    }
118
119    #[test]
120    fn packed_vector_matches_vanilla_quantization() -> Result<()> {
121        let decoded = round_trip(DVec3::new(0.25, 0.5, 0.75))?;
122
123        assert!((decoded.x - 0.25).abs() < 0.0001);
124        assert!((decoded.y - 0.5).abs() < 0.0001);
125        assert!((decoded.z - 0.75).abs() < 0.0001);
126        Ok(())
127    }
128
129    #[test]
130    fn continuation_scale_round_trips_large_vector() -> Result<()> {
131        let decoded = round_trip(DVec3::new(5.0, 0.0, -3.0))?;
132
133        assert!((decoded.x - 5.0).abs() < 0.001);
134        assert!(decoded.y.abs() < 0.001);
135        assert!((decoded.z + 3.0).abs() < 0.001);
136        Ok(())
137    }
138}
steel_utils/codec/lp_vec3.rs

steel_utils/codec/
lp_vec3.rs
