1use std::cmp::Ordering;
4
5use steel_utils::BlockPos;
6
7use crate::entity::ai::node::{Node, NodeHeap, Target};
8use crate::entity::ai::path::{Path, PathfindingContext};
9use crate::entity::ai::walk::{WalkNodeCollision, WalkNodeEvaluator};
10
11const FUDGING: f32 = 1.5;
12
13#[derive(Debug, Clone, Copy, PartialEq)]
14pub struct PathRequest<'a> {
15 pub targets: &'a [BlockPos],
16 pub max_path_length: f32,
17 pub reach_range: i32,
18 pub max_visited_nodes_multiplier: f32,
19}
20
21#[derive(Debug, Clone, PartialEq, Eq)]
22pub struct PathFinder {
23 max_visited_nodes: i32,
24 open_set: NodeHeap,
25}
26
27impl PathFinder {
28 #[must_use]
29 pub const fn new(max_visited_nodes: i32) -> Self {
30 Self {
31 max_visited_nodes,
32 open_set: NodeHeap::new(),
33 }
34 }
35
36 #[must_use]
37 pub const fn max_visited_nodes(&self) -> i32 {
38 self.max_visited_nodes
39 }
40
41 pub const fn set_max_visited_nodes(&mut self, max_visited_nodes: i32) {
42 self.max_visited_nodes = max_visited_nodes;
43 }
44
45 #[must_use]
46 pub fn find_path(
47 &mut self,
48 evaluator: &mut WalkNodeEvaluator,
49 context: &mut PathfindingContext<'_>,
50 collision: &mut impl WalkNodeCollision,
51 request: PathRequest<'_>,
52 ) -> Option<Path> {
53 let from = evaluator.get_start(context);
54 let (from_point, mut target_entries) =
55 self.prepare_search(evaluator, from, request.targets)?;
56 let max_visited_nodes_adjusted =
57 (self.max_visited_nodes as f32 * request.max_visited_nodes_multiplier) as i32;
58 let mut reached_targets = Vec::new();
59 let mut count = 0;
60 while !self.open_set.is_empty() {
61 count += 1;
62 if count >= max_visited_nodes_adjusted {
63 break;
64 }
65
66 let Some(current_hash) = self.open_set.pop(evaluator.nodes_mut()) else {
67 break;
68 };
69 {
70 let Some(current) = evaluator.node_mut(current_hash) else {
71 continue;
72 };
73 current.closed = true;
74 }
75 let Some(current_data) = evaluator.node(current_hash).map(NodeSearchData::from_node)
76 else {
77 continue;
78 };
79
80 for (index, target) in target_entries.iter_mut().enumerate() {
81 if current_data.point.manhattan_to_node(target.target.node())
82 <= request.reach_range as f32
83 {
84 target.target.set_reached();
85 if !reached_targets.contains(&index) {
86 reached_targets.push(index);
87 }
88 }
89 }
90 if !reached_targets.is_empty() {
91 break;
92 }
93
94 if current_data.point.distance_to(from_point) >= request.max_path_length {
95 continue;
96 }
97
98 let neighbors = evaluator.get_neighbors(context, collision, current_hash);
99 for neighbor_hash in neighbors.iter() {
100 let Some(neighbor_data) =
101 evaluator.node(neighbor_hash).map(NodeSearchData::from_node)
102 else {
103 continue;
104 };
105 let distance = current_data.point.distance_to(neighbor_data.point);
106 let walked_distance = current_data.walked_distance + distance;
107 let tentative_g_score = current_data.g + distance + neighbor_data.cost_malus;
108 if walked_distance >= request.max_path_length {
109 continue;
110 }
111 if neighbor_data.in_open_set && tentative_g_score >= neighbor_data.g {
112 continue;
113 }
114
115 let h =
116 Self::get_best_h(evaluator.node(neighbor_hash)?, &mut target_entries) * FUDGING;
117 let f = tentative_g_score + h;
118 {
119 let Some(neighbor) = evaluator.node_mut(neighbor_hash) else {
120 continue;
121 };
122 neighbor.came_from = Some(current_hash);
123 neighbor.g = tentative_g_score;
124 neighbor.h = h;
125 neighbor.walked_distance = walked_distance;
126 }
127
128 if neighbor_data.in_open_set {
129 if !self
130 .open_set
131 .change_cost(evaluator.nodes_mut(), neighbor_hash, f)
132 {
133 return None;
134 }
135 } else {
136 let Some(neighbor) = evaluator.node_mut(neighbor_hash) else {
137 continue;
138 };
139 neighbor.f = f;
140 if !self.open_set.insert(evaluator.nodes_mut(), neighbor_hash) {
141 return None;
142 }
143 }
144 }
145 }
146
147 if reached_targets.is_empty() {
148 Self::best_unreached_path(evaluator, &target_entries)
149 } else {
150 Self::best_reached_path(evaluator, &target_entries, &reached_targets)
151 }
152 }
153
154 fn prepare_search(
155 &mut self,
156 evaluator: &mut WalkNodeEvaluator,
157 from: i32,
158 targets: &[BlockPos],
159 ) -> Option<(NodePoint, Vec<PathTarget>)> {
160 if targets.is_empty() {
161 return None;
162 }
163
164 evaluator.reset_search_state();
165 self.open_set.clear(evaluator.nodes_mut());
166 let mut target_entries = targets
167 .iter()
168 .copied()
169 .map(PathTarget::new)
170 .collect::<Vec<_>>();
171 let from_point = NodePoint::from_node(evaluator.node(from)?);
172 let from_h = Self::get_best_h(evaluator.node(from)?, &mut target_entries);
173 {
174 let from_node = evaluator.node_mut(from)?;
175 from_node.g = 0.0;
176 from_node.h = from_h;
177 from_node.f = from_h;
178 from_node.walked_distance = 0.0;
179 from_node.came_from = None;
180 from_node.closed = false;
181 }
182 if !self.open_set.insert(evaluator.nodes_mut(), from) {
183 return None;
184 }
185
186 Some((from_point, target_entries))
187 }
188
189 fn get_best_h(from: &Node, targets: &mut [PathTarget]) -> f32 {
190 let mut best_h = f32::MAX;
191 for target in targets {
192 let h = from.distance_to(target.target.node());
193 target.target.update_best(h, from);
194 best_h = best_h.min(h);
195 }
196 best_h
197 }
198
199 fn best_reached_path(
200 evaluator: &WalkNodeEvaluator,
201 targets: &[PathTarget],
202 reached_targets: &[usize],
203 ) -> Option<Path> {
204 let mut best = None;
205 for index in reached_targets {
206 let Some(target) = targets.get(*index) else {
207 continue;
208 };
209 let Some(path) =
210 Self::reconstruct_path(evaluator, target.target.best_node(), target.pos, true)
211 else {
212 continue;
213 };
214 if best
215 .as_ref()
216 .is_none_or(|best_path: &Path| path.node_count() < best_path.node_count())
217 {
218 best = Some(path);
219 }
220 }
221 best
222 }
223
224 fn best_unreached_path(evaluator: &WalkNodeEvaluator, targets: &[PathTarget]) -> Option<Path> {
225 let mut best = None;
226 for target in targets {
227 let Some(path) =
228 Self::reconstruct_path(evaluator, target.target.best_node(), target.pos, false)
229 else {
230 continue;
231 };
232 if best
233 .as_ref()
234 .is_none_or(|best_path: &Path| compare_unreached_paths(&path, best_path).is_lt())
235 {
236 best = Some(path);
237 }
238 }
239 best
240 }
241
242 fn reconstruct_path(
243 evaluator: &WalkNodeEvaluator,
244 closest: Option<i32>,
245 target: BlockPos,
246 reached: bool,
247 ) -> Option<Path> {
248 let mut hashes = Vec::new();
249 let mut current_hash = closest?;
250 loop {
251 hashes.push(current_hash);
252 let node = evaluator.node(current_hash)?;
253 let Some(came_from) = node.came_from else {
254 break;
255 };
256 current_hash = came_from;
257 }
258 hashes.reverse();
259
260 let mut nodes = Vec::with_capacity(hashes.len());
261 for hash in hashes {
262 nodes.push(path_node_from(evaluator.node(hash)?));
263 }
264 Some(Path::new(nodes, target, reached))
265 }
266}
267
268impl Default for PathFinder {
269 fn default() -> Self {
270 Self::new(200)
271 }
272}
273
274#[derive(Debug, Clone, PartialEq)]
275struct PathTarget {
276 target: Target,
277 pos: BlockPos,
278}
279
280impl PathTarget {
281 const fn new(pos: BlockPos) -> Self {
282 Self {
283 target: Target::new(Node::new(pos.x(), pos.y(), pos.z())),
284 pos,
285 }
286 }
287}
288
289#[derive(Debug, Clone, Copy, PartialEq, Eq)]
290struct NodePoint {
291 x: i32,
292 y: i32,
293 z: i32,
294}
295
296impl NodePoint {
297 const fn from_node(node: &Node) -> Self {
298 Self {
299 x: node.x,
300 y: node.y,
301 z: node.z,
302 }
303 }
304
305 fn distance_to(self, other: Self) -> f32 {
306 let xd = (other.x - self.x) as f32;
307 let yd = (other.y - self.y) as f32;
308 let zd = (other.z - self.z) as f32;
309 xd.mul_add(xd, yd.mul_add(yd, zd * zd)).sqrt()
310 }
311
312 fn manhattan_to_node(self, other: &Node) -> f32 {
313 (other.x - self.x).abs() as f32
314 + (other.y - self.y).abs() as f32
315 + (other.z - self.z).abs() as f32
316 }
317}
318
319#[derive(Debug, Clone, Copy, PartialEq)]
320struct NodeSearchData {
321 point: NodePoint,
322 g: f32,
323 walked_distance: f32,
324 cost_malus: f32,
325 in_open_set: bool,
326}
327
328impl NodeSearchData {
329 const fn from_node(node: &Node) -> Self {
330 Self {
331 point: NodePoint::from_node(node),
332 g: node.g,
333 walked_distance: node.walked_distance,
334 cost_malus: node.cost_malus,
335 in_open_set: node.in_open_set(),
336 }
337 }
338}
339
340fn compare_unreached_paths(left: &Path, right: &Path) -> Ordering {
341 left.dist_to_target()
342 .total_cmp(&right.dist_to_target())
343 .then_with(|| left.node_count().cmp(&right.node_count()))
344}
345
346const fn path_node_from(node: &Node) -> Node {
347 let mut path_node = Node::new(node.x, node.y, node.z);
348 path_node.g = node.g;
349 path_node.h = node.h;
350 path_node.f = node.f;
351 path_node.came_from = node.came_from;
352 path_node.closed = node.closed;
353 path_node.walked_distance = node.walked_distance;
354 path_node.cost_malus = node.cost_malus;
355 path_node.path_type = node.path_type;
356 path_node
357}
358
359#[cfg(test)]
360mod tests {
361 use std::ops::RangeInclusive;
362
363 use steel_registry::{REGISTRY, init_vanilla_registry, vanilla_blocks};
364 use steel_utils::{BlockPos, BlockStateId, WorldAabb};
365 use steel_worldgen::density_functions::overworld::OverworldNoiseSettings;
366
367 use super::{PathFinder, PathRequest};
368 use crate::behavior::init_behaviors;
369 use crate::entity::ai::node::Node;
370 use crate::entity::ai::path::{PathfindingContext, PathfindingMalus};
371 use crate::entity::ai::walk::{MobPathSettings, WalkNodeEvaluator};
372 use crate::world::LevelReader;
373
374 struct GridLevel {
375 default_state: BlockStateId,
376 states: Vec<(BlockPos, BlockStateId)>,
377 }
378
379 impl GridLevel {
380 fn new(default_state: BlockStateId) -> Self {
381 Self {
382 default_state,
383 states: Vec::new(),
384 }
385 }
386
387 fn with(mut self, pos: BlockPos, state: BlockStateId) -> Self {
388 self.states.push((pos, state));
389 self
390 }
391 }
392
393 impl LevelReader for GridLevel {
394 fn get_block_state(&self, pos: BlockPos) -> BlockStateId {
395 self.states
396 .iter()
397 .find_map(|(state_pos, state)| (*state_pos == pos).then_some(*state))
398 .unwrap_or(self.default_state)
399 }
400
401 fn raw_brightness(&self, _pos: BlockPos, _sky_darkening: u8) -> u8 {
402 0
403 }
404
405 fn min_y(&self) -> i32 {
406 OverworldNoiseSettings::MIN_Y
407 }
408
409 fn height(&self) -> i32 {
410 OverworldNoiseSettings::HEIGHT
411 }
412
413 fn sea_level(&self) -> i32 {
414 OverworldNoiseSettings::SEA_LEVEL
415 }
416 }
417
418 #[test]
419 fn pathfinder_finds_direct_walkable_path() {
420 init_vanilla_registry();
421 init_behaviors();
422
423 let level = flat_level(0..=2, -1..=1);
424 let mut context = PathfindingContext::new(&level, BlockPos::new(0, 64, 0));
425 let mut evaluator = WalkNodeEvaluator::new(test_settings(1, 1, 1));
426 let mut no_collision = |_aabb: WorldAabb| false;
427 let mut pathfinder = PathFinder::new(128);
428
429 let path = pathfinder.find_path(
430 &mut evaluator,
431 &mut context,
432 &mut no_collision,
433 PathRequest {
434 targets: &[BlockPos::new(2, 64, 0)],
435 max_path_length: 16.0,
436 reach_range: 0,
437 max_visited_nodes_multiplier: 1.0,
438 },
439 );
440
441 let Some(path) = path else {
442 panic!("path should be found");
443 };
444 assert!(path.can_reach());
445 assert_eq!(path.node_count(), 3);
446 assert_eq!(
447 path.nodes()
448 .iter()
449 .map(Node::as_block_pos)
450 .collect::<Vec<_>>(),
451 vec![
452 BlockPos::new(0, 64, 0),
453 BlockPos::new(1, 64, 0),
454 BlockPos::new(2, 64, 0)
455 ]
456 );
457 }
458
459 #[test]
460 fn pathfinder_returns_closest_path_when_target_is_not_reached() {
461 init_vanilla_registry();
462 init_behaviors();
463
464 let level = flat_level(0..=4, -1..=1);
465 let mut context = PathfindingContext::new(&level, BlockPos::new(0, 64, 0));
466 let mut evaluator = WalkNodeEvaluator::new(test_settings(1, 1, 1));
467 let mut no_collision = |_aabb: WorldAabb| false;
468 let mut pathfinder = PathFinder::new(128);
469
470 let path = pathfinder.find_path(
471 &mut evaluator,
472 &mut context,
473 &mut no_collision,
474 PathRequest {
475 targets: &[BlockPos::new(4, 64, 0)],
476 max_path_length: 1.5,
477 reach_range: 0,
478 max_visited_nodes_multiplier: 1.0,
479 },
480 );
481
482 let Some(path) = path else {
483 panic!("closest path should be returned");
484 };
485 assert!(!path.can_reach());
486 assert_eq!(
487 path.end_node().map(Node::as_block_pos),
488 Some(BlockPos::new(1, 64, 0))
489 );
490 assert_eq!(path.dist_to_target().to_bits(), 3.0_f32.to_bits());
491 }
492
493 fn flat_level(x_range: RangeInclusive<i32>, z_range: RangeInclusive<i32>) -> GridLevel {
494 let air = REGISTRY.blocks.get_default_state_id(&vanilla_blocks::AIR);
495 let stone = REGISTRY.blocks.get_default_state_id(&vanilla_blocks::STONE);
496 let mut level = GridLevel::new(air);
497 for x in x_range {
498 for z in z_range.clone() {
499 level = level.with(BlockPos::new(x, 63, z), stone);
500 }
501 }
502 level
503 }
504
505 fn test_settings(entity_width: i32, entity_height: i32, entity_depth: i32) -> MobPathSettings {
506 MobPathSettings::new(
507 entity_width,
508 entity_height,
509 entity_depth,
510 BlockPos::new(0, 64, 0),
511 &PathfindingMalus::new(),
512 )
513 }
514}