BLAZINGLY FAST 🚀🚀🚀 pathfinding

azalea-core/src/position.rs

@@ -261,6 +261,7 @@ impl ChunkSectionPos {
 }
 /// The coordinates of a block inside a chunk.
 #[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
+#[repr(align(8))]
 pub struct ChunkBlockPos {
     pub x: u8,
     pub y: i32,
@@ -273,9 +274,33 @@ impl ChunkBlockPos {
     }
 }
 
+impl Hash for ChunkBlockPos {
+    // optimized hash that only calls hash once
+    #[inline]
+    fn hash<H: std::hash::Hasher>(&self, state: &mut H) {
+        u64::from(*self).hash(state);
+    }
+}
+impl From<ChunkBlockPos> for u64 {
+    #[inline]
+    fn from(pos: ChunkBlockPos) -> Self {
+        // convert to u64
+        let mut val: u64 = 0;
+        // first 32 bits are y
+        val |= pos.y as u64;
+        // next 8 bits are z
+        val |= (pos.z as u64) << 32;
+        // last 8 bits are x
+        val |= (pos.x as u64) << 40;
+        val
+    }
+}
+impl nohash_hasher::IsEnabled for ChunkBlockPos {}
+
 /// The coordinates of a block inside a chunk section. Each coordinate must be
 /// in the range [0, 15].
 #[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
+#[repr(align(4))]
 pub struct ChunkSectionBlockPos {
     pub x: u8,
     pub y: u8,
@@ -294,6 +319,28 @@ impl Add<ChunkSectionBlockPos> for ChunkSectionPos {
         )
     }
 }
+impl Hash for ChunkSectionBlockPos {
+    // optimized hash that only calls hash once
+    #[inline]
+    fn hash<H: std::hash::Hasher>(&self, state: &mut H) {
+        u16::from(*self).hash(state);
+    }
+}
+
+impl From<ChunkSectionBlockPos> for u16 {
+    #[inline]
+    fn from(pos: ChunkSectionBlockPos) -> Self {
+        let mut val: u16 = 0;
+        // first 4 bits are z
+        val |= pos.z as u16;
+        // next 4 bits are y
+        val |= (pos.y as u16) << 4;
+        // last 4 bits are x
+        val |= (pos.x as u16) << 8;
+        val
+    }
+}
+impl nohash_hasher::IsEnabled for ChunkSectionBlockPos {}
 
 /// A block pos with an attached world
 #[derive(Debug, Clone)]
@@ -312,22 +359,33 @@ impl From<&BlockPos> for ChunkPos {
         }
     }
 }
-
-impl From<BlockPos> for ChunkSectionPos {
+impl From<BlockPos> for ChunkPos {
+    #[inline]
     fn from(pos: BlockPos) -> Self {
-        ChunkSectionPos {
+        ChunkPos {
             x: pos.x.div_floor(16),
-            y: pos.y.div_floor(16),
             z: pos.z.div_floor(16),
         }
     }
 }
+
+impl From<BlockPos> for ChunkSectionPos {
+    #[inline]
+    fn from(pos: BlockPos) -> Self {
+        ChunkSectionPos {
+            x: pos.x >> 4,
+            y: pos.y >> 4,
+            z: pos.z >> 4,
+        }
+    }
+}
 impl From<&BlockPos> for ChunkSectionPos {
+    #[inline]
     fn from(pos: &BlockPos) -> Self {
         ChunkSectionPos {
-            x: pos.x.div_floor(16),
-            y: pos.y.div_floor(16),
-            z: pos.z.div_floor(16),
+            x: pos.x >> 4,
+            y: pos.y >> 4,
+            z: pos.z >> 4,
         }
     }
 }
@@ -348,17 +406,28 @@ impl From<&BlockPos> for ChunkBlockPos {
         }
     }
 }
-
-impl From<&BlockPos> for ChunkSectionBlockPos {
-    fn from(pos: &BlockPos) -> Self {
-        ChunkSectionBlockPos {
+impl From<BlockPos> for ChunkBlockPos {
+    #[inline]
+    fn from(pos: BlockPos) -> Self {
+        ChunkBlockPos {
             x: pos.x.rem_euclid(16) as u8,
-            y: pos.y.rem_euclid(16) as u8,
+            y: pos.y,
             z: pos.z.rem_euclid(16) as u8,
         }
     }
 }
 
+impl From<BlockPos> for ChunkSectionBlockPos {
+    #[inline]
+    fn from(pos: BlockPos) -> Self {
+        ChunkSectionBlockPos {
+            x: pos.x as u8 & 0xF,
+            y: pos.y as u8 & 0xF,
+            z: pos.z as u8 & 0xF,
+        }
+    }
+}
+
 impl From<&ChunkBlockPos> for ChunkSectionBlockPos {
     #[inline]
     fn from(pos: &ChunkBlockPos) -> Self {
@@ -529,7 +598,7 @@ mod tests {
     fn test_into_chunk_section_block_pos() {
         let block_pos = BlockPos::new(0, -60, 0);
         assert_eq!(
-            ChunkSectionBlockPos::from(&block_pos),
+            ChunkSectionBlockPos::from(block_pos),
             ChunkSectionBlockPos::new(0, 4, 0)
         );
     }

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

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)));
     }
 }

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;
         }