From d18100c87af8a061cf1fe4f4f0e429af9db97152 Mon Sep 17 00:00:00 2001 From: Imbris Date: Fri, 21 Apr 2023 23:16:24 -0400 Subject: [PATCH] Add a max cost parameter to the astar algorithm so that it will terminate as exausted if this limit is reached. This is used to optimize site pathfinding by exiting early from finding a novel path if we know it won't be used. --- common/src/astar.rs | 38 ++++++++++++++++++++++++++++++++++---- world/src/civ/mod.rs | 27 +++++++++++---------------- 2 files changed, 45 insertions(+), 20 deletions(-) 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()