Add find_path algorithm and tests

src/algorithms.rs

@@ -2,7 +2,7 @@ use std::cmp::Ordering;
 use std::collections::{BinaryHeap, VecDeque};
 
 use crate::maps::VertexMap;
-use crate::traits::GraphTopology;
+use crate::traits::{GraphTopology, IncidenceCursor};
 
 #[derive(PartialEq, Eq)]
 struct DistanceOrderedVertex<V> {
@@ -260,3 +260,57 @@ where
     }
     (visited, None)
 }
+
+pub fn find_path<G>(graph: &G, source: G::Vertex, target: G::Vertex) -> Option<Vec<G::Edge>>
+where
+    G: GraphTopology,
+{
+    find_path_where(graph, source, |v| v == target)
+}
+
+pub fn find_path_where<G, P>(graph: &G, source: G::Vertex, predicate: P) -> Option<Vec<G::Edge>>
+where
+    G: GraphTopology,
+    P: Fn(G::Vertex) -> bool,
+{
+    if predicate(source) {
+        return Some(vec![]);
+    }
+
+    let mut visited = graph.vertex_map(false);
+    visited[source] = true;
+
+    struct Frame<G: GraphTopology> {
+        arrival_edge: Option<G::Edge>,
+        cursor: G::IncidenceCursor,
+    }
+
+    let mut stack: Vec<Frame<G>> = vec![Frame {
+        arrival_edge: None,
+        cursor: graph.incidence_cursor(source),
+    }];
+
+    while let Some(frame) = stack.last_mut() {
+        match frame.cursor.next(graph) {
+            None => {
+                stack.pop();
+            }
+            Some((neighbor, edge)) => {
+                if predicate(neighbor) {
+                    let mut path: Vec<G::Edge> =
+                        stack.iter().filter_map(|f| f.arrival_edge).collect();
+                    path.push(edge);
+                    return Some(path);
+                }
+                if !visited[neighbor] {
+                    visited[neighbor] = true;
+                    stack.push(Frame {
+                        arrival_edge: Some(edge),
+                        cursor: graph.incidence_cursor(neighbor),
+                    });
+                }
+            }
+        }
+    }
+    None
+}