BLAZINGLY FAST 🚀🚀🚀 pathfinding

1 files changed, 116 insertions, 18 deletions

diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 1a626c1c..69145eeb 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -1,14 +1,21 @@
 pub mod basic;
 pub mod parkour;
 
-use std::{cell::RefCell, fmt::Debug, sync::Arc};
+use std::{
+    cell::{RefCell, UnsafeCell},
+    fmt::Debug,
+    sync::Arc,
+};
 
 use crate::{JumpEvent, LookAtEvent};
 
 use super::astar;
 use azalea_block::BlockState;
 use azalea_client::{StartSprintEvent, StartWalkEvent};
-use azalea_core::position::{BlockPos, ChunkBlockPos, ChunkPos, Vec3};
+use azalea_core::{
+    bitset::FixedBitSet,
+    position::{BlockPos, ChunkBlockPos, ChunkPos, ChunkSectionBlockPos, ChunkSectionPos, Vec3},
+};
 use azalea_physics::collision::BlockWithShape;
 use azalea_world::Instance;
 use bevy_ecs::{entity::Entity, event::EventWriter};
@@ -38,6 +45,15 @@ pub struct PathfinderCtx {
     min_y: i32,
     world_lock: Arc<RwLock<Instance>>,
     cached_chunks: RefCell<Vec<(ChunkPos, Vec<azalea_world::Section>)>>,
+
+    cached_block_passable: UnsafeCell<Vec<CachedBlocks>>,
+    cached_block_solid: UnsafeCell<Vec<CachedBlocks>>,
+}
+
+pub struct CachedBlocks {
+    pub pos: ChunkSectionPos,
+    pub present: FixedBitSet<4096>,
+    pub value: FixedBitSet<4096>,
 }
 
 impl PathfinderCtx {
@@ -47,10 +63,12 @@ impl PathfinderCtx {
             min_y,
             world_lock,
             cached_chunks: Default::default(),
+            cached_block_passable: Default::default(),
+            cached_block_solid: Default::default(),
         }
     }
 
