azalea/pathfinder/
goals.rs1use 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#[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#[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#[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#[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#[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#[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#[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#[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#[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#[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 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}