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

8 files changed, 908 insertions, 60 deletions

diff --git a/azalea-core/src/aabb.rs b/azalea-core/src/aabb.rs
new file mode 100644
index 00000000..40230fe4
--- /dev/null
+++ b/azalea-core/src/aabb.rs
@@ -0,0 +1,447 @@
+use crate::{Axis, BlockHitResult, BlockPos, Direction, PositionXYZ, Vec3};
+
+pub const EPSILON: f64 = 1.0E-7;
+
+/// A rectangular prism with a starting and ending point.
+#[derive(Copy, Clone, Debug, PartialEq, Default)]
+pub struct AABB {
+    pub min_x: f64,
+    pub min_y: f64,
+    pub min_z: f64,
+
+    pub max_x: f64,
+    pub max_y: f64,
+    pub max_z: f64,
+}
+
+impl AABB {
+    pub fn contract(&self, x: f64, y: f64, z: f64) -> AABB {
+        let mut min_x = self.min_x;
+        let mut min_y = self.min_y;
+        let mut min_z = self.min_z;
+
+        let mut max_x = self.max_x;
+        let mut max_y = self.max_y;
+        let mut max_z = self.max_z;
+
+        if x < 0.0 {
+            min_x -= x;
+        } else if x > 0.0 {
+            max_x -= x;
+        }
+
+        if y < 0.0 {
+            min_y -= y;
+        } else if y > 0.0 {
+            max_y -= y;
+        }
+
+        if z < 0.0 {
+            min_z -= z;
+        } else if z > 0.0 {
+            max_z -= z;
+        }
+
+        AABB {
+            min_x,
+            min_y,
+            min_z,
+
+            max_x,
+            max_y,
+            max_z,
+        }
+    }
+
+    pub fn expand_towards(&self, other: &Vec3) -> AABB {
+        let mut min_x = self.min_x;
+        let mut min_y = self.min_y;
+        let mut min_z = self.min_z;
+
+        let mut max_x = self.max_x;
+        let mut max_y = self.max_y;
+        let mut max_z = self.max_z;
+
+        if other.x < 0.0 {
+            min_x += other.x;
+        } else if other.x > 0.0 {
+            max_x += other.x;
+        }
+
+        if other.y < 0.0 {
+            min_y += other.y;
+        } else if other.y > 0.0 {
+            max_y += other.y;
+        }
+
+        if other.z < 0.0 {
+            min_z += other.z;
+        } else if other.z > 0.0 {
+            max_z += other.z;
+        }
+
+        AABB {
+            min_x,
+            min_y,
+            min_z,
+
+            max_x,
+            max_y,
+            max_z,
+        }
+    }
+
+    pub fn inflate(&self, x: f64, y: f64, z: f64) -> AABB {
+        let min_x = self.min_x - x;
+        let min_y = self.min_y - y;
+        let min_z = self.min_z - z;
+
+        let max_x = self.max_x + x;
+        let max_y = self.max_y + y;
+        let max_z = self.max_z + z;
+
+        AABB {
+            min_x,
+            min_y,
+            min_z,
+
+            max_x,
+            max_y,
+            max_z,
+        }
+    }
+
+    pub fn intersect(&self, other: &AABB) -> AABB {
+        let min_x = self.min_x.max(other.min_x);
+        let min_y = self.min_y.max(other.min_y);
+        let min_z = self.min_z.max(other.min_z);
+
+        let max_x = self.max_x.min(other.max_x);
+        let max_y = self.max_y.min(other.max_y);
+        let max_z = self.max_z.min(other.max_z);
+
+        AABB {
+            min_x,
+            min_y,
+            min_z,
+
+            max_x,
+            max_y,
+            max_z,
+        }
+    }
+
+    pub fn minmax(&self, other: &AABB) -> AABB {
+        let min_x = self.min_x.min(other.min_x);
+        let min_y = self.min_y.min(other.min_y);
+        let min_z = self.min_z.min(other.min_z);
+
+        let max_x = self.max_x.max(other.max_x);
+        let max_y = self.max_y.max(other.max_y);
+        let max_z = self.max_z.max(other.max_z);
+
+        AABB {
+            min_x,
+            min_y,
+            min_z,
+
+            max_x,
+            max_y,
+            max_z,
+        }
+    }
+
+    pub fn move_relative(&self, x: f64, y: f64, z: f64) -> AABB {
+        AABB {
+            min_x: self.min_x + x,
+            min_y: self.min_y + y,
+            min_z: self.min_z + z,
+
+            max_x: self.max_x + x,
+            max_y: self.max_y + y,
+            max_z: self.max_z + z,
+        }
+    }
+
+    pub fn intersects_aabb(&self, other: &AABB) -> bool {
+        self.min_x < other.max_x
+            && self.max_x > other.min_x
+            && self.min_y < other.max_y
+            && self.max_y > other.min_y
+            && self.min_z < other.max_z
+            && self.max_z > other.min_z
+    }
+    pub fn intersects_vec3(&self, other: &Vec3, other2: &Vec3) -> bool {
+        self.intersects_aabb(&AABB {
+            min_x: other.x.min(other2.x),
+            min_y: other.y.min(other2.y),
+            min_z: other.z.min(other2.z),
+
+            max_x: other.x.max(other2.x),
+            max_y: other.y.max(other2.y),
+            max_z: other.z.max(other2.z),
+        })
+    }
+
+    pub fn contains(&self, x: f64, y: f64, z: f64) -> bool {
+        x >= self.min_x
+            && x < self.max_x
+            && y >= self.min_y
+            && y < self.max_y
+            && z >= self.min_z
+            && z < self.max_z
+    }
+
+    pub fn size(&self) -> f64 {
+        let x = self.get_size(Axis::X);
+        let y = self.get_size(Axis::Y);
+        let z = self.get_size(Axis::Z);
+        (x + y + z) / 3.0
+    }
+
+    pub fn get_size(&self, axis: Axis) -> f64 {
+        axis.choose(
+            self.max_x - self.min_x,
+            self.max_y - self.min_y,
+            self.max_z - self.min_z,
+        )
+    }
+
+    pub fn deflate(&mut self, x: f64, y: f64, z: f64) -> AABB {
+        self.inflate(-x, -y, -z)
+    }
+
+    pub fn clip(&self, min: &Vec3, max: &Vec3) -> Option<Vec3> {
+        let mut t = [1.0];
+        let x = max.x - min.x;
+        let y = max.y - min.y;
+        let z = max.z - min.z;
+        let dir = self.get_direction(self, min, &mut t, None, &Vec3 { x, y, z });
+        if dir.is_none() {
+            return None;
+        }
+        let t = t[0];
+        Some(min.add(t * x, t * y, t * z))
+    }
+
+    pub fn clip_iterable(
+        &self,
+        boxes: &Vec<AABB>,
+        from: &Vec3,
+        to: &Vec3,
+        pos: &BlockPos,
+    ) -> Option<BlockHitResult> {
+        let mut t = [1.0];
+        let mut dir = None;
+        let x = to.x - from.x;
+        let y = to.y - from.y;
+        let z = to.z - from.z;
+
+        for aabb in boxes {
+            dir = self.get_direction(aabb, from, &mut t, dir, &Vec3 { x, y, z });
+        }
+        if dir.is_none() {
+            return None;
+        }
+        let t = t[0];
+        Some(BlockHitResult {
+            location: from.add(t * x, t * y, t * z),
+            direction: dir.unwrap(),
+            block_pos: *pos,
+            inside: false,
+            miss: false,
+        })
+    }
+
+    fn get_direction(
+        &self,
+        aabb: &AABB,
+        from: &Vec3,
+        t: &mut [f64],
+        dir: Option<Direction>,
+        delta: &Vec3,
+    ) -> Option<Direction> {
+        if delta.x > EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                delta,
+                aabb.min_x,
+                aabb.min_y,
+                aabb.max_y,
+                aabb.min_z,
+                aabb.max_z,
+                Direction::West,
+                from,
+            );
+        } else if delta.x < -EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                delta,
+                aabb.max_x,
+                aabb.min_y,
+                aabb.max_y,
+                aabb.min_z,
+                aabb.max_z,
+                Direction::East,
+                from,
+            );
+        }
+
+        if delta.y > EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                &Vec3 {
+                    x: delta.y,
+                    y: delta.z,
+                    z: delta.x,
+                },
+                aabb.min_y,
+                aabb.min_z,
+                aabb.max_z,
+                aabb.min_x,
+                aabb.max_x,
+                Direction::Down,
+                &Vec3 {
+                    x: from.y,
+                    y: from.z,
+                    z: from.x,
+                },
+            );
+        } else if delta.y < -EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                &Vec3 {
+                    x: delta.y,
+                    y: delta.z,
+                    z: delta.x,
+                },
+                aabb.max_y,
+                aabb.min_z,
+                aabb.max_z,
+                aabb.min_x,
+                aabb.max_x,
+                Direction::Up,
+                &Vec3 {
+                    x: from.y,
+                    y: from.z,
+                    z: from.x,
+                },
+            );
+        }
+
+        if delta.z > EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                &Vec3 {
+                    x: delta.z,
+                    y: delta.x,
+                    z: delta.y,
+                },
+                aabb.min_z,
+                aabb.min_x,
+                aabb.max_x,
+                aabb.min_y,
+                aabb.max_y,
+                Direction::North,
+                &Vec3 {
+                    x: from.z,
+                    y: from.x,
+                    z: from.y,
+                },
+            );
+        } else if delta.z < -EPSILON {
+            return self.clip_point(
+                t,
+                dir,
+                &Vec3 {
+                    x: delta.z,
+                    y: delta.x,
+                    z: delta.y,
+                },
+                aabb.max_z,
+                aabb.min_x,
+                aabb.max_x,
+                aabb.min_y,
+                aabb.max_y,
+                Direction::South,
+                &Vec3 {
+                    x: from.z,
+                    y: from.x,
+                    z: from.y,
+                },
+            );
+        }
+
+        dir
+    }
+
+    fn clip_point(
+        &self,
+        t: &mut [f64],
+        approach_dir: Option<Direction>,
+        delta: &Vec3,
+        begin: f64,
+        min_x: f64,
+        max_x: f64,
+        min_z: f64,
+        max_z: f64,
+        result_dir: Direction,
+        start: &Vec3,
+    ) -> Option<Direction> {
+        let t_x = (begin - start.x) / delta.x;
+        let t_y = (start.y + t_x) / delta.y;
+        let t_z = (start.z + t_x) / delta.z;
+        if 0.0 < t_x
+            && t_x < t[0]
+            && min_x - EPSILON < t_y
+            && t_y < max_x + EPSILON
+            && min_z - EPSILON < t_z
+            && t_z < max_z + EPSILON
+        {
+            t[0] = t_x;
+            Some(result_dir)
+        } else {
+            approach_dir
+        }
+    }
+
+    pub fn has_nan(&self) -> bool {
+        self.min_x.is_nan()
+            || self.min_y.is_nan()
+            || self.min_z.is_nan()
+            || self.max_x.is_nan()
+            || self.max_y.is_nan()
+            || self.max_z.is_nan()
+    }
+
+    pub fn get_center(&self) -> Vec3 {
+        Vec3 {
+            x: (self.min_x + self.max_x) / 2.0,
+            y: (self.min_y + self.max_y) / 2.0,
+            z: (self.min_z + self.max_z) / 2.0,
+        }
+    }
+
+    pub fn of_size(center: Vec3, dx: f64, dy: f64, dz: f64) -> AABB {
+        AABB {
+            min_x: center.x - dx / 2.0,
+            min_y: center.y - dy / 2.0,
+            min_z: center.z - dz / 2.0,
+            max_x: center.x + dx / 2.0,
+            max_y: center.y + dy / 2.0,
+            max_z: center.z + dz / 2.0,
+        }
+    }
+
+    pub fn max(&self, axis: &Axis) -> f64 {
+        axis.choose(self.max_x, self.max_y, self.max_z)
+    }
+    pub fn min(&self, axis: &Axis) -> f64 {
+        axis.choose(self.min_x, self.min_y, self.min_z)
+    }
+}
diff --git a/azalea-core/src/bitset.rs b/azalea-core/src/bitset.rs
new file mode 100644
index 00000000..2ffd5657
--- /dev/null
+++ b/azalea-core/src/bitset.rs
@@ -0,0 +1,58 @@
+use azalea_buf::{BufReadError, McBufReadable, McBufWritable};
+use std::io::{Read, Write};
+
+/// Represents Java's BitSet, a list of bits.
+#[derive(Debug, Clone, PartialEq, Eq, Hash, Default)]
+pub struct BitSet {
+    data: Vec<u64>,
+}
+
+// the Index trait requires us to return a reference, but we can't do that
+impl BitSet {
+    pub fn new(size: usize) -> Self {
+        BitSet {
+            data: vec![0; size.div_ceil(64)],
+        }
+    }
+
+    pub fn index(&self, index: usize) -> bool {
+        (self.data[index / 64] & (1u64 << (index % 64))) != 0
+    }
+}
+
+impl McBufReadable for BitSet {
+    fn read_from(buf: &mut impl Read) -> Result<Self, BufReadError> {
+        Ok(Self {
+            data: Vec::<u64>::read_from(buf)?,
+        })
+    }
+}
+
+impl McBufWritable for BitSet {
+    fn write_into(&self, buf: &mut impl Write) -> Result<(), std::io::Error> {
+        self.data.write_into(buf)
+    }
+}
+
+impl BitSet {
+    pub fn set(&mut self, bit_index: usize) {
+        self.data[bit_index / 64] |= 1u64 << (bit_index % 64);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_bitset() {
+        let mut bitset = BitSet::new(64);
+        assert_eq!(bitset.index(0), false);
+        assert_eq!(bitset.index(1), false);
+        assert_eq!(bitset.index(2), false);
+        bitset.set(1);
+        assert_eq!(bitset.index(0), false);
+        assert_eq!(bitset.index(1), true);
+        assert_eq!(bitset.index(2), false);
+    }
+}
diff --git a/azalea-core/src/block_hit_result.rs b/azalea-core/src/block_hit_result.rs
new file mode 100644
index 00000000..80c9b8fc
--- /dev/null
+++ b/azalea-core/src/block_hit_result.rs
@@ -0,0 +1,9 @@
+use crate::{BlockPos, Direction, Vec3};
+
+pub struct BlockHitResult {
+    pub location: Vec3,
+    pub direction: Direction,
+    pub block_pos: BlockPos,
+    pub miss: bool,
+    pub inside: bool,
+}
diff --git a/azalea-core/src/cursor3d.rs b/azalea-core/src/cursor3d.rs
new file mode 100644
index 00000000..fa265c8a
--- /dev/null
+++ b/azalea-core/src/cursor3d.rs
@@ -0,0 +1,115 @@
+use crate::BlockPos;
+
+pub struct Cursor3d {
+    index: usize,
+
+    origin_x: i32,
+    origin_y: i32,
+    origin_z: i32,
+
+    width: usize,
+    height: usize,
+    depth: usize,
+
+    end: usize,
+}
+
+impl Iterator for Cursor3d {
+    type Item = CursorIteration;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.index == self.end {
+            return None;
+        }
+        let x = self.index % self.width;
+        let r = self.index / self.width;
+        let y = r % self.height;
+        let z = r / self.height;
+        self.index += 1;
+
+        let mut iteration_type = 0;
+        if x == 0 || x == self.width - 1 {
+            iteration_type += 1;
+        }
+        if y == 0 || y == self.height - 1 {
+            iteration_type += 1;
+        }
+        if z == 0 || z == self.depth - 1 {
+            iteration_type += 1;
+        }
+
+        Some(CursorIteration {
+            pos: BlockPos {
+                x: self.origin_x + x as i32,
+                y: self.origin_y + y as i32,
+                z: self.origin_z + z as i32,
+            },
+            iteration_type: iteration_type.into(),
+        })
+    }
+}
+
+#[repr(u8)]
+#[derive(Eq, PartialEq, Debug)]
+pub enum CursorIterationType {
+    Inside = 0,
+    Face = 1,
+    Edge = 2,
+    Corner = 3,
+}
+
+pub struct CursorIteration {
+    pub pos: BlockPos,
+    pub iteration_type: CursorIterationType,
+}
+
+impl Cursor3d {
+    pub fn new(
+        origin_x: i32,
+        origin_y: i32,
+        origin_z: i32,
+        end_x: i32,
+        end_y: i32,
+        end_z: i32,
+    ) -> Self {
+        println!(
+            "making cursor3d with origin: {}, {}, {} and end: {}, {}, {}",
+            origin_x, origin_y, origin_z, end_x, end_y, end_z
+        );
+        let width = (end_x - origin_x + 1)
+            .try_into()
+            .expect("Impossible width.");
+        let height = (end_y - origin_y + 1)
+            .try_into()
+            .expect("Impossible height.");
+        let depth = (end_z - origin_z + 1)
+            .try_into()
+            .expect("Impossible depth.");
+
+        Self {
+            index: 0,
+
+            origin_x,
+            origin_y,
+            origin_z,
+
+            width,
+            height,
+            depth,
+
+            end: width * height * depth,
+        }
+    }
+}
+
+impl From<u8> for CursorIterationType {
+    fn from(value: u8) -> Self {
+        match value {
+            0 => CursorIterationType::Inside,
+            1 => CursorIterationType::Face,
+            2 => CursorIterationType::Edge,
+            3 => CursorIterationType::Corner,
+            _ => panic!("Invalid iteration type"),
+        }
+    }
+}
diff --git a/azalea-core/src/delta.rs b/azalea-core/src/delta.rs
index c0056411..d1f72c17 100644
--- a/azalea-core/src/delta.rs
+++ b/azalea-core/src/delta.rs
@@ -1,4 +1,6 @@
-use crate::EntityPos;
+use std::ops::{Add, AddAssign};
+
+use crate::Vec3;
 pub use azalea_buf::McBuf;
 
 pub trait PositionDeltaTrait {
@@ -7,13 +9,6 @@ pub trait PositionDeltaTrait {
     fn z(&self) -> f64;
 }
 
-#[derive(Clone, Debug, McBuf, Default)]
-pub struct PositionDelta {
-    pub xa: f64,
-    pub ya: f64,
-    pub za: f64,
-}
-
 /// Only works for up to 8 blocks
 #[derive(Clone, Debug, McBuf, Default)]
 pub struct PositionDelta8 {
@@ -22,18 +17,6 @@ pub struct PositionDelta8 {
     pub za: i16,
 }
 
-impl PositionDeltaTrait for PositionDelta {
-    fn x(&self) -> f64 {
-        self.xa
-    }
-    fn y(&self) -> f64 {
-        self.ya
-    }
-    fn z(&self) -> f64 {
-        self.za
-    }
-}
-
 impl PositionDelta8 {
     #[deprecated]
     pub fn float(&self) -> (f64, f64, f64) {
@@ -57,12 +40,60 @@ impl PositionDeltaTrait for PositionDelta8 {
     }
 }
 
-impl EntityPos {
-    pub fn with_delta(&self, delta: &dyn PositionDeltaTrait) -> EntityPos {
-        EntityPos {
+impl Vec3 {
+    pub fn with_delta(&self, delta: &dyn PositionDeltaTrait) -> Vec3 {
+        Vec3 {
             x: self.x + delta.x(),
             y: self.y + delta.y(),
             z: self.z + delta.z(),
         }
     }
+
+    pub fn length_squared(&self) -> f64 {
+        self.x * self.x + self.y * self.y + self.z * self.z
+    }
+
+    pub fn normalize(&self) -> Vec3 {
+        let length = f64::sqrt(self.x * self.x + self.y * self.y + self.z * self.z);
+        if length < 1e-4 {
+            return Vec3::default();
+        }
+        Vec3 {
+            x: self.x / length,
+            y: self.y / length,
+            z: self.z / length,
+        }
+    }
+
+    pub fn multiply(&self, x: f64, y: f64, z: f64) -> Vec3 {
+        Vec3 {
+            x: self.x * x,
+            y: self.y * y,
+            z: self.z * z,
+        }
+    }
+    pub fn scale(&self, amount: f64) -> Vec3 {
+        self.multiply(amount, amount, amount)
+    }
+}
+
+// impl + and +=
+impl Add for Vec3 {
+    type Output = Vec3;
+
+    fn add(self, other: Vec3) -> Vec3 {
+        Vec3 {
+            x: self.x + other.x,
+            y: self.y + other.y,
+            z: self.z + other.z,
+        }
+    }
+}
+
+impl AddAssign for Vec3 {
+    fn add_assign(&mut self, other: Vec3) {
+        self.x += other.x;
+        self.y += other.y;
+        self.z += other.z;
+    }
 }
diff --git a/azalea-core/src/direction.rs b/azalea-core/src/direction.rs
index d3083922..96d20a10 100644
--- a/azalea-core/src/direction.rs
+++ b/azalea-core/src/direction.rs
@@ -1,5 +1,7 @@
 use azalea_buf::McBuf;
 
+use crate::floor_mod;
+
 #[derive(Clone, Copy, Debug, McBuf)]
 pub enum Direction {
     Down = 0,
@@ -9,3 +11,73 @@ pub enum Direction {
     West = 4,
     East = 5,
 }
+
+#[derive(Clone, Copy, Debug)]
+pub enum Axis {
+    X = 0,
+    Y = 1,
+    Z = 2,
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum AxisCycle {
+    None = 0,
+    Forward = 1,
+    Backward = 2,
+}
+
+impl Axis {
+    /// Pick x, y, or z from the arguments depending on the axis.
+    #[inline]
+    pub fn choose<T>(&self, x: T, y: T, z: T) -> T {
+        match self {
+            Axis::X => x,
+            Axis::Y => y,
+            Axis::Z => z,
+        }
+    }
+
+    pub fn from_ordinal(ordinal: u32) -> Self {
+        match ordinal {
+            0 => Axis::X,
+            1 => Axis::Y,
+            2 => Axis::Z,
+            _ => panic!("Invalid ordinal {}", ordinal),
+        }
+    }
+}
+
+impl AxisCycle {
+    pub fn from_ordinal(ordinal: u32) -> Self {
+        match ordinal {
+            0 => Self::None,
+            1 => Self::Forward,
+            2 => Self::Backward,
+            _ => panic!("invalid ordinal"),
+        }
+    }
+    pub fn between(axis0: Axis, axis1: Axis) -> Self {
+        Self::from_ordinal(floor_mod(axis1 as i32 - axis0 as i32, 3))
+    }
+    pub fn inverse(self) -> Self {
+        match self {
+            Self::None => Self::None,
+            Self::Forward => Self::Backward,
+            Self::Backward => Self::Forward,
+        }
+    }
+    pub fn cycle(self, axis: Axis) -> Axis {
+        match self {
+            Self::None => axis,
+            Self::Forward => Axis::from_ordinal(floor_mod(axis as i32 + 1, 3)),
+            Self::Backward => Axis::from_ordinal(floor_mod(axis as i32 - 1, 3)),
+        }
+    }
+    pub fn cycle_xyz(self, x: u32, y: u32, z: u32, axis: Axis) -> u32 {
+        match self {
+            Self::None => axis.choose(x, y, z),
+            Self::Forward => axis.choose(z, x, y),
+            Self::Backward => axis.choose(y, z, x),
+        }
+    }
+}
diff --git a/azalea-core/src/lib.rs b/azalea-core/src/lib.rs
index a1fa1fca..5aca7d52 100755
--- a/azalea-core/src/lib.rs
+++ b/azalea-core/src/lib.rs
@@ -12,16 +12,55 @@ mod game_type;
 pub use game_type::*;
 
 mod slot;
-pub use slot::{Slot, SlotData};
+pub use slot::*;
 
 mod position;
 pub use position::*;
 
 mod direction;
-pub use direction::Direction;
+pub use direction::*;
 
 mod delta;
 pub use delta::*;
 
 mod particle;
 pub use particle::*;
+
+mod cursor3d;
+pub use cursor3d::*;
+
+mod bitset;
+pub use bitset::*;
+
+mod aabb;
+pub use aabb::*;
+
+mod block_hit_result;
+pub use block_hit_result::*;
+
+// java moment
+// TODO: add tests and optimize/simplify this
+pub fn floor_mod(x: i32, y: u32) -> u32 {
+    if x < 0 {
+        y - ((-x) as u32 % y)
+    } else {
+        x as u32 % y
+    }
+}
+
+// TODO: make this generic
+pub fn binary_search(mut min: u32, max: u32, predicate: &dyn Fn(u32) -> bool) -> u32 {
+    let mut diff = max - min;
+    while diff > 0 {
+        let diff_mid = diff / 2;
+        let mid = min + diff_mid;
+        if predicate(mid) {
+            diff = diff_mid;
+        } else {
+            min = mid + 1;
+            diff -= diff_mid + 1;
+        }
+    }
+
+    min
+}
diff --git a/azalea-core/src/position.rs b/azalea-core/src/position.rs
index 7371d530..f54510b5 100644
--- a/azalea-core/src/position.rs
+++ b/azalea-core/src/position.rs
@@ -2,13 +2,54 @@ use crate::ResourceLocation;
 use azalea_buf::{BufReadError, McBufReadable, McBufWritable};
 use std::{
     io::{Read, Write},
-    ops::Rem,
+    ops::{Add, Mul, Rem},
 };
 
-pub trait PositionXYZ<T> {
-    fn add_x(&self, n: T) -> Self;
-    fn add_y(&self, n: T) -> Self;
-    fn add_z(&self, n: T) -> Self;
+pub trait PositionXYZ<T>
+where
+    T: Add<T, Output = T> + Mul<T, Output = T>,
+{
+    fn x(&self) -> T;
+    fn y(&self) -> T;
+    fn z(&self) -> T;
+
+    fn set_x(&self, n: T) -> Self;
+    fn set_y(&self, n: T) -> Self;
+    fn set_z(&self, n: T) -> Self;
+
+    // hopefully these get optimized
+    fn add_x(&self, n: T) -> Self
+    where
+        Self: Sized,
+    {
+        self.set_x(self.x() + n)
+    }
+    fn add_y(&self, n: T) -> Self
+    where
+        Self: Sized,
+    {
+        self.set_y(self.y() + n)
+    }
+    fn add_z(&self, n: T) -> Self
+    where
+        Self: Sized,
+    {
+        self.set_z(self.z() + n)
+    }
+
+    fn add(&self, x: T, y: T, z: T) -> Self
+    where
+        Self: Sized,
+    {
+        self.add_x(x).add_y(y).add_z(z)
+    }
+
+    fn length_sqr(&self) -> T
+    where
+        Self: Sized,
+    {
+        self.x() * self.x() + self.y() * self.y() + self.z() * self.z()
+    }
 }
 
 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
@@ -22,6 +63,10 @@ impl BlockPos {
     pub fn new(x: i32, y: i32, z: i32) -> Self {
         BlockPos { x, y, z }
     }
+
+    pub fn below(&self) -> Self {
+        self.add(0, -1, 0)
+    }
 }
 
 impl Rem<i32> for BlockPos {
@@ -37,25 +82,34 @@ impl Rem<i32> for BlockPos {
 }
 
 impl PositionXYZ<i32> for BlockPos {
-    fn add_x(&self, n: i32) -> Self {
+    fn x(&self) -> i32 {
+        self.x
+    }
+    fn y(&self) -> i32 {
+        self.y
+    }
+    fn z(&self) -> i32 {
+        self.z
+    }
+    fn set_x(&self, n: i32) -> Self {
         BlockPos {
-            x: self.x + n,
+            x: n,
             y: self.y,
             z: self.z,
         }
     }
-    fn add_y(&self, n: i32) -> Self {
+    fn set_y(&self, n: i32) -> Self {
         BlockPos {
             x: self.x,
-            y: self.y + n,
+            y: n,
             z: self.z,
         }
     }
-    fn add_z(&self, n: i32) -> Self {
+    fn set_z(&self, n: i32) -> Self {
         BlockPos {
             x: self.x,
             y: self.y,
-            z: self.z + n,
+            z: n,
         }
     }
 }
@@ -84,6 +138,9 @@ impl ChunkSectionPos {
     pub fn new(x: i32, y: i32, z: i32) -> Self {
         ChunkSectionPos { x, y, z }
     }
+    pub fn block_to_section_coord(block: i32) -> i32 {
+        block >> 4
+    }
 }
 /// The coordinates of a block inside a chunk.
 #[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
@@ -123,33 +180,43 @@ pub struct GlobalPos {
     pub dimension: ResourceLocation,
 }
 
+/// An exact point in the world.
 #[derive(Debug, Clone, Copy, Default)]
-pub struct EntityPos {
+pub struct Vec3 {
     pub x: f64,
     pub y: f64,
     pub z: f64,
 }
 
-impl PositionXYZ<f64> for EntityPos {
-    fn add_x(&self, n: f64) -> Self {
-        EntityPos {
-            x: self.x + n,
+impl PositionXYZ<f64> for Vec3 {
+    fn x(&self) -> f64 {
+        self.x
+    }
+    fn y(&self) -> f64 {
+        self.y
+    }
+    fn z(&self) -> f64 {
+        self.z
+    }
+    fn set_x(&self, n: f64) -> Self {
+        Vec3 {
+            x: n,
             y: self.y,
             z: self.z,
         }
     }
-    fn add_y(&self, n: f64) -> Self {
-        EntityPos {
+    fn set_y(&self, n: f64) -> Self {
+        Vec3 {
             x: self.x,
-            y: self.y + n,
+            y: n,
             z: self.z,
         }
     }
-    fn add_z(&self, n: f64) -> Self {
-        EntityPos {
+    fn set_z(&self, n: f64) -> Self {
+        Vec3 {
             x: self.x,
             y: self.y,
-            z: self.z + n,
+            z: n,
         }
     }
 }
@@ -208,8 +275,8 @@ impl From<&ChunkBlockPos> for ChunkSectionBlockPos {
         }
     }
 }
-impl From<&EntityPos> for BlockPos {
-    fn from(pos: &EntityPos) -> Self {
+impl From<&Vec3> for BlockPos {
+    fn from(pos: &Vec3) -> Self {
         BlockPos {
             x: pos.x.floor() as i32,
             y: pos.y.floor() as i32,
@@ -218,18 +285,27 @@ impl From<&EntityPos> for BlockPos {
     }
 }
 
-impl From<&EntityPos> for ChunkPos {
-    fn from(pos: &EntityPos) -> Self {
+impl From<&Vec3> for ChunkPos {
+    fn from(pos: &Vec3) -> Self {
         ChunkPos::from(&BlockPos::from(pos))
     }
 }
 
+const PACKED_X_LENGTH: u64 = 1 + 25; // minecraft does something a bit more complicated to get this 25
+const PACKED_Z_LENGTH: u64 = PACKED_X_LENGTH;
+const PACKED_Y_LENGTH: u64 = 64 - PACKED_X_LENGTH - PACKED_Z_LENGTH;
+const PACKED_X_MASK: u64 = (1 << PACKED_X_LENGTH) - 1;
+const PACKED_Y_MASK: u64 = (1 << PACKED_Y_LENGTH) - 1;
+const PACKED_Z_MASK: u64 = (1 << PACKED_Z_LENGTH) - 1;
+const Z_OFFSET: u64 = PACKED_Y_LENGTH;
+const X_OFFSET: u64 = PACKED_Y_LENGTH + PACKED_Z_LENGTH;
+
 impl McBufReadable for BlockPos {
     fn read_from(buf: &mut impl Read) -> Result<Self, BufReadError> {
         let val = u64::read_from(buf)?;
-        let x = (val >> 38) as i32;
-        let y = (val & 0xFFF) as i32;
-        let z = ((val >> 12) & 0x3FFFFFF) as i32;
+        let x = (val << 64 - X_OFFSET - PACKED_X_LENGTH >> 64 - PACKED_X_LENGTH) as i32;
+        let y = (val << 64 - PACKED_Y_LENGTH >> 64 - PACKED_Y_LENGTH) as i32;
+        let z = (val << 64 - Z_OFFSET - PACKED_Z_LENGTH >> 64 - PACKED_Z_LENGTH) as i32;
         Ok(BlockPos { x, y, z })
     }
 }
@@ -256,10 +332,11 @@ impl McBufReadable for ChunkSectionPos {
 
 impl McBufWritable for BlockPos {
     fn write_into(&self, buf: &mut impl Write) -> Result<(), std::io::Error> {
-        let data = (((self.x & 0x3FFFFFF) as i64) << 38)
-            | (((self.z & 0x3FFFFFF) as i64) << 12)
-            | ((self.y & 0xFFF) as i64);
-        data.write_into(buf)
+        let mut val: u64 = 0;
+        val |= ((self.x as u64) & PACKED_X_MASK) << X_OFFSET;
+        val |= ((self.y as u64) & PACKED_Y_MASK) << 0;
+        val |= ((self.z as u64) & PACKED_Z_MASK) << Z_OFFSET;
+        val.write_into(buf)
     }
 }
 
@@ -302,7 +379,7 @@ mod tests {
 
     #[test]
     fn test_from_entity_pos_to_block_pos() {
-        let entity_pos = EntityPos {
+        let entity_pos = Vec3 {
             x: 31.5,
             y: 80.0,
             z: -16.1,
@@ -313,7 +390,7 @@ mod tests {
 
     #[test]
     fn test_from_entity_pos_to_chunk_pos() {
-        let entity_pos = EntityPos {
+        let entity_pos = Vec3 {
             x: 31.5,
             y: 80.0,
             z: -16.1,