BLAZINGLY FAST 🚀🚀🚀 pathfinding

3 files changed, 144 insertions, 46 deletions

diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index dfb90bd0..013b3c19 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -14,7 +14,7 @@ use crate::{
 use super::{default_is_reached, Edge, ExecuteCtx, IsReachedCtx, MoveData, PathfinderCtx};
 
 pub fn basic_move(ctx: &PathfinderCtx, node: BlockPos) -> Vec<Edge> {
-    let mut edges = Vec::new();
+    let mut edges = Vec::with_capacity(8);
     edges.extend(forward_move(ctx, node));
     edges.extend(ascend_move(ctx, node));
     edges.extend(descend_move(ctx, node));
@@ -27,7 +27,7 @@ fn forward_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 0, dir.z());
 
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
 
@@ -73,10 +73,10 @@ fn ascend_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 1, dir.z());
 
-        if !ctx.is_block_passable(&pos.up(2)) {
+        if !ctx.is_block_passable(pos.up(2)) {
             continue;
         }
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
 
@@ -157,18 +157,18 @@ fn descend_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
     for dir in CardinalDirection::iter() {
         let dir_delta = BlockPos::new(dir.x(), 0, dir.z());
         let new_horizontal_position = pos + dir_delta;
-        let fall_distance = ctx.fall_distance(&new_horizontal_position);
+        let fall_distance = ctx.fall_distance(new_horizontal_position);
         if fall_distance == 0 || fall_distance > 3 {
             continue;
         }
         let new_position = new_horizontal_position.down(fall_distance as i32);
 
         // check whether 3 blocks vertically forward are passable
-        if !ctx.is_passable(&new_horizontal_position) {
+        if !ctx.is_passable(new_horizontal_position) {
             continue;
         }
         // check whether we can stand on the target position
-        if !ctx.is_standable(&new_position) {
+        if !ctx.is_standable(new_position) {
             continue;
         }
 
@@ -266,8 +266,8 @@ fn diagonal_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
         let right = dir.right();
         let offset = BlockPos::new(dir.x() + right.x(), 0, dir.z() + right.z());
 
-        if !ctx.is_passable(&BlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z()))
-            && !ctx.is_passable(&BlockPos::new(
+        if !ctx.is_passable(BlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z()))
+            && !ctx.is_passable(BlockPos::new(
                 pos.x + dir.right().x(),
                 pos.y,
                 pos.z + dir.right().z(),
@@ -275,7 +275,7 @@ fn diagonal_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
         {
             continue;
         }
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
         // +0.001 so it doesn't unnecessarily go diagonal sometimes
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)));
     }
 }
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index 77e591d6..d14da735 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -23,20 +23,20 @@ fn parkour_forward_1_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
         let offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
 
         // make sure we actually have to jump
-        if ctx.is_block_solid(&(pos + gap_offset).down(1)) {
+        if ctx.is_block_solid((pos + gap_offset).down(1)) {
             continue;
         }
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
-        if !ctx.is_passable(&(pos + gap_offset)) {
+        if !ctx.is_passable(pos + gap_offset) {
             continue;
         }
-        if !ctx.is_block_passable(&(pos + gap_offset).up(2)) {
+        if !ctx.is_block_passable((pos + gap_offset).up(2)) {
             continue;
         }
         // make sure it's not a headhitter
-        if !ctx.is_block_passable(&pos.up(2)) {
+        if !ctx.is_block_passable(pos.up(2)) {
             continue;
         }
 
@@ -65,29 +65,29 @@ fn parkour_forward_2_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<Edge> {
         let offset = BlockPos::new(dir.x() * 3, 0, dir.z() * 3);
 
         // make sure we actually have to jump
-        if ctx.is_block_solid(&(pos + gap_1_offset).down(1))
-            || ctx.is_block_solid(&(pos + gap_2_offset).down(1))
+        if ctx.is_block_solid((pos + gap_1_offset).down(1))
+            || ctx.is_block_solid((pos + gap_2_offset).down(1))
         {
             continue;
         }
 
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
-        if !ctx.is_passable(&(pos + gap_1_offset)) {
+        if !ctx.is_passable(pos + gap_1_offset) {
             continue;
         }
-        if !ctx.is_block_passable(&(pos + gap_1_offset).up(2)) {
+        if !ctx.is_block_passable((pos + gap_1_offset).up(2)) {
             continue;
         }
-        if !ctx.is_passable(&(pos + gap_2_offset)) {
+        if !ctx.is_passable(pos + gap_2_offset) {
             continue;
         }
-        if !ctx.is_block_passable(&(pos + gap_2_offset).up(2)) {
+        if !ctx.is_block_passable((pos + gap_2_offset).up(2)) {
             continue;
         }
         // make sure it's not a headhitter
-        if !ctx.is_block_passable(&pos.up(2)) {
+        if !ctx.is_block_passable(pos.up(2)) {
             continue;
         }
 
@@ -115,20 +115,20 @@ fn parkour_headhitter_forward_1_move(ctx: &PathfinderCtx, pos: BlockPos) -> Vec<
         let offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
 
         // make sure we actually have to jump
-        if ctx.is_block_solid(&(pos + gap_offset).down(1)) {
+        if ctx.is_block_solid((pos + gap_offset).down(1)) {
             continue;
         }
-        if !ctx.is_standable(&(pos + offset)) {
+        if !ctx.is_standable(pos + offset) {
             continue;
         }
-        if !ctx.is_passable(&(pos + gap_offset)) {
+        if !ctx.is_passable(pos + gap_offset) {
             continue;
         }
-        if !ctx.is_block_passable(&(pos + gap_offset).up(2)) {
+        if !ctx.is_block_passable((pos + gap_offset).up(2)) {
             continue;
         }
         // make sure it is a headhitter
-        if !ctx.is_block_solid(&pos.up(2)) {
+        if !ctx.is_block_solid(pos.up(2)) {
             continue;
         }