Physics (#11)

* Put physics module in azalea-entity

* port aabb

* add more stuff to PositionXYZ

* azalea-physics

* important collision things

* more physics stuff

* backup because i'm about to delete shapes

* more shape stuff

* CubeVoxelShape

* no compile errors???

insane

* impl VoxelShape for ArrayVoxelShape

* Shapes stuff

* collide_x but it doesn't work yet

* binary_search

* it compiles

* Entity has bounding box

* Update discrete_voxel_shape.rs

* Entity::make_bounding_box

* ok i'm about to merge az-entity and az-world

might be a terrible idea which is why i'm committing first

* ok so i moved entity to world

* on_pos and move_entity compiles

* add send_position

* move collision stuff to collision module in az-physics

* dimension is no longer an Option

* start trying to do collision for the client

* collision works :tada:

* start adding palette resizing

* get_and_set (pain)

* it compiles but probably won't work

* add a test

* remove printlns

* add more tests for palette stuff

* ClientboundMoveVec3Packet -> ClientboundMoveEntityPosPacket

i think i changed this on accident once

* palette resizing works

todo: remove the printlns

* Remove printlns in palette.rs

* fix issues from merge

* fixes + work a bit more on physics

* Better entities (#19)

* well it compiles

* add tests to entity storage

* add suggestions in azalea-brigadier

* this probably causes ub

* fix brigadiersuggestions

* get rid of entityid

* test From<EntityMut> for EntityRef

* don't mention other libraries since there's too many

* fix warnings

* do todos in brigadier suggestions

* work on physics

* more physics stuff

* remove trait feature on az-block

i think rust gets confused and compiles the macro without the feature

* bump ahash

* aes tests in az-crypto

* optimize aes's deps

* fix crashes

* fix section_index for negative numbers and test

* fix BlockPos protocol implementation

* remove some debug prints

* prepare to add ai_step

* make ai step work

* clippy

3 files changed, 487 insertions, 0 deletions

diff --git a/azalea-world/src/entity/data.rs b/azalea-world/src/entity/data.rs
new file mode 100644
index 00000000..ff708653
--- /dev/null
+++ b/azalea-world/src/entity/data.rs
@@ -0,0 +1,148 @@
+use azalea_buf::{BufReadError, McBufVarReadable};
+use azalea_buf::{McBuf, McBufReadable, McBufWritable};
+use azalea_chat::component::Component;
+use azalea_core::{BlockPos, Direction, Particle, Slot};
+use std::io::{Read, Write};
+use uuid::Uuid;
+
+#[derive(Clone, Debug)]
+pub struct EntityMetadata(Vec<EntityDataItem>);
+
+#[derive(Clone, Debug)]
+pub struct EntityDataItem {
+    // we can't identify what the index is for here because we don't know the
+    // entity type
+    pub index: u8,
+    pub value: EntityDataValue,
+}
+
+impl McBufReadable for EntityMetadata {
+    fn read_from(buf: &mut impl Read) -> Result<Self, BufReadError> {
+        let mut metadata = Vec::new();
+        loop {
+            let index = u8::read_from(buf)?;
+            if index == 0xff {
+                break;
+            }
+            let value = EntityDataValue::read_from(buf)?;
+            metadata.push(EntityDataItem { index, value });
+        }
+        Ok(EntityMetadata(metadata))
+    }
+}
+
+impl McBufWritable for EntityMetadata {
+    fn write_into(&self, buf: &mut impl Write) -> Result<(), std::io::Error> {
+        for item in &self.0 {
+            item.index.write_into(buf)?;
+            item.value.write_into(buf)?;
+        }
+        0xffu8.write_into(buf)?;
+        Ok(())
+    }
+}
+
+#[derive(Clone, Debug)]
+pub enum EntityDataValue {
+    Byte(u8),
+    // varint
+    Int(i32),
+    Float(f32),
+    String(String),
+    Component(Component),
+    OptionalComponent(Option<Component>),
+    ItemStack(Slot),
+    Boolean(bool),
+    Rotations { x: f32, y: f32, z: f32 },
+    BlockPos(BlockPos),
+    OptionalBlockPos(Option<BlockPos>),
+    Direction(Direction),
+    OptionalUuid(Option<Uuid>),
+    // 0 for absent (implies air); otherwise, a block state ID as per the global palette
+    // this is a varint
+    OptionalBlockState(Option<i32>),
+    CompoundTag(azalea_nbt::Tag),
+    Particle(Particle),
+    VillagerData(VillagerData),
+    // 0 for absent; 1 + actual value otherwise. Used for entity IDs.
+    OptionalUnsignedInt(Option<u32>),
+    Pose(Pose),
+}
+
+impl McBufReadable for EntityDataValue {
+    fn read_from(buf: &mut impl Read) -> Result<Self, BufReadError> {
+        let data_type = u32::var_read_from(buf)?;
+        Ok(match data_type {
+            0 => EntityDataValue::Byte(u8::read_from(buf)?),
+            1 => EntityDataValue::Int(i32::var_read_from(buf)?),
+            2 => EntityDataValue::Float(f32::read_from(buf)?),
+            3 => EntityDataValue::String(String::read_from(buf)?),
+            4 => EntityDataValue::Component(Component::read_from(buf)?),
+            5 => EntityDataValue::OptionalComponent(Option::<Component>::read_from(buf)?),
+            6 => EntityDataValue::ItemStack(Slot::read_from(buf)?),
+            7 => EntityDataValue::Boolean(bool::read_from(buf)?),
+            8 => EntityDataValue::Rotations {
+                x: f32::read_from(buf)?,
+                y: f32::read_from(buf)?,
+                z: f32::read_from(buf)?,
+            },
+            9 => EntityDataValue::BlockPos(BlockPos::read_from(buf)?),
+            10 => EntityDataValue::OptionalBlockPos(Option::<BlockPos>::read_from(buf)?),
+            11 => EntityDataValue::Direction(Direction::read_from(buf)?),
+            12 => EntityDataValue::OptionalUuid(Option::<Uuid>::read_from(buf)?),
+            13 => EntityDataValue::OptionalBlockState({
+                let val = i32::read_from(buf)?;
+                if val == 0 {
+                    None
+                } else {
+                    Some(val)
+                }
+            }),
+            14 => EntityDataValue::CompoundTag(azalea_nbt::Tag::read_from(buf)?),
+            15 => EntityDataValue::Particle(Particle::read_from(buf)?),
+            16 => EntityDataValue::VillagerData(VillagerData::read_from(buf)?),
+            17 => EntityDataValue::OptionalUnsignedInt({
+                let val = u32::var_read_from(buf)?;
+                if val == 0 {
+                    None
+                } else {
+                    Some((val - 1) as u32)
+                }
+            }),
+            18 => EntityDataValue::Pose(Pose::read_from(buf)?),
+            _ => {
+                return Err(BufReadError::UnexpectedEnumVariant {
+                    id: data_type as i32,
+                })
+            }
+        })
+    }
+}
+
+impl McBufWritable for EntityDataValue {
+    fn write_into(&self, _buf: &mut impl Write) -> Result<(), std::io::Error> {
+        todo!();
+    }
+}
+
+#[derive(Clone, Debug, Copy, McBuf)]
+pub enum Pose {
+    Standing = 0,
+    FallFlying = 1,
+    Sleeping = 2,
+    Swimming = 3,
+    SpinAttack = 4,
+    Sneaking = 5,
+    LongJumping = 6,
+    Dying = 7,
+}
+
+#[derive(Debug, Clone, McBuf)]
+pub struct VillagerData {
+    #[var]
+    type_: u32,
+    #[var]
+    profession: u32,
+    #[var]
+    level: u32,
+}
diff --git a/azalea-world/src/entity/dimensions.rs b/azalea-world/src/entity/dimensions.rs
new file mode 100644
index 00000000..1d013d10
--- /dev/null
+++ b/azalea-world/src/entity/dimensions.rs
@@ -0,0 +1,23 @@
+use azalea_core::{Vec3, AABB};
+
+#[derive(Debug, Default)]
+pub struct EntityDimensions {
+    pub width: f32,
+    pub height: f32,
+}
+
+impl EntityDimensions {
+    pub fn make_bounding_box(&self, pos: &Vec3) -> AABB {
+        let radius = (self.width / 2.0) as f64;
+        let height = self.height as f64;
+        AABB {
+            min_x: pos.x - radius,
+            min_y: pos.y,
+            min_z: pos.z - radius,
+
+            max_x: pos.x + radius,
+            max_y: pos.y + height,
+            max_z: pos.z + radius,
+        }
+    }
+}
diff --git a/azalea-world/src/entity/mod.rs b/azalea-world/src/entity/mod.rs
new file mode 100644
index 00000000..37321e0a
--- /dev/null
+++ b/azalea-world/src/entity/mod.rs
@@ -0,0 +1,316 @@
+mod data;
+mod dimensions;
+
+use crate::Dimension;
+use azalea_core::{BlockPos, Vec3, AABB};
+pub use data::*;
+pub use dimensions::*;
+use std::ops::{Deref, DerefMut};
+use std::ptr::NonNull;
+use uuid::Uuid;
+
+#[derive(Debug)]
+pub struct EntityRef<'d> {
+    /// The dimension this entity is in.
+    pub dimension: &'d Dimension,
+    /// The incrementing numerical id of the entity.
+    pub id: u32,
+    pub data: &'d EntityData,
+}
+
+impl<'d> EntityRef<'d> {
+    pub fn new(dimension: &'d Dimension, id: u32, data: &'d EntityData) -> Self {
+        // TODO: have this be based on the entity type
+        Self {
+            dimension,
+            id,
+            data,
+        }
+    }
+}
+
+impl<'d> EntityRef<'d> {
+    #[inline]
+    pub fn pos(&self) -> &Vec3 {
+        &self.pos
+    }
+
+    pub fn make_bounding_box(&self) -> AABB {
+        self.dimensions.make_bounding_box(&self.pos())
+    }
+
+    /// Get the position of the block below the entity, but a little lower.
+    pub fn on_pos_legacy(&self) -> BlockPos {
+        self.on_pos(0.2)
+    }
+
+    // int x = Mth.floor(this.position.x);
+    // int y = Mth.floor(this.position.y - (double)var1);
+    // int z = Mth.floor(this.position.z);
+    // BlockPos var5 = new BlockPos(x, y, z);
+    // if (this.level.getBlockState(var5).isAir()) {
+    //    BlockPos var6 = var5.below();
+    //    BlockState var7 = this.level.getBlockState(var6);
+    //    if (var7.is(BlockTags.FENCES) || var7.is(BlockTags.WALLS) || var7.getBlock() instanceof FenceGateBlock) {
+    //       return var6;
+    //    }
+    // }
+    // return var5;
+    pub fn on_pos(&self, offset: f32) -> BlockPos {
+        let x = self.pos().x.floor() as i32;
+        let y = (self.pos().y - offset as f64).floor() as i32;
+        let z = self.pos().z.floor() as i32;
+        let pos = BlockPos { x, y, z };
+
+        // TODO: check if block below is a fence, wall, or fence gate
+        // let block_pos = pos.below();
+        // let block_state = dimension.get_block_state(&block_pos);
+        // if block_state == Some(BlockState::Air) {
+        //     let block_pos_below = block_pos.below();
+        //     let block_state_below = dimension.get_block_state(&block_pos_below);
+        //     if let Some(block_state_below) = block_state_below {
+        //         if block_state_below.is_fence()
+        //             || block_state_below.is_wall()
+        //             || block_state_below.is_fence_gate()
+        //         {
+        //             return block_pos_below;
+        //         }
+        //     }
+        // }
+
+        pos
+    }
+}
+
+#[derive(Debug)]
+pub struct EntityMut<'d> {
+    /// The dimension this entity is in.
+    pub dimension: &'d mut Dimension,
+    /// The incrementing numerical id of the entity.
+    pub id: u32,
+    pub data: NonNull<EntityData>,
+}
+
+impl<'d> EntityMut<'d> {
+    pub fn new(dimension: &'d mut Dimension, id: u32, data: NonNull<EntityData>) -> Self {
+        Self {
+            dimension,
+            id,
+            data,
+        }
+    }
+
+    /// Sets the position of the entity. This doesn't update the cache in
+    /// azalea-world, and should only be used within azalea-world!
+    pub unsafe fn move_unchecked(&mut self, new_pos: Vec3) {
+        self.pos = new_pos;
+        let bounding_box = self.make_bounding_box();
+        self.bounding_box = bounding_box;
+    }
+
+    pub fn set_rotation(&mut self, y_rot: f32, x_rot: f32) {
+        self.y_rot = y_rot.clamp(-90.0, 90.0) % 360.0;
+        self.x_rot = x_rot % 360.0;
+        // TODO: minecraft also sets yRotO and xRotO to xRot and yRot ... but idk what they're used for so
+    }
+
+    pub fn move_relative(&mut self, speed: f32, acceleration: &Vec3) {
+        let input_vector = self.input_vector(speed, acceleration);
+        self.delta += input_vector;
+    }
+
+    pub fn input_vector(&self, speed: f32, acceleration: &Vec3) -> Vec3 {
+        let distance = acceleration.length_squared();
+        if distance < 1.0E-7 {
+            return Vec3::default();
+        }
+        let acceleration = if distance > 1.0 {
+            acceleration.normalize()
+        } else {
+            *acceleration
+        }
+        .scale(speed as f64);
+        let y_rot = f32::sin(self.y_rot * 0.017453292f32);
+        let x_rot = f32::cos(self.y_rot * 0.017453292f32);
+        Vec3 {
+            x: acceleration.x * (x_rot as f64) - acceleration.z * (y_rot as f64),
+            y: acceleration.y,
+            z: acceleration.z * (x_rot as f64) + acceleration.x * (y_rot as f64),
+        }
+    }
+}
+
+impl<'d> EntityMut<'d> {
+    #[inline]
+    pub fn pos(&self) -> &Vec3 {
+        &self.pos
+    }
+
+    pub fn make_bounding_box(&self) -> AABB {
+        self.dimensions.make_bounding_box(&self.pos())
+    }
+
+    /// Get the position of the block below the entity, but a little lower.
+    pub fn on_pos_legacy(&self) -> BlockPos {
+        self.on_pos(0.2)
+    }
+
+    // int x = Mth.floor(this.position.x);
+    // int y = Mth.floor(this.position.y - (double)var1);
+    // int z = Mth.floor(this.position.z);
+    // BlockPos var5 = new BlockPos(x, y, z);
+    // if (this.level.getBlockState(var5).isAir()) {
+    //    BlockPos var6 = var5.below();
+    //    BlockState var7 = this.level.getBlockState(var6);
+    //    if (var7.is(BlockTags.FENCES) || var7.is(BlockTags.WALLS) || var7.getBlock() instanceof FenceGateBlock) {
+    //       return var6;
+    //    }
+    // }
+    // return var5;
+    pub fn on_pos(&self, offset: f32) -> BlockPos {
+        let x = self.pos().x.floor() as i32;
+        let y = (self.pos().y - offset as f64).floor() as i32;
+        let z = self.pos().z.floor() as i32;
+        let pos = BlockPos { x, y, z };
+
+        // TODO: check if block below is a fence, wall, or fence gate
+        // let block_pos = pos.below();
+        // let block_state = dimension.get_block_state(&block_pos);
+        // if block_state == Some(BlockState::Air) {
+        //     let block_pos_below = block_pos.below();
+        //     let block_state_below = dimension.get_block_state(&block_pos_below);
+        //     if let Some(block_state_below) = block_state_below {
+        //         if block_state_below.is_fence()
+        //             || block_state_below.is_wall()
+        //             || block_state_below.is_fence_gate()
+        //         {
+        //             return block_pos_below;
+        //         }
+        //     }
+        // }
+
+        pos
+    }
+}
+
+impl<'d> From<EntityMut<'d>> for EntityRef<'d> {
+    fn from(entity: EntityMut<'d>) -> EntityRef<'d> {
+        let data = unsafe { entity.data.as_ref() };
+        EntityRef {
+            dimension: entity.dimension,
+            id: entity.id,
+            data,
+        }
+    }
+}
+
+impl Deref for EntityMut<'_> {
+    type Target = EntityData;
+
+    fn deref(&self) -> &Self::Target {
+        unsafe { self.data.as_ref() }
+    }
+}
+
+impl DerefMut for EntityMut<'_> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        unsafe { self.data.as_mut() }
+    }
+}
+
+impl Deref for EntityRef<'_> {
+    type Target = EntityData;
+
+    fn deref(&self) -> &Self::Target {
+        self.data
+    }
+}
+
+#[derive(Debug)]
+pub struct EntityData {
+    pub uuid: Uuid,
+    /// The position of the entity right now.
+    /// This can be changde with unsafe_move, but the correct way is with dimension.move_entity
+    pos: Vec3,
+    /// The position of the entity last tick.
+    pub last_pos: Vec3,
+    pub delta: Vec3,
+
+    /// X acceleration.
+    pub xxa: f32,
+    /// Y acceleration.
+    pub yya: f32,
+    /// Z acceleration.
+    pub zza: f32,
+
+    pub x_rot: f32,
+    pub y_rot: f32,
+
+    pub x_rot_last: f32,
+    pub y_rot_last: f32,
+
+    pub on_ground: bool,
+    pub last_on_ground: bool,
+
+    /// The width and height of the entity.
+    pub dimensions: EntityDimensions,
+    /// The bounding box of the entity. This is more than just width and height, unlike dimensions.
+    pub bounding_box: AABB,
+}
+
+impl EntityData {
+    pub fn new(uuid: Uuid, pos: Vec3) -> Self {
+        let dimensions = EntityDimensions {
+            width: 0.8,
+            height: 1.8,
+        };
+
+        Self {
+            uuid,
+            pos,
+            last_pos: pos,
+            delta: Vec3::default(),
+
+            xxa: 0.,
+            yya: 0.,
+            zza: 0.,
+
+            x_rot: 0.,
+            y_rot: 0.,
+
+            y_rot_last: 0.,
+            x_rot_last: 0.,
+
+            on_ground: false,
+            last_on_ground: false,
+
+            // TODO: have this be based on the entity type
+            bounding_box: dimensions.make_bounding_box(&pos),
+            dimensions,
+        }
+    }
+
+    #[inline]
+    pub fn pos(&self) -> &Vec3 {
+        &self.pos
+    }
+
+    pub(crate) unsafe fn as_ptr(&mut self) -> NonNull<EntityData> {
+        NonNull::new_unchecked(self as *mut EntityData)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn from_mut_entity_to_ref_entity() {
+        let mut dim = Dimension::default();
+        let uuid = Uuid::from_u128(100);
+        dim.add_entity(0, EntityData::new(uuid, Vec3::default()));
+        let entity: EntityMut = dim.entity_mut(0).unwrap();
+        let entity_ref: EntityRef = entity.into();
+        assert_eq!(entity_ref.uuid, uuid);
+    }
+}