214 lines
6.7 KiB
Rust
214 lines
6.7 KiB
Rust
use std::fmt::Formatter;
|
|
|
|
use pumpkin_util::math::vector3::Vector3;
|
|
|
|
use crate::{
|
|
VarInt,
|
|
ser::{NetworkWriteExt, ReadingError, WritingError},
|
|
};
|
|
use serde::{
|
|
Deserializer, Serialize,
|
|
de::{self, Visitor},
|
|
ser::Serializer,
|
|
};
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub struct LpVector3d(pub Vector3<f64>);
|
|
|
|
impl LpVector3d {
|
|
pub fn write<W: std::io::Write>(&self, writer: &mut W) -> Result<(), WritingError> {
|
|
let velocity = self.0;
|
|
|
|
// Clamp and find the maximum component magnitude
|
|
let clamped_x = clamp_value(velocity.x);
|
|
let clamped_y = clamp_value(velocity.y);
|
|
let clamped_z = clamp_value(velocity.z);
|
|
let max_component = abs_max(clamped_x, abs_max(clamped_y, clamped_z));
|
|
|
|
if max_component < MIN_VELOCITY_MAGNITUDE {
|
|
return writer.write_slice(&[0u8]);
|
|
}
|
|
|
|
let scale_factor = max_component.ceil() as i64;
|
|
let is_extended = scale_factor > 3;
|
|
|
|
// The header byte: bits 0-1 are scale, bit 2 is the extension flag
|
|
let header = if is_extended {
|
|
(scale_factor & 3) | 4
|
|
} else {
|
|
scale_factor
|
|
};
|
|
|
|
// Pack the 15-bit quantized components into a 64-bit buffer
|
|
// Quantized components: x (bits 3-17), y (bits 18-32), z (bits 33-47)
|
|
let quantized_x = to_long(clamped_x / scale_factor as f64) << 3;
|
|
let quantized_y = to_long(clamped_y / scale_factor as f64) << 18;
|
|
let quantized_z = to_long(clamped_z / scale_factor as f64) << 33;
|
|
|
|
let packed_data: i64 = header | quantized_x | quantized_y | quantized_z;
|
|
|
|
// Write low 16 bits (Little Endian)
|
|
writer
|
|
.write_all(&(packed_data as u16).to_le_bytes())
|
|
.map_err(WritingError::IoError)?;
|
|
|
|
// Write next 32 bits (Big Endian)
|
|
writer
|
|
.write_all(&((packed_data >> 16) as i32).to_be_bytes())
|
|
.map_err(WritingError::IoError)?;
|
|
|
|
if is_extended {
|
|
let scale_tail = VarInt((scale_factor >> 2) as i32);
|
|
writer.write_var_int(&scale_tail)?;
|
|
}
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn read<R: std::io::Read>(reader: &mut R) -> Result<Self, ReadingError> {
|
|
let mut low_16 = [0u8; 2];
|
|
reader
|
|
.read_exact(&mut low_16)
|
|
.map_err(|e| ReadingError::Message(e.to_string()))?;
|
|
|
|
if low_16[0] == 0 && low_16[1] == 0 {
|
|
return Ok(Self(Vector3::new(0.0, 0.0, 0.0)));
|
|
}
|
|
|
|
let mut mid_32 = [0u8; 4];
|
|
reader
|
|
.read_exact(&mut mid_32)
|
|
.map_err(|e| ReadingError::Message(e.to_string()))?;
|
|
|
|
let low = u16::from_le_bytes(low_16) as i64;
|
|
let mid = i32::from_be_bytes(mid_32) as i64;
|
|
let packed_data = low | (mid << 16);
|
|
|
|
let header = packed_data & 0x07;
|
|
let is_extended = (header & 4) != 0;
|
|
|
|
let scale_factor = if is_extended {
|
|
let scale_tail = VarInt::decode(reader)?;
|
|
((scale_tail.0 as i64) << 2) | (header & 3)
|
|
} else {
|
|
header & 3
|
|
};
|
|
|
|
if scale_factor == 0 && !is_extended {
|
|
return Ok(Self(Vector3::new(0.0, 0.0, 0.0)));
|
|
}
|
|
|
|
let q_x = (packed_data >> 3) & 0x7FFF;
|
|
let q_y = (packed_data >> 18) & 0x7FFF;
|
|
let q_z = (packed_data >> 33) & 0x7FFF;
|
|
|
|
let scale = scale_factor as f64;
|
|
|
|
Ok(Self(Vector3::new(
|
|
from_long(q_x, scale),
|
|
from_long(q_y, scale),
|
|
from_long(q_z, scale),
|
|
)))
|
|
}
|
|
|
|
pub fn write_legacy<W: std::io::Write>(&self, writer: &mut W) -> Result<(), WritingError> {
|
|
writer.write_i16_be(encode_legacy_velocity_component(self.0.x))?;
|
|
writer.write_i16_be(encode_legacy_velocity_component(self.0.y))?;
|
|
writer.write_i16_be(encode_legacy_velocity_component(self.0.z))?;
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
const MAX_VELOCITY_CLAMP: f64 = 1.717_986_918_3E10;
|
|
const LEGACY_COMPONENT_CLAMP: f64 = 3.9;
|
|
const LEGACY_COMPONENT_SCALE: f64 = 8000.0;
|
|
const MIN_VELOCITY_MAGNITUDE: f64 = 3.051_944_088_384_301E-5;
|
|
const MAX_15_BIT_VALUE: f64 = 32766.0;
|
|
|
|
#[must_use]
|
|
pub fn encode_legacy_velocity_component(component: f64) -> i16 {
|
|
// Legacy clients (<= 1.21.8 / protocol 772) encode velocity as clamped component * 8000.
|
|
(component.clamp(-LEGACY_COMPONENT_CLAMP, LEGACY_COMPONENT_CLAMP) * LEGACY_COMPONENT_SCALE)
|
|
as i16
|
|
}
|
|
|
|
fn clamp_value(value: f64) -> f64 {
|
|
if value.is_nan() {
|
|
return 0.0;
|
|
}
|
|
value.clamp(-MAX_VELOCITY_CLAMP, MAX_VELOCITY_CLAMP)
|
|
}
|
|
|
|
const fn abs_max(a: f64, b: f64) -> f64 {
|
|
a.abs().max(b.abs())
|
|
}
|
|
|
|
fn to_long(value: f64) -> i64 {
|
|
((value.mul_add(0.5, 0.5) * MAX_15_BIT_VALUE).round() as i64).clamp(0, 32766)
|
|
}
|
|
|
|
fn from_long(quantized: i64, scale: f64) -> f64 {
|
|
// Reverse: ((v * 0.5 + 0.5) * 32766) -> v
|
|
let normalized = (quantized as f64 / MAX_15_BIT_VALUE) - 0.5;
|
|
(normalized / 0.5) * scale
|
|
}
|
|
|
|
impl Serialize for LpVector3d {
|
|
fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
|
|
let mut buf = Vec::new();
|
|
self.write(&mut buf)
|
|
.map_err(|e| serde::ser::Error::custom(e.to_string()))?;
|
|
serializer.serialize_bytes(&buf)
|
|
}
|
|
}
|
|
|
|
impl<'de> de::Deserialize<'de> for LpVector3d {
|
|
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
|
|
where
|
|
D: Deserializer<'de>,
|
|
{
|
|
struct VelocityVisitor;
|
|
|
|
impl Visitor<'_> for VelocityVisitor {
|
|
type Value = LpVector3d;
|
|
|
|
fn expecting(&self, formatter: &mut Formatter) -> std::fmt::Result {
|
|
formatter.write_str("a byte array representing bit-packed velocity")
|
|
}
|
|
|
|
fn visit_bytes<E>(self, v: &[u8]) -> Result<Self::Value, E>
|
|
where
|
|
E: de::Error,
|
|
{
|
|
let mut cursor = std::io::Cursor::new(v);
|
|
LpVector3d::read(&mut cursor).map_err(de::Error::custom)
|
|
}
|
|
}
|
|
|
|
deserializer.deserialize_bytes(VelocityVisitor)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::{LpVector3d, encode_legacy_velocity_component};
|
|
use pumpkin_util::math::vector3::Vector3;
|
|
|
|
#[test]
|
|
fn legacy_component_is_clamped_and_scaled() {
|
|
assert_eq!(encode_legacy_velocity_component(0.5), 4000);
|
|
assert_eq!(encode_legacy_velocity_component(-0.5), -4000);
|
|
assert_eq!(encode_legacy_velocity_component(4.0), 31200);
|
|
assert_eq!(encode_legacy_velocity_component(-4.0), -31200);
|
|
}
|
|
|
|
#[test]
|
|
fn write_legacy_writes_three_i16_be_components() -> Result<(), Box<dyn std::error::Error>> {
|
|
let velocity = LpVector3d(Vector3::new(0.5, -0.5, 0.0));
|
|
let mut buf = Vec::new();
|
|
velocity.write_legacy(&mut buf)?;
|
|
assert_eq!(buf, vec![0x0F, 0xA0, 0xF0, 0x60, 0x00, 0x00]);
|
|
Ok(())
|
|
}
|
|
}
|