Add mining to the pathfinder (#122)

* basic pathfinder mining poc

* mining descending and autotool

* pathfinder mining descending

* pathfinder fixes

* allow disabling pathfinder miner and other fixes

* small optimization to avoid chunk vec iter lookup sometimes

* seeded rng in pathfinder bench

* consistently use f32::INFINITY

this brings performance much closer to how it was before

* astar heuristic optimization from baritone

* add downward_move

* fix downward move execute

* avoid liquids and falling blocks when mining

* fix COST_HEURISTIC

* fix to not path through flowing liquids

* only reset pathfinder timeout while mining if the block is close enough

* cache mining costs of block positions

* fix mine_while_at_start and move PathfinderDebugParticles to its own module

* add ReachBlockPosGoal

in other news: azalea's sin/cos functions were broken this whole time and i never noticed

* clippy and add things that i accidentally didn't commit

* improve wording on doc for azalea::pathfinder

2 files changed, 224 insertions, 25 deletions

diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 957e24c6..54a6dc6a 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -16,17 +16,20 @@ pub fn basic_move(ctx: &mut PathfinderCtx, node: BlockPos) {
     descend_move(ctx, node);
     diagonal_move(ctx, node);
     descend_forward_1_move(ctx, node);
+    downward_move(ctx, node);
 }
 
 fn forward_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 0, dir.z());
 
