optimize pathfinder

9 files changed, 270 insertions, 90 deletions

diff --git a/azalea/Cargo.toml b/azalea/Cargo.toml
index d1bf4884..3e5aa275 100644
--- a/azalea/Cargo.toml
+++ b/azalea/Cargo.toml
@@ -30,7 +30,7 @@ bevy_app = { workspace = true }
 bevy_ecs = { workspace = true }
 bevy_log = { workspace = true }
 bevy_tasks = { workspace = true, features = ["multi_threaded"] }
-#bevy_time = { workspace = true }
+# bevy_time = { workspace = true }
 derive_more = { workspace = true, features = ["deref", "deref_mut"] }
 futures = { workspace = true }
 futures-lite = { workspace = true }
diff --git a/azalea/benches/pathfinder.rs b/azalea/benches/pathfinder.rs
index bd47a3bf..d172b9e4 100644
--- a/azalea/benches/pathfinder.rs
+++ b/azalea/benches/pathfinder.rs
@@ -5,6 +5,7 @@ use azalea::{
         astar::{self, a_star, PathfinderTimeout},
         goals::{BlockPosGoal, Goal},
         mining::MiningCache,
+        rel_block_pos::RelBlockPos,
         world::CachedWorld,
     },
     BlockPos,
@@ -124,21 +125,23 @@ fn run_pathfinder_benchmark(
 
     let (world, start, end) = generate_world(&mut partial_chunks, 4);
 
+    let origin = start;
+
     b.iter(|| {
-        let cached_world = CachedWorld::new(Arc::new(RwLock::new(world.clone().into())));
+        let cached_world = CachedWorld::new(Arc::new(RwLock::new(world.clone().into())), origin);
         let mining_cache =
             MiningCache::new(Some(Menu::Player(azalea_inventory::Player::default())));
         let goal = BlockPosGoal(end);
 
-        let successors = |pos: BlockPos| {
+        let successors = |pos: RelBlockPos| {
             azalea::pathfinder::call_successors_fn(&cached_world, &mining_cache, successors_fn, pos)
         };
 
         let astar::Path { movements, partial } = a_star(
-            start,
-            |n| goal.heuristic(n),
+            RelBlockPos::get_origin(origin),
+            |n| goal.heuristic(n.apply(origin)),
             successors,
-            |n| goal.success(n),
+            |n| goal.success(n.apply(origin)),
             PathfinderTimeout::Time(Duration::MAX),
         );
 
diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 6573c883..ef932c24 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -74,6 +74,7 @@ where
         num_nodes += 1;
         if success(current_node) {
             debug!("Nodes considered: {num_nodes}");
+
             return Path {
                 movements: reconstruct_path(nodes, current_node),
                 partial: false,
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 8ad0b1d0..b750301e 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -8,6 +8,7 @@ mod debug;
 pub mod goals;
 pub mod mining;
 pub mod moves;
+pub mod rel_block_pos;
 pub mod simulation;
 pub mod world;
 
@@ -35,6 +36,7 @@ use bevy_ecs::schedule::IntoSystemConfigs;
 use bevy_tasks::{AsyncComputeTaskPool, Task};
 use futures_lite::future;
 use parking_lot::RwLock;
+use rel_block_pos::RelBlockPos;
 use tracing::{debug, error, info, trace, warn};
 
 use self::debug::debug_render_path_with_particles;
@@ -321,8 +323,9 @@ pub async fn calculate_path(opts: CalculatePathOpts) -> Option<PathFoundEvent> {
 
     let goto_id = opts.goto_id_atomic.fetch_add(1, atomic::Ordering::SeqCst) + 1;
 
-    let cached_world = CachedWorld::new(opts.world_lock);
-    let successors = |pos: BlockPos| {
+    let origin = opts.start;
+    let cached_world = CachedWorld::new(opts.world_lock, origin);
+    let successors = |pos: RelBlockPos| {
         call_successors_fn(&cached_world, &opts.mining_cache, opts.successors_fn, pos)
     };
 
@@ -345,10 +348,10 @@ pub async fn calculate_path(opts: CalculatePathOpts) -> Option<PathFoundEvent> {
         };
 
         let astar::Path { movements, partial } = a_star(
-            opts.start,
-            |n| opts.goal.heuristic(n),
+            RelBlockPos::get_origin(origin),
+            |n| opts.goal.heuristic(n.apply(origin)),
             successors,
-            |n| opts.goal.success(n),
+            |n| opts.goal.success(n.apply(origin)),
             timeout,
         );
         let end_time = Instant::now();
@@ -368,7 +371,7 @@ pub async fn calculate_path(opts: CalculatePathOpts) -> Option<PathFoundEvent> {
 
         debug!("Path:");
         for movement in &movements {
-            debug!("  {}", movement.target);
+            debug!("  {}", movement.target.apply(origin));
         }
 
         path = movements.into_iter().collect::<VecDeque<_>>();
@@ -394,10 +397,19 @@ pub async fn calculate_path(opts: CalculatePathOpts) -> Option<PathFoundEvent> {
         break;
     }
 
+    // replace the RelBlockPos types with BlockPos
+    let mapped_path = path
+        .into_iter()
+        .map(|movement| astar::Movement {
+            target: movement.target.apply(origin),
+            data: movement.data,
+        })
+        .collect();
+
     Some(PathFoundEvent {
         entity: opts.entity,
         start: opts.start,
-        path: Some(path),
+        path: Some(mapped_path),
         is_partial,
         successors_fn: opts.successors_fn,
         allow_mining: opts.allow_mining,
@@ -448,21 +460,27 @@ pub fn path_found_listener(
                     let world_lock = instance_container
                         .get(instance_name)
                         .expect("Entity tried to pathfind but the entity isn't in a valid world");
+                    let origin = event.start;
                     let successors_fn: moves::SuccessorsFn = event.successors_fn;
-                    let cached_world = CachedWorld::new(world_lock);
+                    let cached_world = CachedWorld::new(world_lock, origin);
                     let mining_cache = MiningCache::new(if event.allow_mining {
                         Some(inventory.inventory_menu.clone())
                     } else {
                         None
                     });
-                    let successors = |pos: BlockPos| {
+                    let successors = |pos: RelBlockPos| {
                         call_successors_fn(&cached_world, &mining_cache, successors_fn, pos)
                     };
 
                     if let Some(first_node_of_new_path) = path.front() {
-                        if successors(last_node_of_current_path.target)
+                        let last_target_of_current_path =
+                            RelBlockPos::from_origin(origin, last_node_of_current_path.target);
+                        let first_target_of_new_path =
+                            RelBlockPos::from_origin(origin, first_node_of_new_path.target);
+
+                        if successors(last_target_of_current_path)
                             .iter()
-                            .any(|edge| edge.movement.target == first_node_of_new_path.target)
+                            .any(|edge| edge.movement.target == first_target_of_new_path)
                         {
                             debug!("combining old and new paths");
                             debug!(
@@ -655,17 +673,19 @@ pub fn check_for_path_obstruction(
             .expect("Entity tried to pathfind but the entity isn't in a valid world");
 
         // obstruction check (the path we're executing isn't possible anymore)
-        let cached_world = CachedWorld::new(world_lock);
+        let origin = executing_path.last_reached_node;
+        let cached_world = CachedWorld::new(world_lock, origin);
         let mining_cache = MiningCache::new(if pathfinder.allow_mining {
             Some(inventory.inventory_menu.clone())
         } else {
             None
         });
         let successors =
-            |pos: BlockPos| call_successors_fn(&cached_world, &mining_cache, successors_fn, pos);
+            |pos: RelBlockPos| call_successors_fn(&cached_world, &mining_cache, successors_fn, pos);
 
         if let Some(obstructed_index) = check_path_obstructed(
-            executing_path.last_reached_node,
+            origin,
+            RelBlockPos::from_origin(origin, executing_path.last_reached_node),
             &executing_path.path,
             successors,
         ) {
@@ -873,18 +893,21 @@ pub fn stop_pathfinding_on_instance_change(
 /// Checks whether the path has been obstructed, and returns Some(index) if it
 /// has been. The index is of the first obstructed node.
 pub fn check_path_obstructed<SuccessorsFn>(
-    mut current_position: BlockPos,
+    origin: BlockPos,
+    mut current_position: RelBlockPos,
     path: &VecDeque<astar::Movement<BlockPos, moves::MoveData>>,
     successors_fn: SuccessorsFn,
 ) -> Option<usize>
 where
-    SuccessorsFn: Fn(BlockPos) -> Vec<astar::Edge<BlockPos, moves::MoveData>>,
+    SuccessorsFn: Fn(RelBlockPos) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>>,
 {
     for (i, movement) in path.iter().enumerate() {
+        let movement_target = RelBlockPos::from_origin(origin, movement.target);
+
         let mut found_obstruction = false;
         for edge in successors_fn(current_position) {
-            if edge.movement.target == movement.target {
-                current_position = movement.target;
+            if edge.movement.target == movement_target {
+                current_position = movement_target;
                 found_obstruction = false;
                 break;
             } else {
@@ -903,8 +926,8 @@ pub fn call_successors_fn(
     cached_world: &CachedWorld,
     mining_cache: &MiningCache,
     successors_fn: SuccessorsFn,
-    pos: BlockPos,
-) -> Vec<astar::Edge<BlockPos, moves::MoveData>> {
+    pos: RelBlockPos,
+) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>> {
     let mut edges = Vec::with_capacity(16);
     let mut ctx = PathfinderCtx {
         edges: &mut edges,
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 89ba9acc..8a679376 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -7,9 +7,9 @@ use azalea_core::{
 };
 
 use super::{default_is_reached, Edge, ExecuteCtx, IsReachedCtx, MoveData, PathfinderCtx};
-use crate::pathfinder::{astar, costs::*};
+use crate::pathfinder::{astar, costs::*, rel_block_pos::RelBlockPos};
 
-pub fn basic_move(ctx: &mut PathfinderCtx, node: BlockPos) {
+pub fn basic_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     forward_move(ctx, node);
     ascend_move(ctx, node);
     descend_move(ctx, node);
@@ -18,9 +18,9 @@ pub fn basic_move(ctx: &mut PathfinderCtx, node: BlockPos) {
     downward_move(ctx, node);
 }
 
-fn forward_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn forward_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
-        let offset = BlockPos::new(dir.x(), 0, dir.z());
+        let offset = RelBlockPos::new(dir.x(), 0, dir.z());
 
         let mut cost = SPRINT_ONE_BLOCK_COST;
 
@@ -57,9 +57,9 @@ fn execute_forward_move(mut ctx: ExecuteCtx) {
     ctx.sprint(SprintDirection::Forward);
 }
 
-fn ascend_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn ascend_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
-        let offset = BlockPos::new(dir.x(), 1, dir.z());
+        let offset = RelBlockPos::new(dir.x(), 1, dir.z());
 
         let break_cost_1 = ctx
             .world
@@ -126,7 +126,7 @@ fn execute_ascend_move(mut ctx: ExecuteCtx) {
         + x_axis as f64 * (target_center.z - position.z).abs();
 
     let lateral_motion = x_axis as f64 * physics.velocity.z + z_axis as f64 * physics.velocity.x;
-    if lateral_motion > 0.1 {
+    if lateral_motion.abs() > 0.1 {
         return;
     }
 
@@ -147,9 +147,9 @@ pub fn ascend_is_reached(
     BlockPos::from(position) == target || BlockPos::from(position) == target.down(1)
 }
 
-fn descend_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn descend_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
-        let dir_delta = BlockPos::new(dir.x(), 0, dir.z());
+        let dir_delta = RelBlockPos::new(dir.x(), 0, dir.z());
         let new_horizontal_position = pos + dir_delta;
 
         let break_cost_1 = ctx
@@ -271,9 +271,9 @@ pub fn descend_is_reached(
         && (position.y - target.y as f64) < 0.5
 }
 
-fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
-        let dir_delta = BlockPos::new(dir.x(), 0, dir.z());
+        let dir_delta = RelBlockPos::new(dir.x(), 0, dir.z());
         let gap_horizontal_position = pos + dir_delta;
         let new_horizontal_position = pos + dir_delta * 2;
 
@@ -323,12 +323,12 @@ fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     }
 }
 
-fn diagonal_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn diagonal_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
         let right = dir.right();
-        let offset = BlockPos::new(dir.x() + right.x(), 0, dir.z() + right.z());
-        let left_pos = BlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z());
-        let right_pos = BlockPos::new(pos.x + right.x(), pos.y, pos.z + right.z());
+        let offset = RelBlockPos::new(dir.x() + right.x(), 0, dir.z() + right.z());
+        let left_pos = RelBlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z());
+        let right_pos = RelBlockPos::new(pos.x + right.x(), pos.y, pos.z + right.z());
 
         // +0.001 so it doesn't unnecessarily go diagonal sometimes
         let mut cost = SPRINT_ONE_BLOCK_COST * SQRT_2 + 0.001;
@@ -369,7 +369,7 @@ fn execute_diagonal_move(mut ctx: ExecuteCtx) {
 }
 
 /// Go directly down, usually by mining.
-fn downward_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn downward_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     // make sure we land on a solid block after breaking the one below us
     if !ctx.world.is_block_solid(pos.down(2)) {
         return;
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 1a435b5f..68c65d5d 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -16,15 +16,16 @@ use parking_lot::RwLock;
 use super::{
     astar,
     mining::MiningCache,
+    rel_block_pos::RelBlockPos,
     world::{is_block_state_passable, CachedWorld},
 };
 use crate::{auto_tool::best_tool_in_hotbar_for_block, JumpEvent, LookAtEvent};
 
-type Edge = astar::Edge<BlockPos, MoveData>;
+type Edge = astar::Edge<RelBlockPos, MoveData>;
 
-pub type SuccessorsFn = fn(&mut PathfinderCtx, BlockPos);
+pub type SuccessorsFn = fn(&mut PathfinderCtx, RelBlockPos);
 
-pub fn default_move(ctx: &mut PathfinderCtx, node: BlockPos) {
+pub fn default_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     basic::basic_move(ctx, node);
     parkour::parkour_move(ctx, node);
 }
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index 1d35f323..1816a5e1 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -3,18 +3,18 @@ use azalea_core::{direction::CardinalDirection, position::BlockPos};
 use tracing::trace;
 
 use super::{Edge, ExecuteCtx, IsReachedCtx, MoveData, PathfinderCtx};
-use crate::pathfinder::{astar, costs::*};
+use crate::pathfinder::{astar, costs::*, rel_block_pos::RelBlockPos};
 
-pub fn parkour_move(ctx: &mut PathfinderCtx, node: BlockPos) {
+pub fn parkour_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     parkour_forward_1_move(ctx, node);
     parkour_forward_2_move(ctx, node);
     parkour_forward_3_move(ctx, node);
 }
 
-fn parkour_forward_1_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn parkour_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
-        let gap_offset = BlockPos::new(dir.x(), 0, dir.z());
-        let offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
+        let gap_offset = RelBlockPos::new(dir.x(), 0, dir.z());
+        let offset = RelBlockPos::new(dir.x() * 2, 0, dir.z() * 2);
 
         // make sure we actually have to jump
         if ctx.world.is_block_solid((pos + gap_offset).down(1)) {
@@ -63,11 +63,11 @@ fn parkour_forward_1_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     }
 }
 
-fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     'dir: for dir in CardinalDirection::iter() {
-        let gap_1_offset = BlockPos::new(dir.x(), 0, dir.z());
-        let gap_2_offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
-        let offset = BlockPos::new(dir.x() * 3, 0, dir.z() * 3);
+        let gap_1_offset = RelBlockPos::new(dir.x(), 0, dir.z());
+        let gap_2_offset = RelBlockPos::new(dir.x() * 2, 0, dir.z() * 2);
+        let offset = RelBlockPos::new(dir.x() * 3, 0, dir.z() * 3);
 
         // make sure we actually have to jump
         if ctx.world.is_block_solid((pos + gap_1_offset).down(1))
@@ -117,12 +117,12 @@ fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     }
 }
 
-fn parkour_forward_3_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+fn parkour_forward_3_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     'dir: for dir in CardinalDirection::iter() {
-        let gap_1_offset = BlockPos::new(dir.x(), 0, dir.z());
-        let gap_2_offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
-        let gap_3_offset = BlockPos::new(dir.x() * 3, 0, dir.z() * 3);
-        let offset = BlockPos::new(dir.x() * 4, 0, dir.z() * 4);
+        let gap_1_offset = RelBlockPos::new(dir.x(), 0, dir.z());
+        let gap_2_offset = RelBlockPos::new(dir.x() * 2, 0, dir.z() * 2);
+        let gap_3_offset = RelBlockPos::new(dir.x() * 3, 0, dir.z() * 3);
+        let offset = RelBlockPos::new(dir.x() * 4, 0, dir.z() * 4);
 
         // make sure we actually have to jump
         if ctx.world.is_block_solid((pos + gap_1_offset).down(1))
diff --git a/azalea/src/pathfinder/rel_block_pos.rs b/azalea/src/pathfinder/rel_block_pos.rs
new file mode 100644
index 00000000..be5583aa
--- /dev/null
+++ b/azalea/src/pathfinder/rel_block_pos.rs
@@ -0,0 +1,114 @@
+use std::ops::{Add, Mul};
+
+use azalea_core::position::BlockPos;
+
+/// An offset from a block position.
+///
+/// This fits in 64 bits, so it's more efficient than a BlockPos in some cases.
+///
+/// The X and Z are limited to ±32k.
+#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)]
+pub struct RelBlockPos {
+    pub x: i16,
+    /// Note that the y isn't relative.
+    pub y: i32,
+    pub z: i16,
+}
+
+impl RelBlockPos {
+    pub fn get_origin(origin: BlockPos) -> Self {
+        Self::new(0, origin.y, 0)
+    }
+
+    #[inline]
+    pub fn new(x: i16, y: i32, z: i16) -> Self {
+        Self { x, y, z }
+    }
+
+    #[inline]
+    pub fn apply(self, origin: BlockPos) -> BlockPos {
+        BlockPos::new(origin.x + self.x as i32, self.y, origin.z + self.z as i32)
+    }
+
+    /// Create a new [`RelBlockPos`] from a given origin and new position.
+    #[inline]
+    pub fn from_origin(origin: BlockPos, new: BlockPos) -> Self {
+        Self {
+            x: (new.x - origin.x) as i16,
+            y: new.y,
+            z: (new.z - origin.z) as i16,
+        }
+    }
+
+    #[inline]
+    pub fn up(&self, y: i32) -> Self {
+        Self {
+            x: self.x,
+            y: self.y + y,
+            z: self.z,
+        }
+    }
+    #[inline]
+    pub fn down(&self, y: i32) -> Self {
+        Self {
+            x: self.x,
+            y: self.y - y,
+            z: self.z,
+        }
+    }
+    #[inline]
+    pub fn north(&self, z: i16) -> Self {
+        Self {
+            x: self.x,
+            y: self.y,
+            z: self.z - z,
+        }
+    }
+    #[inline]
+    pub fn south(&self, z: i16) -> Self {
+        Self {
+            x: self.x,
+            y: self.y,
+            z: self.z + z,
+        }
+    }
+    #[inline]
+    pub fn east(&self, x: i16) -> Self {
+        Self {
+            x: self.x + x,
+            y: self.y,
+            z: self.z,
+        }
+    }
+    #[inline]
+    pub fn west(&self, x: i16) -> Self {
+        Self {
+            x: self.x - x,
+            y: self.y,
+            z: self.z,
+        }
+    }
+}
+
+impl Add<RelBlockPos> for RelBlockPos {
+    type Output = RelBlockPos;
+
+    fn add(self, rhs: RelBlockPos) -> Self::Output {
+        Self {
+            x: self.x + rhs.x,
+            y: self.y + rhs.y,
+            z: self.z + rhs.z,
+        }
+    }
+}
+impl Mul<i16> for RelBlockPos {
+    type Output = RelBlockPos;
+
+    fn mul(self, rhs: i16) -> Self::Output {
+        Self {
+            x: self.x * rhs,
+            y: self.y * rhs as i32,
+            z: self.z * rhs,
+        }
+    }
+}
diff --git a/azalea/src/pathfinder/world.rs b/azalea/src/pathfinder/world.rs
index 255f99d3..cff0e5e6 100644
--- a/azalea/src/pathfinder/world.rs
+++ b/azalea/src/pathfinder/world.rs
@@ -11,12 +11,16 @@ use azalea_core::{
 use azalea_physics::collision::BlockWithShape;
 use azalea_world::Instance;
 use parking_lot::RwLock;
-use rustc_hash::FxHashMap;
 
-use super::mining::MiningCache;
+use super::{mining::MiningCache, rel_block_pos::RelBlockPos};
 
 /// An efficient representation of the world used for the pathfinder.
 pub struct CachedWorld {
+    /// The origin that the [`RelBlockPos`] types will be relative to. This is
+    /// for an optimization that reduces the size of the block positions
+    /// that are used by the pathfinder.
+    origin: BlockPos,
+
     min_y: i32,
     world_lock: Arc<RwLock<Instance>>,
 
@@ -27,7 +31,7 @@ pub struct CachedWorld {
 
     cached_blocks: UnsafeCell<CachedSections>,
 
-    cached_mining_costs: UnsafeCell<FxHashMap<BlockPos, f32>>,
+    cached_mining_costs: UnsafeCell<Box<[(RelBlockPos, f32)]>>,
 }
 
 #[derive(Default)]
@@ -82,15 +86,20 @@ pub struct CachedSection {
 }
 
 impl CachedWorld {
-    pub fn new(world_lock: Arc<RwLock<Instance>>) -> Self {
+    pub fn new(world_lock: Arc<RwLock<Instance>>, origin: BlockPos) -> Self {
         let min_y = world_lock.read().chunks.min_y;
         Self {
+            origin,
             min_y,
             world_lock,
             cached_chunks: Default::default(),
             last_chunk_cache_index: Default::default(),
             cached_blocks: Default::default(),
-            cached_mining_costs: Default::default(),
+            // this uses about 12mb of memory. it *really* helps though.
+            cached_mining_costs: UnsafeCell::new(
+                vec![(RelBlockPos::new(i16::MAX, i32::MAX, i16::MAX), 0.); 2usize.pow(20)]
+                    .into_boxed_slice(),
+            ),
         }
     }
 
@@ -202,7 +211,11 @@ impl CachedWorld {
         })
     }
 
-    pub fn is_block_passable(&self, pos: BlockPos) -> bool {
+    pub fn is_block_passable(&self, pos: RelBlockPos) -> bool {
+        self.is_block_pos_passable(pos.apply(self.origin))
+    }
+
+    fn is_block_pos_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;
@@ -220,7 +233,11 @@ impl CachedWorld {
         passable
     }
 
-    pub fn is_block_solid(&self, pos: BlockPos) -> bool {
+    pub fn is_block_solid(&self, pos: RelBlockPos) -> bool {
+        self.is_block_pos_solid(pos.apply(self.origin))
+    }
+
+    fn is_block_pos_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;
@@ -240,25 +257,40 @@ impl CachedWorld {
 
     /// Returns how much it costs to break this block. Returns 0 if the block is
     /// already passable.
-    pub fn cost_for_breaking_block(&self, pos: BlockPos, mining_cache: &MiningCache) -> f32 {
+    pub fn cost_for_breaking_block(&self, pos: RelBlockPos, mining_cache: &MiningCache) -> f32 {
         // SAFETY: pathfinding is single-threaded
         let cached_mining_costs = unsafe { &mut *self.cached_mining_costs.get() };
-
-        if let Some(&cost) = cached_mining_costs.get(&pos) {
-            return cost;
+        // 20 bits total:
+        // 8 bits for x, 4 bits for y, 8 bits for z
+        let hash_index =
+            (pos.x as usize & 0xff) << 12 | (pos.y as usize & 0xf) << 8 | (pos.z as usize & 0xff);
+        debug_assert!(hash_index < 1048576);
+        let &(cached_pos, potential_cost) =
+            unsafe { cached_mining_costs.get_unchecked(hash_index) };
+        if cached_pos == pos {
+            return potential_cost;
         }
 
         let cost = self.uncached_cost_for_breaking_block(pos, mining_cache);
-        cached_mining_costs.insert(pos, cost);
+        unsafe {
+            *cached_mining_costs.get_unchecked_mut(hash_index) = (pos, cost);
+        };
+
         cost
     }
 
-    fn uncached_cost_for_breaking_block(&self, pos: BlockPos, mining_cache: &MiningCache) -> f32 {
+    fn uncached_cost_for_breaking_block(
+        &self,
+        pos: RelBlockPos,
+        mining_cache: &MiningCache,
+    ) -> f32 {
         if self.is_block_passable(pos) {
             // if the block is passable then it doesn't need to be broken
             return 0.;
         }
 
+        let pos = pos.apply(self.origin);
+
         let (section_pos, section_block_pos) =
             (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));
 
@@ -341,7 +373,7 @@ impl CachedWorld {
             mining_cost
         }) else {
             // the chunk isn't loaded
-            let cost = if self.is_block_solid(pos) {
+            let cost = if self.is_block_pos_solid(pos) {
                 // assume it's unbreakable if it's solid and out of render distance
                 f32::INFINITY
             } else {
@@ -400,22 +432,28 @@ impl CachedWorld {
     }
 
     /// 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: RelBlockPos) -> bool {
+        self.is_passable_at_block_pos(pos.apply(self.origin))
+    }
+    fn is_passable_at_block_pos(&self, pos: BlockPos) -> bool {
+        self.is_block_pos_passable(pos) && self.is_block_pos_passable(pos.up(1))
     }
 
-    pub fn cost_for_passing(&self, pos: BlockPos, mining_cache: &MiningCache) -> f32 {
+    pub fn cost_for_passing(&self, pos: RelBlockPos, mining_cache: &MiningCache) -> f32 {
         self.cost_for_breaking_block(pos, mining_cache)
             + self.cost_for_breaking_block(pos.up(1), mining_cache)
     }
 
     /// 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: RelBlockPos) -> bool {
+        self.is_standable_at_block_pos(pos.apply(self.origin))
+    }
+    fn is_standable_at_block_pos(&self, pos: BlockPos) -> bool {
+        self.is_block_pos_solid(pos.down(1)) && self.is_passable_at_block_pos(pos)
     }
 
-    pub fn cost_for_standing(&self, pos: BlockPos, mining_cache: &MiningCache) -> f32 {
+    pub fn cost_for_standing(&self, pos: RelBlockPos, mining_cache: &MiningCache) -> f32 {
         if !self.is_block_solid(pos.down(1)) {
             return f32::INFINITY;
         }
@@ -423,7 +461,7 @@ impl CachedWorld {
     }
 
     /// 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: RelBlockPos) -> u32 {
         let mut distance = 0;
         let mut current_pos = pos.down(1);
         while self.is_block_passable(current_pos) {
@@ -512,9 +550,9 @@ mod tests {
             .chunks
             .set_block_state(&BlockPos::new(0, 1, 0), BlockState::AIR, &world);
 
-        let ctx = CachedWorld::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),));
+        let ctx = CachedWorld::new(Arc::new(RwLock::new(world.into())), BlockPos::default());
+        assert!(!ctx.is_block_pos_passable(BlockPos::new(0, 0, 0)));
+        assert!(ctx.is_block_pos_passable(BlockPos::new(0, 1, 0),));
     }
 
     #[test]
@@ -533,9 +571,9 @@ mod tests {
             .chunks
             .set_block_state(&BlockPos::new(0, 1, 0), BlockState::AIR, &world);
 
-        let ctx = CachedWorld::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)));
+        let ctx = CachedWorld::new(Arc::new(RwLock::new(world.into())), BlockPos::default());
+        assert!(ctx.is_block_pos_solid(BlockPos::new(0, 0, 0)));
+        assert!(!ctx.is_block_pos_solid(BlockPos::new(0, 1, 0)));
     }
 
     #[test]
@@ -560,9 +598,9 @@ mod tests {
             .chunks
             .set_block_state(&BlockPos::new(0, 3, 0), BlockState::AIR, &world);
 
-        let ctx = CachedWorld::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)));
+        let ctx = CachedWorld::new(Arc::new(RwLock::new(world.into())), BlockPos::default());
+        assert!(ctx.is_standable_at_block_pos(BlockPos::new(0, 1, 0)));
+        assert!(!ctx.is_standable_at_block_pos(BlockPos::new(0, 0, 0)));
+        assert!(!ctx.is_standable_at_block_pos(BlockPos::new(0, 2, 0)));
     }
 }