ghast_protocol/types/
lpvec3.rs1use bytes::{BufMut, BytesMut};
14
15use crate::codec::{Decode, DecodeResult, Encode, EncodeResult, McWrite, Reader};
16
17const MAX_Q: f64 = 32766.0;
18const ABS_MAX: f64 = 1.717_986_918_3e10;
19const ABS_MIN: f64 = 3.051_944_088_384_301e-5;
20
21#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
23pub struct LpVec3 {
24 pub packed: u64,
26 pub scale_high: u32,
28}
29
30impl LpVec3 {
31 pub const ZERO: LpVec3 = LpVec3 {
33 packed: 0,
34 scale_high: 0,
35 };
36
37 pub fn read(r: &mut Reader<'_>) -> DecodeResult<Self> {
39 let lowest = r.read_u8()?;
40 if lowest == 0 {
41 return Ok(Self::ZERO);
42 }
43 let middle = r.read_u8()?;
44 let highest = r.read_u32()?;
45 let packed = u64::from(highest) << 16 | u64::from(middle) << 8 | u64::from(lowest);
46 let scale_high = if lowest & 4 != 0 {
47 r.read_varint()? as u32
48 } else {
49 0
50 };
51 Ok(LpVec3 { packed, scale_high })
52 }
53
54 pub fn write(&self, w: &mut BytesMut) {
58 let lowest = self.packed as u8;
59 if lowest == 0 {
60 w.put_u8(0);
61 return;
62 }
63 w.put_u8(lowest);
64 w.put_u8((self.packed >> 8) as u8);
65 w.put_u32((self.packed >> 16) as u32);
66 if lowest & 4 != 0 {
67 w.put_varint(self.scale_high as i32);
68 }
69 }
70
71 pub fn scale(&self) -> u64 {
73 let low = self.packed & 3;
74 if self.packed & 4 != 0 {
75 low | u64::from(self.scale_high) << 2
76 } else {
77 low
78 }
79 }
80
81 pub fn to_vec3(&self) -> [f64; 3] {
83 if self.packed == 0 {
84 return [0.0; 3];
85 }
86 let s = self.scale() as f64;
87 let unpack =
88 |shift: u32| ((self.packed >> shift & 0x7fff) as f64).min(MAX_Q) * 2.0 / MAX_Q - 1.0;
89 [unpack(3) * s, unpack(18) * s, unpack(33) * s]
90 }
91
92 pub fn from_vec3(v: [f64; 3]) -> Self {
94 let clean = |x: f64| {
95 if x.is_nan() {
96 0.0
97 } else {
98 x.clamp(-ABS_MAX, ABS_MAX)
99 }
100 };
101 let [x, y, z] = v.map(clean);
102 let len = x.abs().max(y.abs()).max(z.abs());
103 if len < ABS_MIN {
104 return Self::ZERO;
105 }
106 let scale = len.ceil() as u64;
108 let partial = scale & 3 != scale;
109 let markers = if partial { scale & 3 | 4 } else { scale };
110 let pack = |c: f64| ((c / scale as f64 * 0.5 + 0.5) * MAX_Q).round() as u64;
112 let packed = markers | pack(x) << 3 | pack(y) << 18 | pack(z) << 33;
113 LpVec3 {
114 packed,
115 scale_high: if partial { (scale >> 2) as u32 } else { 0 },
116 }
117 }
118}
119
120impl Decode for LpVec3 {
121 #[inline]
122 fn decode(r: &mut Reader<'_>) -> DecodeResult<Self> {
123 Self::read(r)
124 }
125}
126
127impl Encode for LpVec3 {
128 #[inline]
129 fn encode(&self, w: &mut BytesMut) -> EncodeResult {
130 self.write(w);
131 Ok(())
132 }
133}
134
135#[cfg(feature = "testgen")]
136impl crate::testgen::Generate for LpVec3 {
137 fn generate(g: &mut crate::testgen::Gen) -> Self {
138 if g.chance(15) {
139 return Self::ZERO;
140 }
141 let mut packed = g.u64() & 0xffff_ffff_ffff;
143 if packed & 0xff == 0 {
144 packed |= 1;
145 }
146 let scale_high = if packed & 4 != 0 { g.u64() as u32 } else { 0 };
147 LpVec3 { packed, scale_high }
148 }
149}
150
151#[cfg(test)]
152mod tests {
153 use super::*;
154 use crate::codec::decode_exact;
155
156 fn rt(v: LpVec3) -> Vec<u8> {
157 let mut w = BytesMut::new();
158 v.write(&mut w);
159 assert_eq!(decode_exact::<LpVec3>(&w.clone().freeze()).unwrap(), v);
160 w.to_vec()
161 }
162
163 #[test]
164 fn zero_is_one_byte() {
165 assert_eq!(rt(LpVec3::from_vec3([0.0, 1e-6, -1e-6])), vec![0]);
166 assert_eq!(LpVec3::ZERO.to_vec3(), [0.0; 3]);
167 }
168
169 #[test]
170 fn quantisation() {
171 let v = LpVec3::from_vec3([0.0, -0.0784000015258789, 0.0]);
173 assert_eq!(v.scale(), 1);
174 assert_eq!(rt(v).len(), 6);
175 let back = v.to_vec3();
176 assert!((back[1] + 0.0784).abs() < 1.0 / 16383.0);
177 assert!(back[0].abs() < 1e-4 && back[2].abs() < 1e-4);
178 let big = LpVec3::from_vec3([100.0, -3.0, 0.5]);
180 assert_eq!(big.scale(), 100);
181 assert_eq!(big.packed & 4, 4);
182 assert_eq!(rt(big).len(), 7);
183 assert!((big.to_vec3()[0] - 100.0).abs() < 0.01);
184 assert_eq!(LpVec3::from_vec3([f64::NAN; 3]), LpVec3::ZERO);
186 }
187}