azalea/pathfinder/
goals.rs

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