azalea/pathfinder/
goals.rs

1//! The goals that a pathfinder can try to reach.
2
3use std::{
4    f32::consts::SQRT_2,
5    fmt::{self, Debug},
6};
7
8use azalea_core::position::{BlockPos, Vec3};
9use azalea_world::ChunkStorage;
10#[cfg(feature = "serde")]
11use serde::{Deserialize, Serialize};
12
13use super::costs::{COST_HEURISTIC, FALL_N_BLOCKS_COST, JUMP_ONE_BLOCK_COST};
14
15pub trait Goal: Debug + Send + Sync {
16    #[must_use]
17    fn heuristic(&self, n: BlockPos) -> f32;
18    #[must_use]
19    fn success(&self, n: BlockPos) -> bool;
20}
21
22/// Move to the given block position.
23#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
24#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
25pub struct BlockPosGoal(pub BlockPos);
26impl Goal for BlockPosGoal {
27    fn heuristic(&self, n: BlockPos) -> f32 {
28        let dx = (self.0.x - n.x) as f32;
29        let dy = (self.0.y - n.y) as f32;
30        let dz = (self.0.z - n.z) as f32;
31
32        xz_heuristic(dx, dz) + y_heuristic(dy)
33    }
34    fn success(&self, n: BlockPos) -> bool {
35        // the second half of this condition is intended to fix issues when pathing to
36        // non-full blocks
37        n == self.0 || n.down(1) == self.0
38    }
39}
40
41fn xz_heuristic(dx: f32, dz: f32) -> f32 {
42    let x = dx.abs();
43    let z = dz.abs();
44
45    let diagonal;
46    let straight;
47
48    if x < z {
49        straight = z - x;
50        diagonal = x;
51    } else {
52        straight = x - z;
53        diagonal = z;
54    }
55
56    (diagonal * SQRT_2 + straight) * COST_HEURISTIC
57}
58
59/// Move to the given block position, ignoring the y-axis.
60#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
61#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
62pub struct XZGoal {
63    pub x: i32,
64    pub z: i32,
65}
66impl Goal for XZGoal {
67    fn heuristic(&self, n: BlockPos) -> f32 {
68        let dx = (self.x - n.x) as f32;
69        let dz = (self.z - n.z) as f32;
70        xz_heuristic(dx, dz)
71    }
72    fn success(&self, n: BlockPos) -> bool {
73        n.x == self.x && n.z == self.z
74    }
75}
76
77fn y_heuristic(dy: f32) -> f32 {
78    if dy > 0.0 {
79        *JUMP_ONE_BLOCK_COST * dy
80    } else {
81        FALL_N_BLOCKS_COST[2] / 2. * -dy
82    }
83}
84
85/// Move to the given y coordinate.
86#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
87#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
88pub struct YGoal {
89    pub y: i32,
90}
91impl Goal for YGoal {
92    fn heuristic(&self, n: BlockPos) -> f32 {
93        let dy = (self.y - n.y) as f32;
94        y_heuristic(dy)
95    }
96    fn success(&self, n: BlockPos) -> bool {
97        n.y == self.y
98    }
99}
100
101/// Get within the given radius of the given position.
102#[derive(Clone, Copy, Debug, Default, PartialEq)]
103#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
104pub struct RadiusGoal {
105    pub pos: Vec3,
106    pub radius: f32,
107}
108impl RadiusGoal {
109    pub fn new(pos: Vec3, radius: f32) -> Self {
110        Self { pos, radius }
111    }
112}
113impl Goal for RadiusGoal {
114    fn heuristic(&self, n: BlockPos) -> f32 {
115        let n = n.center();
116        let dx = (self.pos.x - n.x) as f32;
117        let dy = (self.pos.y - n.y) as f32;
118        let dz = (self.pos.z - n.z) as f32;
119        dx.powi(2) + dy.powi(2) + dz.powi(2)
120    }
121    fn success(&self, n: BlockPos) -> bool {
122        let n = n.center();
123        let dx = (self.pos.x - n.x) as f32;
124        let dy = (self.pos.y - n.y) as f32;
125        let dz = (self.pos.z - n.z) as f32;
126        dx.powi(2) + dy.powi(2) + dz.powi(2) <= self.radius.powi(2)
127    }
128}
129
130/// Do the opposite of the given goal.
131#[derive(Debug)]
132pub struct InverseGoal<T: Goal>(pub T);
133impl<T: Goal> Goal for InverseGoal<T> {
134    fn heuristic(&self, n: BlockPos) -> f32 {
135        -self.0.heuristic(n)
136    }
137    fn success(&self, n: BlockPos) -> bool {
138        !self.0.success(n)
139    }
140}
141
142/// Do either of the given goals, whichever is closer.
143#[derive(Debug)]
144pub struct OrGoal<T: Goal, U: Goal>(pub T, pub U);
145impl<T: Goal, U: Goal> Goal for OrGoal<T, U> {
146    fn heuristic(&self, n: BlockPos) -> f32 {
147        self.0.heuristic(n).min(self.1.heuristic(n))
148    }
149    fn success(&self, n: BlockPos) -> bool {
150        self.0.success(n) || self.1.success(n)
151    }
152}
153
154/// Do any of the given goals, whichever is closest.
155#[derive(Debug)]
156pub struct OrGoals<T: Goal>(pub Vec<T>);
157impl<T: Goal> Goal for OrGoals<T> {
158    fn heuristic(&self, n: BlockPos) -> f32 {
159        self.0
160            .iter()
161            .map(|goal| goal.heuristic(n))
162            .min_by(|a, b| a.partial_cmp(b).unwrap())
163            .unwrap_or(f32::INFINITY)
164    }
165    fn success(&self, n: BlockPos) -> bool {
166        self.0.iter().any(|goal| goal.success(n))
167    }
168}
169
170/// Try to reach both of the given goals.
171#[derive(Debug)]
172pub struct AndGoal<T: Goal, U: Goal>(pub T, pub U);
173impl<T: Goal, U: Goal> Goal for AndGoal<T, U> {
174    fn heuristic(&self, n: BlockPos) -> f32 {
175        self.0.heuristic(n).max(self.1.heuristic(n))
176    }
177    fn success(&self, n: BlockPos) -> bool {
178        self.0.success(n) && self.1.success(n)
179    }
180}
181
182/// Try to reach all the given goals.
183#[derive(Debug)]
184pub struct AndGoals<T: Goal>(pub Vec<T>);
185impl<T: Goal> Goal for AndGoals<T> {
186    fn heuristic(&self, n: BlockPos) -> f32 {
187        self.0
188            .iter()
189            .map(|goal| goal.heuristic(n))
190            .max_by(|a, b| a.partial_cmp(b).unwrap())
191            .unwrap_or(f32::INFINITY)
192    }
193    fn success(&self, n: BlockPos) -> bool {
194        self.0.iter().all(|goal| goal.success(n))
195    }
196}
197
198/// Move to a position where we can reach the given block.
199#[derive(Clone)]
200pub struct ReachBlockPosGoal {
201    pub pos: BlockPos,
202    pub distance: f64,
203    pub chunk_storage: ChunkStorage,
204
205    max_check_distance: i32,
206}
207impl ReachBlockPosGoal {
208    pub fn new(pos: BlockPos, chunk_storage: ChunkStorage) -> Self {
209        Self::new_with_distance(pos, 4.5, chunk_storage)
210    }
211
212    pub fn new_with_distance(pos: BlockPos, distance: f64, chunk_storage: ChunkStorage) -> Self {
213        Self {
214            pos,
215            distance,
216            chunk_storage,
217            max_check_distance: (distance + 2.).ceil() as i32,
218        }
219    }
220}
221impl Goal for ReachBlockPosGoal {
222    fn heuristic(&self, n: BlockPos) -> f32 {
223        BlockPosGoal(self.pos).heuristic(n)
224    }
225    fn success(&self, n: BlockPos) -> bool {
226        if n.up(1) == self.pos {
227            // our head is in the block, assume it's always reachable (to reduce the amount
228            // of impossible goals)
229            return true;
230        }
231
232        // only do the expensive check if we're close enough
233        let distance_squared = self.pos.distance_squared_to(n);
234        if distance_squared > self.max_check_distance.pow(2) {
235            return false;
236        }
237
238        let eye_position = n.center_bottom().up(1.62);
239        let look_direction = crate::direction_looking_at(eye_position, self.pos.center());
240        let block_hit_result = azalea_client::interact::pick::pick_block(
241            look_direction,
242            eye_position,
243            &self.chunk_storage,
244            self.distance,
245        );
246
247        block_hit_result.block_pos == self.pos
248    }
249}
250impl Debug for ReachBlockPosGoal {
251    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
252        f.debug_struct("ReachBlockPosGoal")
253            .field("pos", &self.pos)
254            .field("distance", &self.distance)
255            .field("max_check_distance", &self.max_check_distance)
256            .finish()
257    }
258}
azalea/pathfinder/goals.rs

azalea/pathfinder/
goals.rs
