Merge branch 'main' into ndr/small-fixes

rust/routee-compass-core/src/algorithm/search/a_star/a_star_algorithm.rs

@@ -1,3 +1,4 @@
+use crate::algorithm::search::direction::Direction;
 use crate::algorithm::search::edge_traversal::EdgeTraversal;
 use crate::algorithm::search::search_error::SearchError;
 use crate::algorithm::search::search_instance::SearchInstance;
@@ -8,7 +9,7 @@ use crate::model::road_network::vertex_id::VertexId;
 use crate::model::unit::cost::ReverseCost;
 use crate::model::unit::Cost;
 use crate::util::priority_queue::InternalPriorityQueue;
-use priority_queue::PriorityQueue;
+
 use std::collections::HashMap;
 use std::time::Instant;
 
@@ -20,15 +21,15 @@ use std::time::Instant;
 pub fn run_a_star(
     source: VertexId,
     target: Option<VertexId>,
+    direction: &Direction,
     si: &SearchInstance,
 ) -> Result<SearchResult, SearchError> {
     if target.map_or(false, |t| t == source) {
         return Ok(SearchResult::default());
     }
 
     // context for the search (graph, search functions, frontier priority queue)
-    let mut costs: InternalPriorityQueue<VertexId, ReverseCost> =
-        InternalPriorityQueue(PriorityQueue::new());
+    let mut costs: InternalPriorityQueue<VertexId, ReverseCost> = InternalPriorityQueue::default();
     let mut traversal_costs: HashMap<VertexId, Cost> = HashMap::new();
     let mut solution: HashMap<VertexId, SearchTreeBranch> = HashMap::new();
 
@@ -45,60 +46,18 @@ pub fn run_a_star(
     let mut iterations = 0;
 
     loop {
-        // handle app-level termination conditions
-        let should_terminate =
-            si.termination_model
-                .terminate_search(&start_time, solution.len(), iterations)?;
-
-        if should_terminate {
-            let explanation =
-                si.termination_model
-                    .explain_termination(&start_time, solution.len(), iterations);
-            match explanation {
-                None => {
-                    return Err(SearchError::InternalSearchError(format!(
-                        "unable to explain termination with start_time, solution_size, iterations: {:?}, {}, {}",
-                        &start_time,
-                        solution.len(),
-                        iterations
-                    )))
-                }
-                Some(msg) => return Err(SearchError::QueryTerminated(msg)),
-            }
-        }
+        si.termination_model
+            .test(&start_time, solution.len(), iterations)?;
 
-        // grab the current vertex id, but handle some other termination conditions
-        // based on the state of the priority queue and optional search destination
-        // - we reach the destination                                       (Ok)
-        // - if the set is ever empty and there's no destination            (Ok)
-        // - if the set is ever empty and there's a destination             (Err)
-        let current_vertex_id = match (costs.pop(), target) {
-            (None, Some(target_vertex_id)) => {
-                return Err(SearchError::NoPathExists(source, target_vertex_id))
-            }
-            (None, None) => break,
-            (Some((current_v, _)), Some(target_v)) if current_v == target_v => break,
-            (Some((current_vertex_id, _)), _) => current_vertex_id,
+        let current_vertex_id = match advance_search(&mut costs, source, target)? {
+            None => break,
+            Some(id) => id,
         };
 
-        let previous_edge = if current_vertex_id == source {
-            None
-        } else {
-            let edge_id = solution
-                .get(&current_vertex_id)
-                .ok_or_else(|| {
-                    SearchError::InternalSearchError(format!(
-                        "expected vertex id {} missing from solution",
-                        current_vertex_id
-                    ))
-                })?
-                .edge_traversal
-                .edge_id;
-            let edge = si
-                .directed_graph
-                .get_edge(edge_id)
-                .map_err(SearchError::GraphError)?;
-            Some(edge)
+        let last_edge_id = get_last_traversed_edge_id(&current_vertex_id, &source, &solution)?;
+        let last_edge = match last_edge_id {
+            Some(id) => Some(si.directed_graph.get_edge(id)?),
+            None => None,
         };
 
         // grab the current state from the solution
@@ -119,52 +78,48 @@ pub fn run_a_star(
         };
 
         // visit all neighbors of this source vertex
-        for edge_id in si.directed_graph.out_edges_iter(current_vertex_id)? {
+        let incident_edge_iterator = direction.get_incident_edges(&current_vertex_id, si)?;
+        for edge_id in incident_edge_iterator {
             let e = si.directed_graph.get_edge(*edge_id)?;
-            let src_id = e.src_vertex_id;
-            let dst_id = e.dst_vertex_id;
-
-            if !si.frontier_model.valid_frontier(
-                e,
-                &current_state,
-                previous_edge,
-                &si.state_model,
-            )? {
+
+            let terminal_vertex_id = direction.terminal_vertex_id(e);
+            let key_vertex_id = direction.tree_key_vertex_id(e);
+
+            let valid_frontier =
+                si.frontier_model
+                    .valid_frontier(e, &current_state, last_edge, &si.state_model)?;
+            if !valid_frontier {
                 continue;
             }
-            let et = EdgeTraversal::perform_traversal(
-                *edge_id,
-                previous_edge.map(|pe| pe.edge_id),
-                &current_state,
-                si,
-            )?;
+            let et =
+                direction.perform_edge_traversal(*edge_id, last_edge_id, &current_state, si)?;
             let current_gscore = traversal_costs
-                .get(&src_id)
+                .get(&terminal_vertex_id)
                 .unwrap_or(&Cost::INFINITY)
                 .to_owned();
             let tentative_gscore = current_gscore + et.total_cost();
             let existing_gscore = traversal_costs
-                .get(&dst_id)
+                .get(&key_vertex_id)
                 .unwrap_or(&Cost::INFINITY)
                 .to_owned();
             if tentative_gscore < existing_gscore {
-                traversal_costs.insert(dst_id, tentative_gscore);
+                traversal_costs.insert(key_vertex_id, tentative_gscore);
 
                 // update solution
                 let traversal = SearchTreeBranch {
-                    terminal_vertex: src_id,
+                    terminal_vertex: terminal_vertex_id,
                     edge_traversal: et,
                 };
-                solution.insert(dst_id, traversal);
+                solution.insert(key_vertex_id, traversal);
 
                 let dst_h_cost = match target {
                     None => Cost::ZERO,
                     Some(target_v) => {
-                        si.estimate_traversal_cost(dst_id, target_v, &current_state)?
+                        si.estimate_traversal_cost(key_vertex_id, target_v, &current_state)?
                     }
                 };
                 let f_score_value = tentative_gscore + dst_h_cost;
-                costs.push_increase(dst_id, f_score_value.into());
+                costs.push_increase(key_vertex_id, f_score_value.into());
             }
         }
         iterations += 1;
@@ -221,6 +176,7 @@ pub fn run_a_star(
 pub fn run_a_star_edge_oriented(
     source: EdgeId,
     target: Option<EdgeId>,
+    direction: &Direction,
     si: &SearchInstance,
 ) -> Result<SearchResult, SearchError> {
     // 1. guard against edge conditions (src==dst, src.dst_v == dst.src_v)
@@ -242,7 +198,7 @@ pub fn run_a_star_edge_oriented(
             let SearchResult {
                 mut tree,
                 iterations,
-            } = run_a_star(e1_dst, None, si)?;
+            } = run_a_star(e1_dst, None, direction, si)?;
             if !tree.contains_key(&e1_dst) {
                 tree.extend([(e1_dst, src_branch)]);
             }
@@ -261,8 +217,8 @@ pub fn run_a_star_edge_oriented(
             } else if e1_dst == e2_src {
                 // route is simply source -> target
                 let init_state = si.state_model.initial_state()?;
-                let src_et = EdgeTraversal::perform_traversal(source, None, &init_state, si)?;
-                let dst_et = EdgeTraversal::perform_traversal(
+                let src_et = EdgeTraversal::forward_traversal(source, None, &init_state, si)?;
+                let dst_et = EdgeTraversal::forward_traversal(
                     target_edge,
                     Some(source),
                     &src_et.result_state,
@@ -287,7 +243,7 @@ pub fn run_a_star_edge_oriented(
                 let SearchResult {
                     mut tree,
                     iterations,
-                } = run_a_star(e1_dst, Some(e2_src), si)?;
+                } = run_a_star(e1_dst, Some(e2_src), direction, si)?;
 
                 if tree.is_empty() {
                     return Err(SearchError::NoPathExists(e1_dst, e2_src));
@@ -328,6 +284,72 @@ pub fn run_a_star_edge_oriented(
     }
 }
 
+/// grab the current vertex id, but handle some other termination conditions
+/// based on the state of the priority queue and optional search destination
+/// - we reach the destination                                       (Ok)
+/// - if the set is ever empty and there's no destination            (Ok)
+/// - if the set is ever empty and there's a destination             (Err)
+///
+/// # Arguments
+/// * `cost`   - queue of priority-ranked vertices for exploration
+/// * `source` - search source vertex
+/// * `target` - optional search destination
+///
+/// # Results
+/// The next vertex to search. None if the queue has been exhausted in a search with no
+/// destination, or we have reached our destination.
+/// An error if no path exists for a search that includes a destination.
+fn advance_search(
+    cost: &mut InternalPriorityQueue<VertexId, ReverseCost>,
+    source: VertexId,
+    target: Option<VertexId>,
+) -> Result<Option<VertexId>, SearchError> {
+    match (cost.pop(), target) {
+        (None, Some(target_vertex_id)) => Err(SearchError::NoPathExists(source, target_vertex_id)),
+        (None, None) => Ok(None),
+        (Some((current_v, _)), Some(target_v)) if current_v == target_v => Ok(None),
+        (Some((current_vertex_id, _)), _) => Ok(Some(current_vertex_id)),
+    }
+}
+
+/// Find the last-traversed edge before reaching this vertex id.
+/// The logic is the same for forward and reverse searches but finds
+/// a different result because the trees are different.
+/// Forward case: find `prev` from v2 in `(v1)-[prev]->(v2)-[next]->(v3)`
+/// Reverse case: find `next` from v2 in `(v1)-[prev]->(v2)-[next]->(v3)`
+///
+/// # Arguments
+/// * `this_vertex_id`  - current vertex, v2 in diagram
+/// * `first_vertex_id` - source of this search, the origin vertex in a forward
+///                       search or the destination vertex in a reverse search
+/// * `tree`            - current search solution tree
+///
+/// # Returns
+///
+/// The EdgeId for the edge that was traversed to reach this vertex, or None
+/// if no edges have yet been traversed.
+fn get_last_traversed_edge_id(
+    this_vertex_id: &VertexId,
+    first_vertex_id: &VertexId,
+    tree: &HashMap<VertexId, SearchTreeBranch>,
+) -> Result<Option<EdgeId>, SearchError> {
+    if this_vertex_id == first_vertex_id {
+        Ok(None)
+    } else {
+        let edge_id = tree
+            .get(this_vertex_id)
+            .ok_or_else(|| {
+                SearchError::InternalSearchError(format!(
+                    "expected vertex id {} missing from solution",
+                    this_vertex_id
+                ))
+            })?
+            .edge_traversal
+            .edge_id;
+        Ok(Some(edge_id))
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;
@@ -473,7 +495,8 @@ mod tests {
             .clone()
             .into_par_iter()
             .map(|(o, d, _expected)| {
-                run_a_star(o, Some(d), &si).map(|search_result| search_result.tree)
+                run_a_star(o, Some(d), &Direction::Forward, &si)
+                    .map(|search_result| search_result.tree)
             })
             .collect();
 

rust/routee-compass-core/src/algorithm/search/a_star/bidirectional_a_star_algorithm.rs

@@ -0,0 +1,78 @@
+use itertools::Itertools;
+
+use crate::algorithm::search::{
+    edge_traversal::EdgeTraversal, search_error::SearchError, search_instance::SearchInstance,
+};
+
+/// helper function to address how the reverse route state and costs are assigned.
+///
+/// the state values in the reverse route monotonically increase in the reverse direction.
+/// the marginal state changes need to be extracted for each link and then added to the final
+/// state of the forward route.
+///
+/// finally, the updated route is reversed so that it can be appended to the forward route.
+///
+/// # Arguments
+/// * `fwd_route` - the forward-oriented route of a bidirectional search
+/// * `rev_route` - the reverse-oriented route of a bidirectional search (mutated here)
+/// * `si`        - the search instance
+pub fn reorient_reverse_route(
+    fwd_route: &[EdgeTraversal],
+    rev_route: &[EdgeTraversal],
+    si: &SearchInstance,
+) -> Result<Vec<EdgeTraversal>, SearchError> {
+    // get the final edge id and state for the forward traversal
+    let (final_fwd_edge_id, mut acc_state) = match fwd_route.last() {
+        None => (
+            None,
+            si.state_model
+                .initial_state()
+                .map_err(SearchError::StateError)?,
+        ),
+        Some(last_edge) => (Some(last_edge.edge_id), last_edge.result_state.clone()),
+    };
+
+    // get all edge ids along the reverse route when traversed in forward direction,
+    // appending the (optional) last edge id of the forward route
+    let mut edge_ids = rev_route
+        .iter()
+        .rev()
+        .map(|e| Some(e.edge_id))
+        .collect_vec();
+    edge_ids.insert(0, final_fwd_edge_id);
+
+    // re-create all EdgeTraversal instances from each successive edge id pair, building
+    // from the final state of the forward traversal
+    let mut result: Vec<EdgeTraversal> = Vec::with_capacity(rev_route.len());
+    for (prev_opt, next_opt) in edge_ids.iter().tuple_windows() {
+        let next = next_opt.ok_or_else(|| {
+            SearchError::InternalSearchError(String::from("next_opt should never be None"))
+        })?;
+        let et = EdgeTraversal::forward_traversal(next, *prev_opt, &acc_state, si)?;
+        acc_state = et.result_state.clone();
+        result.push(et);
+    }
+
+    Ok(result)
+}
+
+/// identifies routes that have loops by checking if any two edges share
+/// the same source vertex.
+///
+/// # Arguments
+/// * `route` - the route to check
+/// * `si`    - the search instance
+///
+/// # Returns
+///
+/// true if there is a loop
+pub fn route_contains_loop(
+    route: &[EdgeTraversal],
+    si: &SearchInstance,
+) -> Result<bool, SearchError> {
+    let src_vertices = route
+        .iter()
+        .map(|e| si.directed_graph.src_vertex_id(e.edge_id))
+        .collect::<Result<Vec<_>, _>>()?;
+    Ok(src_vertices.iter().unique().collect_vec().len() < src_vertices.len())
+}

rust/routee-compass-core/src/algorithm/search/a_star/mod.rs

@@ -1 +1,2 @@
 pub mod a_star_algorithm;
+pub mod bidirectional_a_star_algorithm;