3 files changed, 70 insertions, 16 deletions

diff --git a/azalea-physics/src/collision/discrete_voxel_shape.rs b/azalea-physics/src/collision/discrete_voxel_shape.rs
index 4a329398..2bcd1f61 100755
--- a/azalea-physics/src/collision/discrete_voxel_shape.rs
+++ b/azalea-physics/src/collision/discrete_voxel_shape.rs
@@ -45,6 +45,7 @@ impl DiscreteVoxelShape {
             return false;
         }
         let (x, y, z) = (x as u32, y as u32, z as u32);
+
         (x < self.size(Axis::X) && y < self.size(Axis::Y) && z < self.size(Axis::Z))
             && (self.is_full(x, y, z))
     }
diff --git a/azalea-physics/src/collision/mod.rs b/azalea-physics/src/collision/mod.rs
index 53efd2fe..a99b5710 100644
--- a/azalea-physics/src/collision/mod.rs
+++ b/azalea-physics/src/collision/mod.rs
@@ -5,10 +5,7 @@ mod shape;
 mod world_collisions;
 
 use azalea_core::{Axis, Vec3, AABB, EPSILON};
-use azalea_world::{
-    entity::{self},
-    Instance, MoveEntityError,
-};
+use azalea_world::{entity, Instance, MoveEntityError};
 pub use blocks::BlockWithShape;
 pub use discrete_voxel_shape::*;
 pub use shape::*;
@@ -219,7 +216,11 @@ fn collide_with_shapes(
     if y_movement != 0. {
         y_movement = Shapes::collide(&Axis::Y, &entity_box, collision_boxes, y_movement);
         if y_movement != 0. {
-            entity_box = entity_box.move_relative(0., y_movement, 0.);
+            entity_box = entity_box.move_relative(&Vec3 {
+                x: 0.,
+                y: y_movement,
+                z: 0.,
+            });
         }
     }
 
@@ -230,14 +231,22 @@ fn collide_with_shapes(
     if more_z_movement && z_movement != 0. {
         z_movement = Shapes::collide(&Axis::Z, &entity_box, collision_boxes, z_movement);
         if z_movement != 0. {
-            entity_box = entity_box.move_relative(0., 0., z_movement);
+            entity_box = entity_box.move_relative(&Vec3 {
+                x: 0.,
+                y: 0.,
+                z: z_movement,
+            });
         }
     }
 
     if x_movement != 0. {
         x_movement = Shapes::collide(&Axis::X, &entity_box, collision_boxes, x_movement);
         if x_movement != 0. {
-            entity_box = entity_box.move_relative(x_movement, 0., 0.);
+            entity_box = entity_box.move_relative(&Vec3 {
+                x: x_movement,
+                y: 0.,
+                z: 0.,
+            });
         }
     }
 
diff --git a/azalea-physics/src/collision/shape.rs b/azalea-physics/src/collision/shape.rs
index cc184591..29c1b440 100755
--- a/azalea-physics/src/collision/shape.rs
+++ b/azalea-physics/src/collision/shape.rs
@@ -1,9 +1,11 @@
 use super::mergers::IndexMerger;
 use crate::collision::{BitSetDiscreteVoxelShape, DiscreteVoxelShape, AABB};
-use azalea_core::{binary_search, Axis, AxisCycle, EPSILON};
+use azalea_core::{
+    binary_search, Axis, AxisCycle, BlockHitResult, BlockPos, Direction, Vec3, EPSILON,
+};
 use std::{cmp, num::NonZeroU32};
 
-pub struct Shapes {}
+pub struct Shapes;
 
 pub fn block_shape() -> VoxelShape {
     let mut shape = BitSetDiscreteVoxelShape::new(1, 1, 1);
@@ -390,6 +392,33 @@ impl VoxelShape {
         }
     }
 
+    pub fn clip(&self, from: &Vec3, to: &Vec3, block_pos: &BlockPos) -> Option<BlockHitResult> {
+        if self.is_empty() {
+            return None;
+        }
+        let vector = to - from;
+        if vector.length_sqr() < EPSILON {
+            return None;
+        }
+        let right_after_start = from + &(vector * 0.0001);
+
+        if self.shape().is_full_wide(
+            self.find_index(Axis::X, right_after_start.x - block_pos.x as f64),
+            self.find_index(Axis::Y, right_after_start.y - block_pos.y as f64),
+            self.find_index(Axis::Z, right_after_start.z - block_pos.z as f64),
+        ) {
+            Some(BlockHitResult {
+                block_pos: *block_pos,
+                direction: Direction::nearest(vector).opposite(),
+                location: right_after_start,
+                inside: true,
+                miss: false,
+            })
+        } else {
+            AABB::clip_iterable(&self.to_aabbs(), from, to, block_pos)
+        }
+    }
+
     pub fn collide(&self, axis: &Axis, entity_box: &AABB, movement: f64) -> f64 {
         self.collide_x(AxisCycle::between(*axis, Axis::X), entity_box, movement)
     }
@@ -531,19 +560,34 @@ impl VoxelShape {
         let y_coords = self.get_coords(Axis::Y);
         let z_coords = self.get_coords(Axis::Z);
         self.shape().for_all_boxes(
-            |var4x, var5, var6, var7, var8, var9| {
+            |min_x, min_y, min_z, max_x, max_y, max_z| {
                 consumer(
-                    x_coords[var4x as usize],
-                    y_coords[var5 as usize],
-                    z_coords[var6 as usize],
-                    x_coords[var7 as usize],
-                    y_coords[var8 as usize],
-                    z_coords[var9 as usize],
+                    x_coords[min_x as usize],
+                    y_coords[min_y as usize],
+                    z_coords[min_z as usize],
+                    x_coords[max_x as usize],
+                    y_coords[max_y as usize],
+                    z_coords[max_z as usize],
                 );
             },
             true,
         );
     }
+
+    pub fn to_aabbs(&self) -> Vec<AABB> {
+        let mut aabbs = Vec::new();
+        self.for_all_boxes(|min_x, min_y, min_z, max_x, max_y, max_z| {
+            aabbs.push(AABB {
+                min_x,
+                min_y,
+                min_z,
+                max_x,
+                max_y,
+                max_z,
+            });
+        });
+        aabbs
+    }
 }
 
 impl From<AABB> for VoxelShape {