berkus
/
topola
mirror of https://codeberg.org/topola/topola.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

router: step on every probe, not only navvertices

This commit is contained in:
Mikolaj Wielgus 2024-07-02 02:36:42 +02:00
parent 3c578ccada
commit 27d1c9e712
3 changed files with 82 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
src/router/astar.rs
View File

@ -5,7 +5,7 @@
* Copyright (c) 2015
**/
use std::collections::hash_map::Entry::{Occupied, Vacant};
use std::collections::{BinaryHeap, HashMap};
use std::collections::{BinaryHeap, HashMap, VecDeque};
use std::hash::Hash;
@ -100,7 +100,7 @@ pub trait AstarStrategy<G, K, R>
where
G: GraphBase,
G::NodeId: Eq + Hash,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
K: Measure + Copy,
{
fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker<G>) -> Option<R>;
@ -112,12 +112,15 @@ where
fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K;
}
#[derive(Debug)]
pub trait MakeEdgeRef: IntoEdgeReferences {
fn edge_ref(&self, edge_id: Self::EdgeId) -> Self::EdgeRef;
}
pub struct Astar<G, K>
where
G: GraphBase,
G::NodeId: Eq + Hash,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
K: Measure + Copy,
{
pub graph: G,
@ -125,6 +128,9 @@ where
pub scores: HashMap<G::NodeId, K>,
pub estimate_scores: HashMap<G::NodeId, K>,
pub path_tracker: PathTracker<G>,
pub maybe_curr_node: Option<G::NodeId>,
// FIXME: To work around edge references borrowing from the graph we collect then reiterate over tem.
pub edge_ids: VecDeque<G::EdgeId>,
}
#[derive(Error, Debug, Clone)]
@ -138,7 +144,7 @@ pub enum AstarStatus<G, K, R>
where
G: GraphBase,
G::NodeId: Eq + Hash,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
K: Measure + Copy,
{
Running,
@ -149,7 +155,7 @@ impl<G, K> Astar<G, K>
where
G: GraphBase,
G::NodeId: Eq + Hash,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
K: Measure + Copy,
{
pub fn new<R>(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy<G, K, R>) -> Self {
@ -159,6 +165,8 @@ where
scores: HashMap::new(),
estimate_scores: HashMap::new(),
path_tracker: PathTracker::<G>::new(),
maybe_curr_node: None,
edge_ids: VecDeque::new(),
};
let zero_score = K::default();
@ -174,57 +182,66 @@ where
&mut self,
strategy: &mut impl AstarStrategy<G, K, R>,
) -> Result<AstarStatus<G, K, R>, AstarError> {
let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else {
return Err(AstarError::NotFound);
};
if let Some(curr_node) = self.maybe_curr_node {
if let Some(edge_id) = self.edge_ids.pop_front() {
// This lookup can be unwrapped without fear of panic since the node was
// necessarily scored before adding it to `visit_next`.
let node_score = self.scores[&curr_node];
let edge = (&self.graph).edge_ref(edge_id);
if let Some(result) = strategy.is_goal(&self.graph, node, &self.path_tracker) {
let path = self.path_tracker.reconstruct_path_to(node);
let cost = self.scores[&node];
return Ok(AstarStatus::Finished(cost, path, result));
}
if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) {
let next = edge.target();
let next_score = node_score + edge_cost;
// This lookup can be unwrapped without fear of panic since the node was
// necessarily scored before adding it to `visit_next`.
let node_score = self.scores[&node];
match self.estimate_scores.entry(node) {
Occupied(mut entry) => {
// If the node has already been visited with an equal or lower score than
// now, then we do not need to re-visit it.
if *entry.get() <= estimate_score {
return Ok(AstarStatus::Running);
}
entry.insert(estimate_score);
}
Vacant(entry) => {
entry.insert(estimate_score);
}
}
for edge in self.graph.edges(node) {
if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) {
let next = edge.target();
let next_score = node_score + edge_cost;
match self.scores.entry(next) {
Occupied(mut entry) => {
// No need to add neighbors that we have already reached through a
// shorter path than now.
if *entry.get() <= next_score {
return Ok(AstarStatus::Running);
match self.scores.entry(next) {
Occupied(mut entry) => {
// No need to add neighbors that we have already reached through a
// shorter path than now.
if *entry.get() <= next_score {
return Ok(AstarStatus::Running);
}
entry.insert(next_score);
}
Vacant(entry) => {
entry.insert(next_score);
}
entry.insert(next_score);
}
Vacant(entry) => {
entry.insert(next_score);
}
}
self.path_tracker.set_predecessor(next, node);
let next_estimate_score = next_score + strategy.estimate_cost(&self.graph, next);
self.visit_next.push(MinScored(next_estimate_score, next));
self.path_tracker.set_predecessor(next, curr_node);
let next_estimate_score =
next_score + strategy.estimate_cost(&self.graph, next);
self.visit_next.push(MinScored(next_estimate_score, next));
}
} else {
self.maybe_curr_node = None;
}
} else {
let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else {
return Err(AstarError::NotFound);
};
if let Some(result) = strategy.is_goal(&self.graph, node, &self.path_tracker) {
let path = self.path_tracker.reconstruct_path_to(node);
let cost = self.scores[&node];
return Ok(AstarStatus::Finished(cost, path, result));
}
match self.estimate_scores.entry(node) {
Occupied(mut entry) => {
// If the node has already been visited with an equal or lower score than
// now, then we do not need to re-visit it.
if *entry.get() <= estimate_score {
return Ok(AstarStatus::Running);
}
entry.insert(estimate_score);
}
Vacant(entry) => {
entry.insert(estimate_score);
}
}
self.maybe_curr_node = Some(node);
self.edge_ids = self.graph.edges(node).map(|edge| edge.id()).collect();
}
Ok(AstarStatus::Running)
@ -239,7 +256,7 @@ pub fn astar<G, K, R>(
where
G: GraphBase,
G::NodeId: Eq + Hash,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
K: Measure + Copy,
{
let mut astar = Astar::new(graph, start, strategy);

13
src/router/navmesh.rs
View File

@ -26,6 +26,7 @@ use crate::{
geometry::shape::ShapeTrait,
graph::GetPetgraphIndex,
layout::Layout,
router::astar::MakeEdgeRef,
triangulation::{GetTrianvertexNodeIndex, Triangulation},
};
@ -322,3 +323,15 @@ impl<'a> IntoEdges for &'a Navmesh {
)
}
}
impl<'a> MakeEdgeRef for &'a Navmesh {
fn edge_ref(
&self,
edge_id: <&'a Navmesh as GraphBase>::EdgeId,
) -> <&'a Navmesh as IntoEdgeReferences>::EdgeRef {
NavmeshEdgeReference {
from: edge_id.0,
to: edge_id.1,
}
}
}

1
src/router/route.rs
View File

@ -31,7 +31,6 @@ use crate::{
},
};
#[derive(Debug)]
pub struct Route {
astar: Astar<Navmesh, f64>,
trace: Trace,