Move rrt algorithm into its own function

common/src/path.rs

@@ -673,16 +673,6 @@ where
 }
 
 // Enable when airbraking/sensible flight is a thing
-/// Attempts to find a path from a start to the end using an informed
-/// RRT-Connect algorithm. A point is sampled from a bounding spheroid
-/// between the start and end. Two separate rapidly exploring random
-/// trees extend toward the sampled point. Nodes are stored in k-d trees
-/// for quicker nearest node calculations. Points are sampled until the
-/// trees connect. A final path is then reconstructed from the nodes.
-/// This pathfinding algorithm is more appropriate for 3D pathfinding
-/// with wider gaps, such as flying through a forest than for terrain
-/// with narrow gaps, such as navigating a maze.
-/// Returns a path and whether that path is complete or not.
 #[cfg(rrt_pathfinding)]
 fn find_air_path<V>(
     vol: &V,
@@ -694,7 +684,6 @@ where
     V: BaseVol<Vox = Block> + ReadVol,
 {
     let radius = traversal_cfg.node_tolerance;
-    let mut path = Vec::new();
     let mut connect = false;
     let total_dist_sqrd = startf.distance_squared(endf);
     // First check if a straight line path works
@@ -706,8 +695,10 @@ where
         .powi(2)
         >= total_dist_sqrd
     {
+        let mut path = Vec::new();
         path.push(endf.map(|e| e.floor() as i32));
         connect = true;
+        (Some(path.into_iter().collect()), connect)
     // Else use RRTs
     } else {
         let is_traversable = |start: &Vec3<f32>, end: &Vec3<f32>| {
@@ -720,10 +711,31 @@ where
             //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).
             // is_fluid();
         };
-        let mut node_index1: usize = 0;
+        informed_rrt_connect(start, end, is_traversable)
-        let mut node_index2: usize = 0;
+    }
+}
+
+/// Attempts to find a path from a start to the end using an informed
+/// RRT-Connect algorithm. A point is sampled from a bounding spheroid
+/// between the start and end. Two separate rapidly exploring random
+/// trees extend toward the sampled point. Nodes are stored in k-d trees
+/// for quicker nearest node calculations. Points are sampled until the
+/// trees connect. A final path is then reconstructed from the nodes.
+/// This pathfinding algorithm is more appropriate for 3D pathfinding
+/// with wider gaps, such as flying through a forest than for terrain
+/// with narrow gaps, such as navigating a maze.
+/// Returns a path and whether that path is complete or not.
+#[cfg(rrt_pathfinding)]
+fn informed_rrt_connect(
+    start: Vec3<f32>,
+    end: Vec3<f32>,
+    is_valid_edge: impl Fn(&Vec3<f32>, &Vec3<f32>) -> bool,
+) -> (Option<Path<Vec3<i32>>>, bool) {
+    let mut path = Vec::new();
 
     // Each tree has a vector of nodes
+    let mut node_index1: usize = 0;
+    let mut node_index2: usize = 0;
     let mut nodes1 = Vec::new();
     let mut nodes2 = Vec::new();
 
@@ -758,6 +770,8 @@ where
     let mut connection1_idx = 0;
     let mut connection2_idx = 0;
 
+    let mut connect = false;
+
     // Scalar non-dimensional value that is proportional to the size of the
     // sample spheroid volume. This increases in value until a path is found.
     let mut search_parameter = 0.01;
@@ -791,13 +805,11 @@ where
         let nearest2 = nodes2[nearest_index2];
 
         // Extend toward the sampled point from the nearest node of each tree
-            let new_point1 =
+        let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius);
-                nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius);
+        let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius);
-            let new_point2 =
-                nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius);
 
         // Ensure the new nodes are valid/traversable
-            if is_traversable(&nearest1, &new_point1) {
+        if is_valid_edge(&nearest1, &new_point1) {
             kdtree1
                 .add(&[new_point1.x, new_point1.y, new_point1.z], node_index1)
                 .unwrap_or_default();
@@ -821,7 +833,7 @@ where
         }
 
         // Repeat the validity check for the second tree
-            if is_traversable(&nearest2, &new_point2) {
+        if is_valid_edge(&nearest2, &new_point2) {
             kdtree2
                 .add(&[new_point2.x, new_point2.y, new_point1.z], node_index2)
                 .unwrap_or_default();
@@ -887,8 +899,7 @@ where
         }
         path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
         // Construct the path
-            while current_node_index1 != 0
+        while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0
-                && nodes1[current_node_index1].distance_squared(startf) > 4.0
         {
             current_node_index1 = *parents1.get(&current_node_index1).unwrap_or(&0);
             path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
@@ -929,8 +940,6 @@ where
     }
     path = new_path;
 }
-    (Some(path.into_iter().collect()), connect)
-}
 
 /// Returns a random point within a radially symmetrical ellipsoid with given
 /// foci and a `search parameter` to determine the size of the ellipse beyond