-        if !ctx.world.is_standable(pos + offset) {
+        let mut cost = SPRINT_ONE_BLOCK_COST;
+
+        let break_cost = ctx.world.cost_for_standing(pos + offset, ctx.mining_cache);
+        if break_cost == f32::INFINITY {
             continue;
         }
-
-        let cost = SPRINT_ONE_BLOCK_COST;
+        cost += break_cost;
 
         ctx.edges.push(Edge {
             movement: astar::Movement {
@@ -43,6 +46,14 @@ fn forward_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
 
 fn execute_forward_move(mut ctx: ExecuteCtx) {
     let center = ctx.target.center();
+
+    if ctx.mine_while_at_start(ctx.target.up(1)) {
+        return;
+    }
+    if ctx.mine_while_at_start(ctx.target) {
+        return;
+    }
+
     ctx.look_at(center);
     ctx.sprint(SprintDirection::Forward);
 }
@@ -51,14 +62,22 @@ fn ascend_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 1, dir.z());
 
-        if !ctx.world.is_block_passable(pos.up(2)) {
+        let break_cost_1 = ctx
+            .world
+            .cost_for_breaking_block(pos.up(2), ctx.mining_cache);
+        if break_cost_1 == f32::INFINITY {
             continue;
         }
-        if !ctx.world.is_standable(pos + offset) {
+        let break_cost_2 = ctx.world.cost_for_standing(pos + offset, ctx.mining_cache);
+        if break_cost_2 == f32::INFINITY {
             continue;
         }
 
-        let cost = SPRINT_ONE_BLOCK_COST + JUMP_PENALTY + *JUMP_ONE_BLOCK_COST;
+        let cost = SPRINT_ONE_BLOCK_COST
+            + JUMP_PENALTY
+            + *JUMP_ONE_BLOCK_COST
+            + break_cost_1
+            + break_cost_2;
 
         ctx.edges.push(Edge {
             movement: astar::Movement {
@@ -81,6 +100,16 @@ fn execute_ascend_move(mut ctx: ExecuteCtx) {
         ..
     } = ctx;
 
+    if ctx.mine_while_at_start(start.up(2)) {
+        return;
+    }
+    if ctx.mine_while_at_start(target) {
+        return;
+    }
+    if ctx.mine_while_at_start(target.up(1)) {
+        return;
+    }
+
     let target_center = target.center();
 
     ctx.look_at(target_center);
@@ -123,19 +152,39 @@ fn descend_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
     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.world.fall_distance(new_horizontal_position);
-        if fall_distance == 0 || fall_distance > 3 {
+
+        let break_cost_1 = ctx
+            .world
+            .cost_for_passing(new_horizontal_position, ctx.mining_cache);
+        if break_cost_1 == f32::INFINITY {
             continue;
         }
-        let new_position = new_horizontal_position.down(fall_distance as i32);
 
-        // check whether 3 blocks vertically forward are passable
-        if !ctx.world.is_passable(new_horizontal_position) {
+        let mut fall_distance = ctx.world.fall_distance(new_horizontal_position);
+        if fall_distance > 3 {
             continue;
         }
-        // check whether we can stand on the target position
-        if !ctx.world.is_standable(new_position) {
-            continue;
+
+        if fall_distance == 0 {
+            // if the fall distance is 0, set it to 1 so we try mining
+            fall_distance = 1
+        }
+
+        let new_position = new_horizontal_position.down(fall_distance as i32);
+
+        // only mine if we're descending 1 block
+        let break_cost_2;
+        if fall_distance == 1 {
+            break_cost_2 = ctx.world.cost_for_standing(new_position, ctx.mining_cache);
+            if break_cost_2 == f32::INFINITY {
+                continue;
+            }
+        } else {
+            // check whether we can stand on the target position
+            if !ctx.world.is_standable(new_position) {
+                continue;
+            }
+            break_cost_2 = 0.;
         }
 
         let cost = WALK_OFF_BLOCK_COST
@@ -145,9 +194,11 @@ fn descend_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
                     .copied()
                     // avoid panicking if we fall more than the size of FALL_N_BLOCKS_COST
                     // probably not possible but just in case
-                    .unwrap_or(f32::MAX),
+                    .unwrap_or(f32::INFINITY),
                 CENTER_AFTER_FALL_COST,
-            );
+            )
+            + break_cost_1
+            + break_cost_2;
 
         ctx.edges.push(Edge {
             movement: astar::Movement {
@@ -169,6 +220,12 @@ fn execute_descend_move(mut ctx: ExecuteCtx) {
         ..
     } = ctx;
 
+    for i in (0..=(start.y - target.y + 1)).rev() {
+        if ctx.mine_while_at_start(target.up(i)) {
+            return;
+        }
+    }
+
     let start_center = start.center();
     let center = target.center();
 
@@ -249,7 +306,7 @@ fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
                     .copied()
                     // avoid panicking if we fall more than the size of FALL_N_BLOCKS_COST
                     // probably not possible but just in case
-                    .unwrap_or(f32::MAX),
+                    .unwrap_or(f32::INFINITY),
                 CENTER_AFTER_FALL_COST,
             );
 
@@ -310,3 +367,53 @@ fn execute_diagonal_move(mut ctx: ExecuteCtx) {
     ctx.look_at(target_center);
     ctx.sprint(SprintDirection::Forward);
 }
+
+/// Go directly down, usually by mining.
+fn downward_move(ctx: &mut PathfinderCtx, pos: BlockPos) {
+    // make sure we land on a solid block after breaking the one below us
+    if !ctx.world.is_block_solid(pos.down(2)) {
+        return;
+    }
+
+    let break_cost = ctx
+        .world
+        .cost_for_breaking_block(pos.down(1), ctx.mining_cache);
+    if break_cost == f32::INFINITY {
+        return;
+    }
+
+    let cost = FALL_N_BLOCKS_COST[1] + break_cost;
+
+    ctx.edges.push(Edge {
+        movement: astar::Movement {
+            target: pos.down(1),
+            data: MoveData {
+                execute: &execute_downward_move,
+                is_reached: &default_is_reached,
+            },
+        },
+        cost,
+    })
+}
+fn execute_downward_move(mut ctx: ExecuteCtx) {
+    let ExecuteCtx {
+        target, position, ..
+    } = ctx;
+
+    let target_center = target.center();
+
+    let horizontal_distance_from_target =
+        (target_center - position).horizontal_distance_sqr().sqrt();
+
+    if horizontal_distance_from_target > 0.25 {
+        ctx.look_at(target_center);
+        ctx.walk(WalkDirection::Forward);
+    } else if ctx.mine_while_at_start(target) {
+        ctx.walk(WalkDirection::None);
+    } else if BlockPos::from(position) != target {
+        ctx.look_at(target_center);
+        ctx.walk(WalkDirection::Forward);
+    } else {
+        ctx.walk(WalkDirection::None);
+    }
+}
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index e5b837ea..bb10b192 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -1,14 +1,24 @@
 pub mod basic;
 pub mod parkour;
 
-use std::fmt::Debug;
-
-use crate::{JumpEvent, LookAtEvent};
-
-use super::{astar, mining::MiningCache, world::CachedWorld};
-use azalea_client::{SprintDirection, StartSprintEvent, StartWalkEvent, WalkDirection};
+use std::{fmt::Debug, sync::Arc};
+
+use crate::{auto_tool::best_tool_in_hotbar_for_block, JumpEvent, LookAtEvent};
+
+use super::{
+    astar,
+    mining::MiningCache,
+    world::{is_block_state_passable, CachedWorld},
+};
+use azalea_client::{
+    inventory::SetSelectedHotbarSlotEvent, mining::StartMiningBlockEvent, SprintDirection,
+    StartSprintEvent, StartWalkEvent, WalkDirection,
+};
 use azalea_core::position::{BlockPos, Vec3};
+use azalea_inventory::Menu;
+use azalea_world::Instance;
 use bevy_ecs::{entity::Entity, event::EventWriter};
+use parking_lot::RwLock;
 
 type Edge = astar::Edge<BlockPos, MoveData>;
 
@@ -35,7 +45,7 @@ impl Debug for MoveData {
     }
 }
 
-pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'a> {
+pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'w5, 'w6, 'a> {
     pub entity: Entity,
     /// The node that we're trying to reach.
     pub target: BlockPos,
@@ -43,14 +53,19 @@ pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'a> {
     pub start: BlockPos,
     pub position: Vec3,
     pub physics: &'a azalea_entity::Physics,
+    pub is_currently_mining: bool,
+    pub instance: Arc<RwLock<Instance>>,
+    pub menu: Menu,
 
     pub look_at_events: &'a mut EventWriter<'w1, LookAtEvent>,
     pub sprint_events: &'a mut EventWriter<'w2, StartSprintEvent>,
     pub walk_events: &'a mut EventWriter<'w3, StartWalkEvent>,
     pub jump_events: &'a mut EventWriter<'w4, JumpEvent>,
+    pub start_mining_events: &'a mut EventWriter<'w5, StartMiningBlockEvent>,
+    pub set_selected_hotbar_slot_events: &'a mut EventWriter<'w6, SetSelectedHotbarSlotEvent>,
 }
 
-impl ExecuteCtx<'_, '_, '_, '_, '_> {
+impl ExecuteCtx<'_, '_, '_, '_, '_, '_, '_> {
     pub fn look_at(&mut self, position: Vec3) {
         self.look_at_events.send(LookAtEvent {
             entity: self.entity,
@@ -63,6 +78,13 @@ impl ExecuteCtx<'_, '_, '_, '_, '_> {
         });
     }
 
+    pub fn look_at_exact(&mut self, position: Vec3) {
+        self.look_at_events.send(LookAtEvent {
+            entity: self.entity,
+            position,
+        });
+    }
+
     pub fn sprint(&mut self, direction: SprintDirection) {
         self.sprint_events.send(StartSprintEvent {
             entity: self.entity,
@@ -82,6 +104,76 @@ impl ExecuteCtx<'_, '_, '_, '_, '_> {
             entity: self.entity,
         });
     }
+
+    /// Returns whether this block could be mined.
+    pub fn should_mine(&mut self, block: BlockPos) -> bool {
+        let block_state = self
+            .instance
+            .read()
+            .get_block_state(&block)
+            .unwrap_or_default();
+        if is_block_state_passable(block_state) {
+            // block is already passable, no need to mine it
+            return false;
+        }
+
+        true
+    }
+
+    /// Mine the block at the given position. Returns whether the block is being
+    /// mined.
+    pub fn mine(&mut self, block: BlockPos) -> bool {
+        let block_state = self
+            .instance
+            .read()
+            .get_block_state(&block)
+            .unwrap_or_default();
+        if is_block_state_passable(block_state) {
+            // block is already passable, no need to mine it
+            return false;
+        }
+
+        let best_tool_result = best_tool_in_hotbar_for_block(block_state, &self.menu);
+
+        self.set_selected_hotbar_slot_events
+            .send(SetSelectedHotbarSlotEvent {
+                entity: self.entity,
+                slot: best_tool_result.index as u8,
+            });
+
+        self.is_currently_mining = true;
+
+        self.walk(WalkDirection::None);
+        self.look_at_exact(block.center());
+        self.start_mining_events.send(StartMiningBlockEvent {
+            entity: self.entity,
+            position: block,
+        });
+
+        true
+    }
+
+    /// Mine the given block, but make sure the player is standing at the start
+    /// of the current node first.
+    pub fn mine_while_at_start(&mut self, block: BlockPos) -> bool {
+        let horizontal_distance_from_start = (self.start.center() - self.position)
+            .horizontal_distance_sqr()
+            .sqrt();
+        let at_start_position =
+            BlockPos::from(self.position) == self.start && horizontal_distance_from_start < 0.25;
+
+        if self.should_mine(block) {
+            if at_start_position {
+                self.mine(block);
+            } else {
+                self.look_at(self.start.center());
+                self.walk(WalkDirection::Forward);
+            }
+            true
+        } else {
+            false
+        }
+    }
 }
 
 pub struct IsReachedCtx<'a> {