-    fn get_block_state(&self, pos: &BlockPos) -> Option<BlockState> {
+    fn get_block_state(&self, pos: BlockPos) -> Option<BlockState> {
         let chunk_pos = ChunkPos::from(pos);
         let chunk_block_pos = ChunkBlockPos::from(pos);
 
@@ -83,7 +101,7 @@ impl PathfinderCtx {
     }
 
     /// whether this block is passable
-    pub fn is_block_passable(&self, pos: &BlockPos) -> bool {
+    fn uncached_is_block_passable(&self, pos: BlockPos) -> bool {
         let Some(block) = self.get_block_state(pos) else {
             return false;
         };
@@ -109,8 +127,48 @@ impl PathfinderCtx {
         true
     }
 
+    pub fn is_block_passable(&self, pos: BlockPos) -> bool {
+        let (section_pos, section_block_pos) =
+            (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));
+        let index = u16::from(section_block_pos) as usize;
+        let cached_block_passable = unsafe { &mut *self.cached_block_passable.get() };
+        if let Some(cached) = cached_block_passable.iter_mut().find_map(|cached| {
+            if cached.pos == section_pos {
+                Some(cached)
+            } else {
+                None
+            }
+        }) {
+            if cached.present.index(index) {
+                return cached.value.index(index);
+            } else {
+                let passable = self.uncached_is_block_passable(pos);
+                cached.present.set(index);
+                if passable {
+                    cached.value.set(index);
+                }
+                return passable;
+            }
+        }
+
+        let passable = self.uncached_is_block_passable(pos);
+        let mut present_bitset = FixedBitSet::new();
+        let mut value_bitset = FixedBitSet::new();
+        present_bitset.set(index);
+        if passable {
+            value_bitset.set(index);
+        }
+
+        cached_block_passable.push(CachedBlocks {
+            pos: section_pos,
+            present: present_bitset,
+            value: value_bitset,
+        });
+        passable
+    }
+
     /// whether this block has a solid hitbox (i.e. we can stand on it)
-    pub fn is_block_solid(&self, pos: &BlockPos) -> bool {
+    fn uncached_is_block_solid(&self, pos: BlockPos) -> bool {
         let Some(block) = self.get_block_state(pos) else {
             return false;
         };
@@ -121,23 +179,63 @@ impl PathfinderCtx {
         block.is_shape_full()
     }
 
+    pub fn is_block_solid(&self, pos: BlockPos) -> bool {
+        let (section_pos, section_block_pos) =
+            (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));
+        let index = u16::from(section_block_pos) as usize;
+        let cached_block_solid = unsafe { &mut *self.cached_block_solid.get() };
+        if let Some(cached) = cached_block_solid.iter_mut().find_map(|cached| {
+            if cached.pos == section_pos {
+                Some(cached)
+            } else {
+                None
+            }
+        }) {
+            if cached.present.index(index) {
+                return cached.value.index(index);
+            } else {
+                let solid = self.uncached_is_block_solid(pos);
+                cached.present.set(index);
+                if solid {
+                    cached.value.set(index);
+                }
+                return solid;
+            }
+        }
+
+        let solid = self.uncached_is_block_solid(pos);
+        let mut present_bitset = FixedBitSet::new();
+        let mut value_bitset = FixedBitSet::new();
+        present_bitset.set(index);
+        if solid {
+            value_bitset.set(index);
+        }
+
+        cached_block_solid.push(CachedBlocks {
+            pos: section_pos,
+            present: present_bitset,
+            value: value_bitset,
+        });
+        solid
+    }
+
     /// Whether this block and the block above are passable
-    pub fn is_passable(&self, pos: &BlockPos) -> bool {
-        self.is_block_passable(pos) && self.is_block_passable(&pos.up(1))
+    pub fn is_passable(&self, pos: BlockPos) -> bool {
+        self.is_block_passable(pos) && self.is_block_passable(pos.up(1))
     }
 
     /// Whether we can stand in this position. Checks if the block below is
     /// solid, and that the two blocks above that are passable.
 
-    pub fn is_standable(&self, pos: &BlockPos) -> bool {
-        self.is_block_solid(&pos.down(1)) && self.is_passable(pos)
+    pub fn is_standable(&self, pos: BlockPos) -> bool {
+        self.is_block_solid(pos.down(1)) && self.is_passable(pos)
     }
 
     /// Get the amount of air blocks until the next solid block below this one.
-    pub fn fall_distance(&self, pos: &BlockPos) -> u32 {
+    pub fn fall_distance(&self, pos: BlockPos) -> u32 {
         let mut distance = 0;
         let mut current_pos = pos.down(1);
-        while self.is_block_passable(&current_pos) {
+        while self.is_block_passable(current_pos) {
             distance += 1;
             current_pos = current_pos.down(1);
 
@@ -215,8 +313,8 @@ mod tests {
             .set_block_state(&BlockPos::new(0, 1, 0), BlockState::AIR, &world);
 
         let ctx = PathfinderCtx::new(Arc::new(RwLock::new(world.into())));
-        assert!(!ctx.is_block_passable(&BlockPos::new(0, 0, 0)));
-        assert!(ctx.is_block_passable(&BlockPos::new(0, 1, 0),));
+        assert!(!ctx.is_block_passable(BlockPos::new(0, 0, 0)));
+        assert!(ctx.is_block_passable(BlockPos::new(0, 1, 0),));
     }
 
     #[test]
@@ -236,8 +334,8 @@ mod tests {
             .set_block_state(&BlockPos::new(0, 1, 0), BlockState::AIR, &world);
 
         let ctx = PathfinderCtx::new(Arc::new(RwLock::new(world.into())));
-        assert!(ctx.is_block_solid(&BlockPos::new(0, 0, 0)));
-        assert!(!ctx.is_block_solid(&BlockPos::new(0, 1, 0)));
+        assert!(ctx.is_block_solid(BlockPos::new(0, 0, 0)));
+        assert!(!ctx.is_block_solid(BlockPos::new(0, 1, 0)));
     }
 
     #[test]
@@ -263,8 +361,8 @@ mod tests {
             .set_block_state(&BlockPos::new(0, 3, 0), BlockState::AIR, &world);
 
         let ctx = PathfinderCtx::new(Arc::new(RwLock::new(world.into())));
-        assert!(ctx.is_standable(&BlockPos::new(0, 1, 0)));
-        assert!(!ctx.is_standable(&BlockPos::new(0, 0, 0)));
-        assert!(!ctx.is_standable(&BlockPos::new(0, 2, 0)));
+        assert!(ctx.is_standable(BlockPos::new(0, 1, 0)));
+        assert!(!ctx.is_standable(BlockPos::new(0, 0, 0)));
+        assert!(!ctx.is_standable(BlockPos::new(0, 2, 0)));
     }
 }