add pathfinder benchmark

9 files changed, 140 insertions, 33 deletions

diff --git a/Cargo.lock b/Cargo.lock
index b663ca49..0f777ddf 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -183,6 +183,7 @@ dependencies = [
  "bevy_log",
  "bevy_tasks",
  "bevy_time",
+ "criterion",
  "derive_more",
  "futures",
  "futures-lite",
@@ -191,6 +192,7 @@ dependencies = [
  "num-traits",
  "parking_lot",
  "priority-queue",
+ "rand",
  "thiserror",
  "tokio",
  "uuid",
diff --git a/azalea-world/src/chunk_storage.rs b/azalea-world/src/chunk_storage.rs
index f7800d2b..2e93f2fb 100755
--- a/azalea-world/src/chunk_storage.rs
+++ b/azalea-world/src/chunk_storage.rs
@@ -279,6 +279,7 @@ impl Chunk {
         get_block_state_from_sections(&self.sections, pos, min_y)
     }
 
+    #[must_use = "Use Chunk::set instead if you don't need the previous state"]
     pub fn get_and_set(
         &mut self,
         pos: &ChunkBlockPos,
diff --git a/azalea-world/src/palette.rs b/azalea-world/src/palette.rs
index 38cf5853..f1d6b1a3 100755
--- a/azalea-world/src/palette.rs
+++ b/azalea-world/src/palette.rs
@@ -172,7 +172,7 @@ impl PalettedContainer {
                 }
             }
             Palette::Linear(palette) => {
-                if let Some(index) = palette.iter().position(|v| *v == value) {
+                if let Some(index) = palette.iter().position(|&v| v == value) {
                     return index;
                 }
                 let capacity = 2usize.pow(self.bits_per_entry.into());
diff --git a/azalea/Cargo.toml b/azalea/Cargo.toml
index 582168a4..61a0cc93 100644
--- a/azalea/Cargo.toml
+++ b/azalea/Cargo.toml
@@ -43,7 +43,15 @@ bevy_log = "0.11.2"
 azalea-entity = { version = "0.8.0", path = "../azalea-entity" }
 bevy_time = "0.11.2"
 
+[dev-dependencies]
+criterion = "0.5.1"
+rand = "0.8.5"
+
 [features]
 default = ["log"]
 # enables bevy_log::LogPlugin by default
 log = ["azalea-client/log"]
+
+[[bench]]
+name = "pathfinder"
+harness = false
diff --git a/azalea/benches/pathfinder.rs b/azalea/benches/pathfinder.rs
new file mode 100644
index 00000000..34b6f91c
--- /dev/null
+++ b/azalea/benches/pathfinder.rs
@@ -0,0 +1,98 @@
+use std::{hint::black_box, time::Duration};
+
+use azalea::{
+    pathfinder::{
+        astar::{self, a_star},
+        goals::BlockPosGoal,
+        Goal,
+    },
+    BlockPos,
+};
+use azalea_core::position::{ChunkBlockPos, ChunkPos};
+use azalea_world::{Chunk, ChunkStorage, PartialChunkStorage};
+use criterion::{criterion_group, criterion_main, Criterion};
+use rand::Rng;
+
+fn generate_bedrock_world(
+    partial_chunks: &mut PartialChunkStorage,
+    size: u32,
+) -> (ChunkStorage, BlockPos, BlockPos) {
+    let size = size as i32;
+
+    let mut chunks = ChunkStorage::default();
+    for chunk_x in -size..size {
+        for chunk_z in -size..size {
+            let chunk_pos = ChunkPos::new(chunk_x, chunk_z);
+            partial_chunks.set(&chunk_pos, Some(Chunk::default()), &mut chunks);
+        }
+    }
+
+    let mut rng = rand::thread_rng();
+
+    for chunk_x in -size..size {
+        for chunk_z in -size..size {
+            let chunk_pos = ChunkPos::new(chunk_x, chunk_z);
+            let chunk = chunks.get(&chunk_pos).unwrap();
+            let mut chunk = chunk.write();
+            for x in 0..16_u8 {
+                for z in 0..16_u8 {
+                    chunk.set(
+                        &ChunkBlockPos::new(x, 1, z),
+                        azalea_registry::Block::Bedrock.into(),
+                        chunks.min_y,
+                    );
+                    if rng.gen_bool(0.5) {
+                        chunk.set(
+                            &ChunkBlockPos::new(x, 2, z),
+                            azalea_registry::Block::Bedrock.into(),
+                            chunks.min_y,
+                        );
+                    }
+                }
+            }
+        }
+    }
+
+    let mut start = BlockPos::new(-64, 4, -64);
+    // move start down until it's on bedrock
+    while chunks.get_block_state(&start).unwrap().is_air() {
+        start = start.down(1);
+    }
+    start = start.up(1);
+
+    let mut end = BlockPos::new(63, 4, 63);
+    // move end down until it's on bedrock
+    while chunks.get_block_state(&end).unwrap().is_air() {
+        end = end.down(1);
+    }
+    end = end.up(1);
+
+    (chunks, start, end)
+}
+
+fn bench_pathfinder(c: &mut Criterion) {
+    c.bench_function("bedrock", |b| {
+        let mut partial_chunks = PartialChunkStorage::new(32);
+        let successors_fn = azalea::pathfinder::moves::parkour::parkour_move;
+
+        b.iter(|| {
+            let (world, start, end) = generate_bedrock_world(&mut partial_chunks, 4);
+            let goal = BlockPosGoal(end);
+
+            let successors = |pos: BlockPos| successors_fn(&world, pos);
+
+            let astar::Path { movements, partial } = a_star(
+                start,
+                |n| goal.heuristic(n),
+                successors,
+                |n| goal.success(n),
+                Duration::MAX,
+            );
+
+            black_box((movements, partial));
+        })
+    });
+}
+
+criterion_group!(benches, bench_pathfinder);
+criterion_main!(benches);
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 0be063a4..02b7d935 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -1,10 +1,10 @@
 //! A pathfinding plugin to make bots navigate the world. A lot of this code is
 //! based on [Baritone](https://github.com/cabaletta/baritone).
 
-mod astar;
+pub mod astar;
 pub mod costs;
 pub mod goals;
-mod moves;
+pub mod moves;
 pub mod simulation;
 
 use crate::bot::{JumpEvent, LookAtEvent};
@@ -177,10 +177,8 @@ fn goto_listener(
         let task = thread_pool.spawn(async move {
             debug!("start: {start:?}");
 
-            let successors = |pos: BlockPos| {
-                let world = world_lock.read();
-                successors_fn(&world, pos)
-            };
+            let world = &world_lock.read().chunks;
+            let successors = |pos: BlockPos| successors_fn(world, pos);
 
             let mut attempt_number = 0;
 
@@ -279,8 +277,8 @@ fn path_found_listener(
                         );
                         let successors_fn: moves::SuccessorsFn = event.successors_fn;
                         let successors = |pos: BlockPos| {
-                            let world = world_lock.read();
-                            successors_fn(&world, pos)
+                            let world = &world_lock.read().chunks;
+                            successors_fn(world, pos)
                         };
 
                         if successors(last_node.target)
@@ -442,8 +440,8 @@ fn tick_execute_path(
         {
             // obstruction check (the path we're executing isn't possible anymore)
             let successors = |pos: BlockPos| {
-                let world = world_lock.read();
-                successors_fn(&world, pos)
+                let world = &world_lock.read().chunks;
+                successors_fn(world, pos)
             };
 
             if let Some(last_reached_node) = pathfinder.last_reached_node {
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 188eb3a6..1785ec2e 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -5,7 +5,7 @@ use azalea_core::{
     direction::CardinalDirection,
     position::{BlockPos, Vec3},
 };
-use azalea_world::Instance;
+use azalea_world::ChunkStorage;
 
 use crate::{
     pathfinder::{astar, costs::*},
@@ -17,7 +17,7 @@ use super::{
     ExecuteCtx, IsReachedCtx, MoveData,
 };
 
-pub fn basic_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
+pub fn basic_move(world: &ChunkStorage, node: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     edges.extend(forward_move(world, node));
     edges.extend(ascend_move(world, node));
@@ -26,7 +26,7 @@ pub fn basic_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
     edges
 }
 
-fn forward_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn forward_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 0, dir.z());
@@ -72,7 +72,7 @@ fn execute_forward_move(
     });
 }
 
-fn ascend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn ascend_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 1, dir.z());
@@ -156,7 +156,7 @@ pub fn ascend_is_reached(
     BlockPos::from(position) == target || BlockPos::from(position) == target.down(1)
 }
 
-fn descend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn descend_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let dir_delta = BlockPos::new(dir.x(), 0, dir.z());
@@ -258,7 +258,7 @@ pub fn descend_is_reached(
         && (position.y - target.y as f64) < 0.5
 }
 
-fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn diagonal_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let right = dir.right();
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index d631ac8a..9bd074c0 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -9,13 +9,13 @@ use super::astar;
 use azalea_client::{StartSprintEvent, StartWalkEvent};
 use azalea_core::position::{BlockPos, Vec3};
 use azalea_physics::collision::{self, BlockWithShape};
-use azalea_world::Instance;
+use azalea_world::ChunkStorage;
 use bevy_ecs::{entity::Entity, event::EventWriter};
 
 type Edge = astar::Edge<BlockPos, MoveData>;
 
 pub type SuccessorsFn =
-    fn(&azalea_world::Instance, BlockPos) -> Vec<astar::Edge<BlockPos, MoveData>>;
+    fn(&azalea_world::ChunkStorage, BlockPos) -> Vec<astar::Edge<BlockPos, MoveData>>;
 
 #[derive(Clone)]
 pub struct MoveData {
@@ -34,8 +34,8 @@ impl Debug for MoveData {
 }
 
 /// whether this block is passable
-fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool {
-    if let Some(block) = world.chunks.get_block_state(pos) {
+fn is_block_passable(pos: &BlockPos, world: &ChunkStorage) -> bool {
+    if let Some(block) = world.get_block_state(pos) {
         if block.shape() != &collision::empty_shape() {
             return false;
         }
@@ -58,8 +58,8 @@ fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool {
 }
 
 /// whether this block has a solid hitbox (i.e. we can stand on it)
-fn is_block_solid(pos: &BlockPos, world: &Instance) -> bool {
-    if let Some(block) = world.chunks.get_block_state(pos) {
+fn is_block_solid(pos: &BlockPos, world: &ChunkStorage) -> bool {
+    if let Some(block) = world.get_block_state(pos) {
         block.shape() == &collision::block_shape()
     } else {
         false
@@ -67,26 +67,26 @@ fn is_block_solid(pos: &BlockPos, world: &Instance) -> bool {
 }
 
 /// Whether this block and the block above are passable
-fn is_passable(pos: &BlockPos, world: &Instance) -> bool {
+fn is_passable(pos: &BlockPos, world: &ChunkStorage) -> bool {
     is_block_passable(pos, world) && is_block_passable(&pos.up(1), world)
 }
 
 /// Whether we can stand in this position. Checks if the block below is solid,
 /// and that the two blocks above that are passable.
 
-fn is_standable(pos: &BlockPos, world: &Instance) -> bool {
+fn is_standable(pos: &BlockPos, world: &ChunkStorage) -> bool {
     is_block_solid(&pos.down(1), world) && is_passable(pos, world)
 }
 
 /// Get the amount of air blocks until the next solid block below this one.
-fn fall_distance(pos: &BlockPos, world: &Instance) -> u32 {
+fn fall_distance(pos: &BlockPos, world: &ChunkStorage) -> u32 {
     let mut distance = 0;
     let mut current_pos = pos.down(1);
     while is_block_passable(&current_pos, world) {
         distance += 1;
         current_pos = current_pos.down(1);
 
-        if current_pos.y < world.chunks.min_y {
+        if current_pos.y < world.min_y {
             return u32::MAX;
         }
     }
@@ -115,7 +115,7 @@ pub struct IsReachedCtx<'a> {
     pub physics: &'a azalea_entity::Physics,
 }
 
-pub fn default_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
+pub fn default_move(world: &ChunkStorage, node: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     edges.extend(basic::basic_move(world, node));
     edges.extend(parkour::parkour_move(world, node));
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index ab9c509c..03635faa 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -1,6 +1,6 @@
 use azalea_client::{SprintDirection, StartSprintEvent, StartWalkEvent, WalkDirection};
 use azalea_core::{direction::CardinalDirection, position::BlockPos};
-use azalea_world::Instance;
+use azalea_world::ChunkStorage;
 
 use crate::{
     pathfinder::{astar, costs::*},
@@ -12,7 +12,7 @@ use super::{
     ExecuteCtx, IsReachedCtx, MoveData,
 };
 
-pub fn parkour_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
+pub fn parkour_move(world: &ChunkStorage, node: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     edges.extend(parkour_forward_1_move(world, node));
     edges.extend(parkour_headhitter_forward_1_move(world, node));
@@ -20,7 +20,7 @@ pub fn parkour_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
     edges
 }
 
-fn parkour_forward_1_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn parkour_forward_1_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let gap_offset = BlockPos::new(dir.x(), 0, dir.z());
@@ -61,7 +61,7 @@ fn parkour_forward_1_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
     edges
 }
 
-fn parkour_forward_2_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn parkour_forward_2_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let gap_1_offset = BlockPos::new(dir.x(), 0, dir.z());
@@ -112,7 +112,7 @@ fn parkour_forward_2_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
     edges
 }
 
-fn parkour_headhitter_forward_1_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
+fn parkour_headhitter_forward_1_move(world: &ChunkStorage, pos: BlockPos) -> Vec<Edge> {
     let mut edges = Vec::new();
     for dir in CardinalDirection::iter() {
         let gap_offset = BlockPos::new(dir.x(), 0, dir.z());