75 lines
2.2 KiB
Rust
75 lines
2.2 KiB
Rust
use pumpkin_util::math::vector3::Vector3;
|
|
|
|
use crate::VarInt;
|
|
use serde::{Serialize, ser::Serializer};
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub struct Velocity(pub Vector3<f64>);
|
|
|
|
const MAX_VELOCITY_CLAMP: f64 = 1.7179869183E10;
|
|
const MIN_VELOCITY_MAGNITUDE: f64 = 3.051944088384301E-5;
|
|
const MAX_15_BIT_VALUE: f64 = 32766.0;
|
|
|
|
fn clamp_value(value: f64) -> f64 {
|
|
if value.is_nan() {
|
|
return 0.0;
|
|
}
|
|
value.clamp(-MAX_VELOCITY_CLAMP, MAX_VELOCITY_CLAMP)
|
|
}
|
|
|
|
fn abs_max(a: f64, b: f64) -> f64 {
|
|
a.abs().max(b.abs())
|
|
}
|
|
|
|
fn to_long(value: f64) -> i64 {
|
|
((value * 0.5 + 0.5) * MAX_15_BIT_VALUE).round() as i64
|
|
}
|
|
|
|
impl Serialize for Velocity {
|
|
fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
|
|
let mut buf = Vec::new();
|
|
let velocity = self.0;
|
|
let d = clamp_value(velocity.x);
|
|
let e = clamp_value(velocity.y);
|
|
let f = clamp_value(velocity.z);
|
|
let g = abs_max(d, abs_max(e, f));
|
|
|
|
if g < MIN_VELOCITY_MAGNITUDE {
|
|
buf.push(0u8);
|
|
return serializer.serialize_bytes(&buf);
|
|
}
|
|
|
|
// l is the scale factor
|
|
let l = g.ceil() as i64;
|
|
// bl is true if the scale factor is > 3 and needs VarInt extension
|
|
let bl = l > 3;
|
|
|
|
// m is the first byte of the packed data (low 3 bits of l + 4 if bl is true)
|
|
let m = if bl { l & 3 | 4 } else { l };
|
|
|
|
// Quantize and shift the velocity components (15 bits each)
|
|
let n: i64 = to_long(d / l as f64) << 3; // X shifted 3
|
|
let o: i64 = to_long(e / l as f64) << 18; // Y shifted 18
|
|
let p: i64 = to_long(f / l as f64) << 33; // Z shifted 33
|
|
|
|
// q is the packed 48-bit value (m + X + Y + Z)
|
|
let q: i64 = m | n | o | p;
|
|
|
|
// 1. Write first byte (low 8 bits)
|
|
buf.push(q as u8);
|
|
|
|
// 2. Write second byte (bits 8-15)
|
|
buf.push((q >> 8) as u8);
|
|
|
|
// 3. Write remaining 4 bytes (bits 16-47) as a Big Endian i32 (Java's writeInt)
|
|
buf.extend_from_slice(&((q >> 16) as i32).to_be_bytes());
|
|
|
|
// 4. Write VarInt for scale factor tail if needed
|
|
if bl {
|
|
VarInt::encode(&VarInt((l >> 2) as i32), &mut buf).unwrap();
|
|
}
|
|
|
|
serializer.serialize_bytes(&buf)
|
|
}
|
|
}
|