voxel: Reconstruct path from init consuming parent_map

paradise-fi · Nov 13, 2023 · 8f5932e · 8f5932e
1 parent 68468d7
commit 8f5932e
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 7 deletions.
diff --git a/softwareComponents/voxelReconfig/src/algs/astar/mod.rs b/softwareComponents/voxelReconfig/src/algs/astar/mod.rs
@@ -40,9 +40,7 @@ where
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, |parent_info| {
-        parent_info.0.clone()
-    }))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p.0))
 }
 
 fn compute_parents<TGraph, TMetric>(

diff --git a/softwareComponents/voxelReconfig/src/algs/astar/opt.rs b/softwareComponents/voxelReconfig/src/algs/astar/opt.rs
@@ -42,9 +42,7 @@ where
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, |parent_info| {
-        parent_info.0.clone()
-    }))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p.0))
 }
 
 fn compute_parents<TGraph, TMetric>(

diff --git a/softwareComponents/voxelReconfig/src/algs/bfs.rs b/softwareComponents/voxelReconfig/src/algs/bfs.rs
@@ -27,7 +27,7 @@ pub fn compute_path<TGraph: StateGraph>(
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, Clone::clone))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p))
 }
 
 fn compute_parents<TGraph: StateGraph>(

diff --git a/softwareComponents/voxelReconfig/src/algs/mod.rs b/softwareComponents/voxelReconfig/src/algs/mod.rs
@@ -31,6 +31,36 @@ pub trait StateGraph {
 }
 
 fn reconstruct_path_to<TItem: Eq + std::hash::Hash, TParentInfo, S: BuildHasher>(
+    goal: TItem,
+    mut parent_map: HashMap<TItem, TParentInfo, S>,
+    mut get_parent: impl FnMut(TParentInfo) -> Option<TItem>,
+) -> Vec<TItem> {
+    assert!(parent_map.contains_key(&goal), "Missing goal parent");
+    let mut path = Vec::new();
+    let mut parent = goal;
+    loop {
+        if let Some(new_parent_info) = parent_map.remove(&parent) {
+            if let Some(new_parent) = get_parent(new_parent_info) {
+                path.push(std::mem::replace(&mut parent, new_parent));
+            } else {
+                path.push(parent);
+                break;
+            }
+        } else {
+            if path.contains(&parent) {
+                panic!("Cyclic dependency in parent graph");
+            } else {
+                panic!("Missing parent info");
+            }
+        }
+    }
+
+    path.reverse();
+    path
+}
+
+#[allow(unused)]
+fn reconstruct_path_to_noconsume<TItem: Eq + std::hash::Hash, TParentInfo, S: BuildHasher>(
     goal: TItem,
     parent_map: &HashMap<TItem, TParentInfo, S>,
     mut get_parent: impl FnMut(&TParentInfo) -> Option<TItem>,