optimize physics a bit more

4 files changed, 147 insertions, 129 deletions

diff --git a/azalea-physics/src/collision/mergers.rs b/azalea-physics/src/collision/mergers.rs
index 69ae2ab5..c43412d1 100755
--- a/azalea-physics/src/collision/mergers.rs
+++ b/azalea-physics/src/collision/mergers.rs
@@ -1,4 +1,4 @@
-use std::cmp::Ordering;
+use std::cmp::{self, Ordering};
 
 use super::CubePointRange;
 use azalea_core::math::{gcd, lcm, EPSILON};
@@ -135,27 +135,27 @@ impl IndexMerger {
         }
     }
 
-    pub fn new_indirect(var1: &[f64], var2: &[f64], var3: bool, var4: bool) -> Self {
+    pub fn new_indirect(coords1: &[f64], coords2: &[f64], var3: bool, var4: bool) -> Self {
         let mut var5 = f64::NAN;
-        let var7 = var1.len();
-        let var8 = var2.len();
-        let var9 = var7 + var8;
-        let mut result = vec![0.0; var9];
-        let mut first_indices: Vec<isize> = vec![0; var9];
-        let mut second_indices: Vec<isize> = vec![0; var9];
+        let coords1_len = coords1.len();
+        let coords2_len = coords2.len();
+        let number_of_indices = coords1_len + coords2_len;
+        let mut result = vec![0.0; number_of_indices];
+        let mut first_indices: Vec<isize> = vec![0; number_of_indices];
+        let mut second_indices: Vec<isize> = vec![0; number_of_indices];
         let var10 = !var3;
         let var11 = !var4;
         let mut var12 = 0;
-        let mut var13 = 0;
-        let mut var14 = 0;
+        let mut coords1_index = 0;
+        let mut coords2_index = 0;
 
         loop {
-            let mut var17: bool;
+            let mut iterating_coords1: bool;
             loop {
-                let var15 = var13 >= var7;
-                let var16 = var14 >= var8;
-                if var15 && var16 {
-                    let result_length = std::cmp::max(1, var12);
+                let at_end_of_coords1 = coords1_index >= coords1_len;
+                let at_end_of_coords2 = coords2_index >= coords2_len;
+                if at_end_of_coords1 && at_end_of_coords2 {
+                    let result_length = cmp::max(1, var12);
                     return Self::Indirect {
                         result,
                         first_indices,
@@ -164,26 +164,28 @@ impl IndexMerger {
                     };
                 }
 
-                var17 = !var15 && (var16 || var1[var13] < var2[var14] + EPSILON);
-                if var17 {
-                    var13 += 1;
-                    if !var10 || var14 != 0 && !var16 {
+                iterating_coords1 = !at_end_of_coords1
+                    && (at_end_of_coords2
+                        || coords1[coords1_index] < coords2[coords2_index] + EPSILON);
+                if iterating_coords1 {
+                    coords1_index += 1;
+                    if !var10 || coords2_index != 0 && !at_end_of_coords2 {
                         break;
                     }
                 } else {
-                    var14 += 1;
-                    if !var11 || var13 != 0 && !var15 {
+                    coords2_index += 1;
+                    if !var11 || coords1_index != 0 && !at_end_of_coords1 {
                         break;
                     }
                 }
             }
 
-            let var18: isize = (var13 as isize) - 1;
-            let var19: isize = (var14 as isize) - 1;
-            let var20 = if var17 {
-                var1[TryInto::<usize>::try_into(var18).unwrap()]
+            let var18: isize = (coords1_index as isize) - 1;
+            let var19: isize = (coords2_index as isize) - 1;
+            let var20 = if iterating_coords1 {
+                coords1[usize::try_from(var18).unwrap()]
             } else {
-                var2[TryInto::<usize>::try_into(var19).unwrap()]
+                coords2[usize::try_from(var19).unwrap()]
             };
             match var5.partial_cmp(&(var20 - EPSILON)) {
                 None | Some(Ordering::Less) => {
diff --git a/azalea-physics/src/collision/mod.rs b/azalea-physics/src/collision/mod.rs
index 72151b6b..3986dc47 100644
--- a/azalea-physics/src/collision/mod.rs
+++ b/azalea-physics/src/collision/mod.rs
@@ -266,7 +266,6 @@ fn collide_bounding_box(
 
     let block_collisions =
         get_block_collisions(world, entity_bounding_box.expand_towards(movement));
-    let block_collisions = block_collisions.collect::<Vec<_>>();
     collision_boxes.extend(block_collisions);
     collide_with_shapes(movement, *entity_bounding_box, &collision_boxes)
 }
diff --git a/azalea-physics/src/collision/shape.rs b/azalea-physics/src/collision/shape.rs
index 56befa39..710074de 100755
--- a/azalea-physics/src/collision/shape.rs
+++ b/azalea-physics/src/collision/shape.rs
@@ -169,22 +169,22 @@ impl Shapes {
         // var5.getList(), var6.getList(), var7.getList()));
         let var5 = Self::create_index_merger(
             1,
-            a.get_coords(Axis::X).to_vec(),
-            b.get_coords(Axis::X).to_vec(),
+            a.get_coords(Axis::X),
+            b.get_coords(Axis::X),
             op_true_false,
             op_false_true,
         );
         let var6 = Self::create_index_merger(
             (var5.size() - 1).try_into().unwrap(),
-            a.get_coords(Axis::Y).to_vec(),
-            b.get_coords(Axis::Y).to_vec(),
+            a.get_coords(Axis::Y),
+            b.get_coords(Axis::Y),
             op_true_false,
             op_false_true,
         );
         let var7 = Self::create_index_merger(
             ((var5.size() - 1) * (var6.size() - 1)).try_into().unwrap(),
-            a.get_coords(Axis::Z).to_vec(),
-            b.get_coords(Axis::Z).to_vec(),
+            a.get_coords(Axis::Z),
+            b.get_coords(Axis::Z),
             op_true_false,
             op_false_true,
         );
@@ -208,8 +208,12 @@ impl Shapes {
 
     /// Check if the op is true anywhere when joining the two shapes
     /// vanilla calls this joinIsNotEmpty
-    pub fn matches_anywhere(a: &VoxelShape, b: &VoxelShape, op: fn(bool, bool) -> bool) -> bool {
-        assert!(!op(false, false));
+    pub fn matches_anywhere(
+        a: &VoxelShape,
+        b: &VoxelShape,
+        op: impl Fn(bool, bool) -> bool,
+    ) -> bool {
+        debug_assert!(!op(false, false));
         let a_is_empty = a.is_empty();
         let b_is_empty = b.is_empty();
         if a_is_empty || b_is_empty {
@@ -233,22 +237,22 @@ impl Shapes {
 
         let x_merger = Self::create_index_merger(
             1,
-            a.get_coords(Axis::X).to_vec(),
-            b.get_coords(Axis::X).to_vec(),
+            a.get_coords(Axis::X),
+            b.get_coords(Axis::X),
             op_true_false,
             op_false_true,
         );
         let y_merger = Self::create_index_merger(
             (x_merger.size() - 1) as i32,
-            a.get_coords(Axis::Y).to_vec(),
-            b.get_coords(Axis::Y).to_vec(),
+            a.get_coords(Axis::Y),
+            b.get_coords(Axis::Y),
             op_true_false,
             op_false_true,
         );
         let z_merger = Self::create_index_merger(
             ((x_merger.size() - 1) * (y_merger.size() - 1)) as i32,
-            a.get_coords(Axis::Z).to_vec(),
-            b.get_coords(Axis::Z).to_vec(),
+            a.get_coords(Axis::Z),
+            b.get_coords(Axis::Z),
             op_true_false,
             op_false_true,
         );
@@ -269,7 +273,7 @@ impl Shapes {
         merged_z: IndexMerger,
         shape1: DiscreteVoxelShape,
         shape2: DiscreteVoxelShape,
-        op: fn(bool, bool) -> bool,
+        op: impl Fn(bool, bool) -> bool,
     ) -> bool {
         !merged_x.for_merged_indexes(|var5x, var6, _var7| {
             merged_y.for_merged_indexes(|var6x, var7x, _var8| {
@@ -285,13 +289,13 @@ impl Shapes {
 
     pub fn create_index_merger(
         _var0: i32,
-        var1: Vec<f64>,
-        var2: Vec<f64>,
+        coords1: &[f64],
+        coords2: &[f64],
         var3: bool,
         var4: bool,
     ) -> IndexMerger {
-        let var5 = var1.len() - 1;
-        let var6 = var2.len() - 1;
+        let var5 = coords1.len() - 1;
+        let var6 = coords2.len() - 1;
         // if (&var1 as &dyn Any).is::<CubePointRange>() && (&var2 as &dyn
         // Any).is::<CubePointRange>() {
         // return new DiscreteCubeMerger(var0, var5, var6, var3, var4);
@@ -302,22 +306,24 @@ impl Shapes {
         // }
         // }
 
-        if var1[var5] < var2[0] - EPSILON {
+        if coords1[var5] < coords2[0] - EPSILON {
             IndexMerger::NonOverlapping {
-                lower: var1,
-                upper: var2,
+                lower: coords1.to_vec(),
+                upper: coords2.to_vec(),
                 swap: false,
             }
-        } else if var2[var6] < var1[0] - EPSILON {
+        } else if coords2[var6] < coords1[0] - EPSILON {
             IndexMerger::NonOverlapping {
-                lower: var2,
-                upper: var1,
+                lower: coords2.to_vec(),
+                upper: coords1.to_vec(),
                 swap: true,
             }
-        } else if var5 == var6 && var1 == var2 {
-            IndexMerger::Identical { coords: var1 }
+        } else if var5 == var6 && coords1 == coords2 {
+            IndexMerger::Identical {
+                coords: coords1.to_vec(),
+            }
         } else {
-            IndexMerger::new_indirect(&var1, &var2, var3, var4)
+            IndexMerger::new_indirect(&coords1, &coords2, var3, var4)
         }
     }
 }
@@ -386,6 +392,7 @@ impl VoxelShape {
         ))
     }
 
+    #[inline]
     pub fn get(&self, axis: Axis, index: usize) -> f64 {
         // self.get_coords(axis)[index]
         match self {
@@ -403,9 +410,8 @@ impl VoxelShape {
         match self {
             VoxelShape::Cube(s) => s.find_index(axis, coord),
             _ => {
-                binary_search(0, (self.shape().size(axis) + 1) as i32, &|t| {
-                    coord < self.get(axis, t as usize)
-                }) - 1
+                let upper_limit = (self.shape().size(axis) + 1) as i32;
+                binary_search(0, upper_limit, &|t| coord < self.get(axis, t as usize)) - 1
             }
         }
     }
@@ -657,6 +663,7 @@ impl ArrayVoxelShape {
         &self.shape
     }
 
+    #[inline]
     fn get_coords(&self, axis: Axis) -> &[f64] {
         axis.choose(&self.xs, &self.ys, &self.zs)
     }
@@ -693,6 +700,7 @@ impl CubeVoxelShape {
         parts
     }
 
+    #[inline]
     fn get_coords(&self, axis: Axis) -> &[f64] {
         axis.choose(&self.x_coords, &self.y_coords, &self.z_coords)
     }
diff --git a/azalea-physics/src/collision/world_collisions.rs b/azalea-physics/src/collision/world_collisions.rs
index 8493b847..54493d62 100644
--- a/azalea-physics/src/collision/world_collisions.rs
+++ b/azalea-physics/src/collision/world_collisions.rs
@@ -4,17 +4,79 @@ use azalea_block::BlockState;
 use azalea_core::{
     cursor3d::{Cursor3d, CursorIterationType},
     math::EPSILON,
-    position::{BlockPos, ChunkPos, ChunkSectionBlockPos, ChunkSectionPos},
+    position::{BlockPos, ChunkBlockPos, ChunkPos, ChunkSectionBlockPos, ChunkSectionPos},
 };
 use azalea_world::{Chunk, Instance};
 use parking_lot::RwLock;
 use std::sync::Arc;
 
-pub fn get_block_collisions(world: &Instance, aabb: AABB) -> BlockCollisions<'_> {
-    BlockCollisions::new(world, aabb)
+pub fn get_block_collisions(world: &Instance, aabb: AABB) -> Vec<VoxelShape> {
+    let mut state = BlockCollisionsState::new(world, aabb);
+    let mut block_collisions = Vec::new();
+
+    let initial_chunk_pos = ChunkPos::from(state.cursor.origin());
+    let initial_chunk = world.chunks.get(&initial_chunk_pos).unwrap();
+    let initial_chunk = initial_chunk.read();
+
+    while let Some(item) = state.cursor.next() {
+        if item.iteration_type == CursorIterationType::Corner {
+            continue;
+        }
+
+        let item_chunk_pos = ChunkPos::from(item.pos);
+        let block_state: BlockState = if item_chunk_pos == initial_chunk_pos {
+            initial_chunk
+                .get(&ChunkBlockPos::from(item.pos), state.world.chunks.min_y)
+                .unwrap_or(BlockState::AIR)
+        } else {
+            state.get_block_state(item.pos)
+        };
+
+        if block_state.is_air() {
+            // fast path since we can't collide with air
+            continue;
+        }
+
+        // TODO: continue if self.only_suffocating_blocks and the block is not
+        // suffocating
+
+        // if it's a full block do a faster collision check
+        if block_state.is_shape_full() {
+            if !state.aabb.intersects_aabb(&AABB {
+                min_x: item.pos.x as f64,
+                min_y: item.pos.y as f64,
+                min_z: item.pos.z as f64,
+                max_x: (item.pos.x + 1) as f64,
+                max_y: (item.pos.y + 1) as f64,
+                max_z: (item.pos.z + 1) as f64,
+            }) {
+                continue;
+            }
+
+            block_collisions.push(BLOCK_SHAPE.move_relative(
+                item.pos.x as f64,
+                item.pos.y as f64,
+                item.pos.z as f64,
+            ));
+            continue;
+        }
+
+        let block_shape = state.get_block_shape(block_state);
+
+        let block_shape =
+            block_shape.move_relative(item.pos.x as f64, item.pos.y as f64, item.pos.z as f64);
+        // if the entity shape and block shape don't collide, continue
+        if !Shapes::matches_anywhere(&block_shape, &state.entity_shape, |a, b| a && b) {
+            continue;
+        }
+
+        block_collisions.push(block_shape);
+    }
+
+    block_collisions
 }
 
-pub struct BlockCollisions<'a> {
+pub struct BlockCollisionsState<'a> {
     pub world: &'a Instance,
     pub aabb: AABB,
     pub entity_shape: VoxelShape,
@@ -25,17 +87,21 @@ pub struct BlockCollisions<'a> {
     cached_block_shapes: Vec<(BlockState, &'static VoxelShape)>,
 }
 
-impl<'a> BlockCollisions<'a> {
+impl<'a> BlockCollisionsState<'a> {
     pub fn new(world: &'a Instance, aabb: AABB) -> Self {
-        let origin_x = (aabb.min_x - EPSILON).floor() as i32 - 1;
-        let origin_y = (aabb.min_y - EPSILON).floor() as i32 - 1;
-        let origin_z = (aabb.min_z - EPSILON).floor() as i32 - 1;
+        let origin = BlockPos {
+            x: (aabb.min_x - EPSILON).floor() as i32 - 1,
+            y: (aabb.min_y - EPSILON).floor() as i32 - 1,
+            z: (aabb.min_z - EPSILON).floor() as i32 - 1,
+        };
 
-        let end_x = (aabb.max_x + EPSILON).floor() as i32 + 1;
-        let end_y = (aabb.max_y + EPSILON).floor() as i32 + 1;
-        let end_z = (aabb.max_z + EPSILON).floor() as i32 + 1;
+        let end = BlockPos {
+            x: (aabb.max_x + EPSILON).floor() as i32 + 1,
+            y: (aabb.max_y + EPSILON).floor() as i32 + 1,
+            z: (aabb.max_z + EPSILON).floor() as i32 + 1,
+        };
 
-        let cursor = Cursor3d::new(origin_x, origin_y, origin_z, end_x, end_y, end_z);
+        let cursor = Cursor3d::new(origin, end);
 
         Self {
             world,
@@ -97,10 +163,8 @@ impl<'a> BlockCollisions<'a> {
 
         self.cached_sections.push((section_pos, section.clone()));
 
-        // println!(
-        //     "chunk section length: {}",
-        //     section.states.storage.data.len()
-        // );
+        // println!("chunk section palette: {:?}", section.states.palette);
+        // println!("chunk section data: {:?}", section.states.storage.data);
         // println!("biome length: {}", section.biomes.storage.data.len());
 
         section.get(section_block_pos)
@@ -119,58 +183,3 @@ impl<'a> BlockCollisions<'a> {
         shape
     }
 }
-
-impl<'a> Iterator for BlockCollisions<'a> {
-    type Item = VoxelShape;
-
-    fn next(&mut self) -> Option<Self::Item> {
-        while let Some(item) = self.cursor.next() {
-            if item.iteration_type == CursorIterationType::Corner {
-                continue;
-            }
-
-            let block_state = self.get_block_state(item.pos);
-
-            if block_state.is_air() {
-                // fast path since we can't collide with air
-                continue;
-            }
-
-            // TODO: continue if self.only_suffocating_blocks and the block is not
-            // suffocating
-
-            // if it's a full block do a faster collision check
-            if block_state.is_shape_full() {
-                if !self.aabb.intersects_aabb(&AABB {
-                    min_x: item.pos.x as f64,
-                    min_y: item.pos.y as f64,
-                    min_z: item.pos.z as f64,
-                    max_x: (item.pos.x + 1) as f64,
-                    max_y: (item.pos.y + 1) as f64,
-                    max_z: (item.pos.z + 1) as f64,
-                }) {
-                    continue;
-                }
-
-                return Some(BLOCK_SHAPE.move_relative(
-                    item.pos.x as f64,
-                    item.pos.y as f64,
-                    item.pos.z as f64,
-                ));
-            }
-
-            let block_shape = self.get_block_shape(block_state);
-
-            let block_shape =
-                block_shape.move_relative(item.pos.x as f64, item.pos.y as f64, item.pos.z as f64);
-            // if the entity shape and block shape don't collide, continue
-            if !Shapes::matches_anywhere(&block_shape, &self.entity_shape, |a, b| a && b) {
-                continue;
-            }
-
-            return Some(block_shape);
-        }
-
-        None
-    }
-}