1use std::io::{self, Cursor, Write};
2
3pub use azalea_buf::AzBuf;
4use azalea_buf::{AzaleaRead, AzaleaReadVar, AzaleaWrite, AzaleaWriteVar, BufReadError};
5
6use crate::{math, position::Vec3};
7
8pub trait PositionDeltaTrait {
9 fn x(&self) -> f64;
10 fn y(&self) -> f64;
11 fn z(&self) -> f64;
12}
13
14#[derive(Clone, Debug, AzBuf, Default, PartialEq)]
16pub struct PositionDelta8 {
17 pub xa: i16,
18 pub ya: i16,
19 pub za: i16,
20}
21
22impl PositionDelta8 {
23 #[deprecated = "Use Self::x, y, z instead"]
24 pub fn float(&self) -> (f64, f64, f64) {
25 (
26 (self.xa as f64) / 4096.0,
27 (self.ya as f64) / 4096.0,
28 (self.za as f64) / 4096.0,
29 )
30 }
31}
32
33impl PositionDeltaTrait for PositionDelta8 {
34 fn x(&self) -> f64 {
35 (self.xa as f64) / 4096.0
36 }
37 fn y(&self) -> f64 {
38 (self.ya as f64) / 4096.0
39 }
40 fn z(&self) -> f64 {
41 (self.za as f64) / 4096.0
42 }
43}
44impl<T: PositionDeltaTrait> From<T> for Vec3 {
45 fn from(value: T) -> Self {
46 Vec3::new(value.x(), value.y(), value.z())
47 }
48}
49
50impl Vec3 {
51 #[must_use]
52 pub fn with_delta(&self, delta: &impl PositionDeltaTrait) -> Vec3 {
53 Vec3 {
54 x: self.x + delta.x(),
55 y: self.y + delta.y(),
56 z: self.z + delta.z(),
57 }
58 }
59
60 pub fn normalize(&self) -> Vec3 {
61 let length = f64::sqrt(self.x * self.x + self.y * self.y + self.z * self.z);
62 if length < 1e-5 {
63 return Vec3::ZERO;
64 }
65 Vec3 {
66 x: self.x / length,
67 y: self.y / length,
68 z: self.z / length,
69 }
70 }
71
72 pub fn multiply(&self, x: f64, y: f64, z: f64) -> Vec3 {
73 Vec3 {
74 x: self.x * x,
75 y: self.y * y,
76 z: self.z * z,
77 }
78 }
79 pub fn scale(&self, amount: f64) -> Vec3 {
80 self.multiply(amount, amount, amount)
81 }
82}
83
84#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
89pub enum LpVec3 {
90 #[default]
91 Zero,
92 Normal {
93 a: u8,
94 b: u8,
95 c: u32,
96 },
97 Extended {
98 a: u8,
99 b: u8,
100 c: u32,
101 d: u32,
102 },
103}
104
105impl AzaleaRead for LpVec3 {
106 fn azalea_read(buf: &mut Cursor<&[u8]>) -> Result<Self, BufReadError> {
107 let a = u8::azalea_read(buf)?;
108 if a == 0 {
109 return Ok(LpVec3::Zero);
110 }
111 let b = u8::azalea_read(buf)?;
112 let c = u32::azalea_read(buf)?;
113 if a & 4 == 4 {
114 let d = u32::azalea_read_var(buf)?;
115 Ok(LpVec3::Extended { a, b, c, d })
116 } else {
117 Ok(LpVec3::Normal { a, b, c })
118 }
119 }
120}
121impl AzaleaWrite for LpVec3 {
122 fn azalea_write(&self, buf: &mut impl Write) -> io::Result<()> {
123 match self {
124 LpVec3::Zero => {
125 0u8.azalea_write(buf)?;
126 }
127 LpVec3::Normal { a, b, c } => {
128 a.azalea_write(buf)?;
129 b.azalea_write(buf)?;
130 c.azalea_write(buf)?;
131 }
132 LpVec3::Extended { a, b, c, d } => {
133 a.azalea_write(buf)?;
134 b.azalea_write(buf)?;
135 c.azalea_write(buf)?;
136 d.azalea_write_var(buf)?;
137 }
138 }
139 Ok(())
140 }
141}
142impl LpVec3 {
143 pub fn from_vec3(vec3: Vec3) -> Self {
144 let x = Self::sanitize(vec3.x);
145 let y = Self::sanitize(vec3.y);
146 let z = Self::sanitize(vec3.z);
147 let max = x.abs().max(y.abs()).max(z.abs());
148 if max < 3.051944088384301E-5 {
149 return LpVec3::Zero;
150 }
151
152 let divisor = math::ceil_long(max);
153 let is_extended = divisor & 3 != divisor;
154 let packed_divisor = if is_extended {
155 (divisor as u64 & 3) | 4
156 } else {
157 divisor as u64
158 };
159 let packed_x = Self::pack(x / (divisor as f64)) << 3;
160 let packed_y = Self::pack(y / (divisor as f64)) << 18;
161 let packed_z = Self::pack(z / (divisor as f64)) << 33;
162 let packed = packed_divisor | packed_x | packed_y | packed_z;
163
164 let a = packed as u8;
165 let b = (packed >> 8) as u8;
166 let c = (packed >> 16) as u32;
167
168 if is_extended {
169 let d = ((divisor as u64) >> 2) as u32;
170 Self::Extended { a, b, c, d }
171 } else {
172 Self::Normal { a, b, c }
173 }
174 }
175
176 pub fn to_vec3(self) -> Vec3 {
177 match self {
178 LpVec3::Zero => Vec3::ZERO,
179 LpVec3::Normal { a, b, c } => {
180 let packed: u64 = (c as u64) << 16 | (b as u64) << 8 | (a as u64);
181 let multiplier = (a & 3) as u64 as f64;
182
183 Vec3 {
184 x: Self::unpack(packed >> 3) * multiplier,
185 y: Self::unpack(packed >> 18) * multiplier,
186 z: Self::unpack(packed >> 33) * multiplier,
187 }
188 }
189 LpVec3::Extended { a, b, c, d } => {
190 let packed: u64 = (c as u64) << 16 | (b as u64) << 8 | (a as u64);
191 let multiplier = (a & 3) as u64;
192 let multiplier = multiplier | ((d as u64) << 2);
193 let multiplier = multiplier as f64;
194
195 Vec3 {
196 x: Self::unpack(packed >> 3) * multiplier,
197 y: Self::unpack(packed >> 18) * multiplier,
198 z: Self::unpack(packed >> 33) * multiplier,
199 }
200 }
201 }
202 }
203
204 fn unpack(value: u64) -> f64 {
205 f64::min((value & 32767) as f64, 32766.) * 2. / 32766. - 1.
206 }
207
208 fn pack(value: f64) -> u64 {
209 f64::round((value * 0.5 + 0.5) * 32766.) as u64
210 }
211
212 fn sanitize(value: f64) -> f64 {
213 if value.is_nan() {
214 0.
215 } else {
216 f64::clamp(value, -1.7179869183E10, 1.7179869183E10)
217 }
218 }
219}
220impl From<LpVec3> for Vec3 {
221 fn from(value: LpVec3) -> Self {
222 value.to_vec3()
223 }
224}
225impl From<Vec3> for LpVec3 {
226 fn from(value: Vec3) -> Self {
227 LpVec3::from_vec3(value)
228 }
229}
230#[cfg(test)]
231mod tests {
232 use azalea_buf::AzaleaWrite;
233
234 use super::*;
235
236 static TEST_VALUES: [Vec3; 3] = [
237 Vec3::ZERO,
238 Vec3 {
239 x: 1.234,
240 y: -5.678,
241 z: 9.876,
242 },
243 Vec3 {
244 x: 10000000.,
245 y: -5000000.,
246 z: 9876543.,
247 },
248 ];
249
250 #[test]
251 fn test_lpvec3_roundtrip() {
252 fn close_enough(a: f64, b: f64) -> bool {
253 a == b || (a / b - 1.).abs() < 0.01
254 }
255
256 for v in TEST_VALUES {
257 let lp = LpVec3::from_vec3(v);
258 let v2 = lp.to_vec3();
259 assert!(
260 close_enough(v.x, v2.x) && close_enough(v.y, v2.y) && close_enough(v.z, v2.z),
261 "Original: {:?}, Roundtrip: {:?}",
262 v,
263 v2
264 );
265 }
266 }
267
268 #[test]
269 fn test_encode_decode_lpvec3() {
270 for v in TEST_VALUES {
271 let v: LpVec3 = LpVec3::from(v);
272 let mut first_buf = Vec::new();
273 v.azalea_write(&mut first_buf).unwrap();
274 let decoded = LpVec3::azalea_read(&mut Cursor::new(&first_buf)).unwrap();
275 assert_eq!(v, decoded);
276
277 let mut second_buf = Vec::new();
278 LpVec3::from(Vec3::from(decoded))
279 .azalea_write(&mut second_buf)
280 .unwrap();
281
282 assert_eq!(first_buf, second_buf);
283 }
284 }
285}