diff --git a/common/src/astar.rs b/common/src/astar.rs index c5895ffa8a..bed2874415 100644 --- a/common/src/astar.rs +++ b/common/src/astar.rs @@ -45,9 +45,19 @@ impl PartialOrd for PathEntry { } pub enum PathResult { + /// No reachable nodes were satisfactory. + /// + /// Contains path to node with the lowest heuristic value (out of the + /// explored nodes). None(Path), + /// Either max_iters or max_cost was reached. + /// + /// Contains path to node with the lowest heuristic value (out of the + /// explored nodes). Exhausted(Path), - // second field is cost + /// Path succefully found. + /// + /// Second field is cost. Path(Path, f32), Pending, } @@ -84,6 +94,7 @@ struct NodeEntry { pub struct Astar { iter: usize, max_iters: usize, + max_cost: f32, potential_nodes: BinaryHeap>, // cost, node pairs visited_nodes: HashMap, Hasher>, /// Node with the lowest heuristic value so far. @@ -109,6 +120,7 @@ impl Astar { pub fn new(max_iters: usize, start: S, hasher: H) -> Self { Self { max_iters, + max_cost: f32::MAX, iter: 0, potential_nodes: core::iter::once(PathEntry { cost_estimate: 0.0, @@ -127,6 +139,11 @@ impl Astar { } } + pub fn with_max_cost(mut self, max_cost: f32) -> Self { + self.max_cost = max_cost; + self + } + pub fn poll( &mut self, iters: usize, @@ -143,15 +160,28 @@ impl Astar { { let iter_limit = self.max_iters.min(self.iter + iters); while self.iter < iter_limit { - if let Some(PathEntry { node, .. }) = self.potential_nodes.pop() { + if let Some(PathEntry { + node, + cost_estimate, + }) = self.potential_nodes.pop() + { let (node_cheapest, came_from) = self .visited_nodes .get(&node) .map(|n| (n.cheapest_score, n.came_from.clone())) - .expect(""); + .expect("All nodes in the queue should be included in visisted_nodes"); if satisfied(&node) { return PathResult::Path(self.reconstruct_path_to(node), node_cheapest); + // Note, we assume that cost_estimate isn't an overestimation + // (i.e. that `heuristic` doesn't overestimate). + } else if cost_estimate > self.max_cost { + return PathResult::Exhausted( + self.closest_node + .clone() + .map(|(lc, _)| self.reconstruct_path_to(lc)) + .unwrap_or_default(), + ); } else { for (neighbor, transition) in neighbors(&node) { if neighbor == came_from { @@ -174,7 +204,7 @@ impl Astar { }) .is_some(); let h = heuristic(&neighbor, &node); - // note that cheapest_scores does not include the heuristic + // note that cheapest_score does not include the heuristic // priority queue does include heuristic let cost_estimate = cost + h; diff --git a/world/src/civ/mod.rs b/world/src/civ/mod.rs index 130f12c243..a4932b58d9 100644 --- a/world/src/civ/mod.rs +++ b/world/src/civ/mod.rs @@ -1130,10 +1130,12 @@ impl Civs { // Find neighbors // Note, the maximum distance that I have so far observed not hitting the - // iteration limit in `find_path` is 297. So I think this is a reasonible + // iteration limit in `find_path` is 364. So I think this is a reasonible // limit (although the relationship between distance and pathfinding iterations - // can be a bit variable). - const MAX_NEIGHBOR_DISTANCE: f32 = 350.0; + // can be a bit variable). Note, I have seen paths reach the iteration limit + // with distances as small as 137, so this certainly doesn't catch all + // cases that would fail. + const MAX_NEIGHBOR_DISTANCE: f32 = 400.0; let mut nearby = self .sites .iter() @@ -1173,18 +1175,8 @@ impl Civs { let start = loc; let end = self.sites.get(nearby).center; // Find a novel path. - // - // We rely on the cost at least being equal to the distance, to avoid - // unnecessary novel pathfinding. - let maybe_path = ((start.distance_squared(end) as f32).sqrt() < max_novel_cost) - .then(|| { - prof_span!("find path"); - let get_bridge = |start| self.bridges.get(&start).map(|(end, _)| *end); - find_path(ctx, get_bridge, start, end) - }) - .flatten() - .filter(|&(_, cost)| cost < max_novel_cost); - if let Some((path, cost)) = maybe_path { + let get_bridge = |start| self.bridges.get(&start).map(|(end, _)| *end); + if let Some((path, cost)) = find_path(ctx, get_bridge, start, end, max_novel_cost) { // Write the track to the world as a path for locs in path.nodes().windows(3) { let mut randomize_offset = false; @@ -1328,7 +1320,9 @@ fn find_path( get_bridge: impl Fn(Vec2) -> Option>, a: Vec2, b: Vec2, + max_path_cost: f32, ) -> Option<(Path>, f32)> { + prof_span!("find_path"); const MAX_PATH_ITERS: usize = 100_000; let sim = &ctx.sim; // NOTE: If heuristic overestimates the actual cost, then A* is not guaranteed @@ -1355,7 +1349,8 @@ fn find_path( MAX_PATH_ITERS, a, BuildHasherDefault::::default(), - ); + ) + .with_max_cost(max_path_cost); astar .poll(MAX_PATH_ITERS, heuristic, neighbors, satisfied) .into_path()