when scaled, find all paths then pick the one with the smaller length

src/instance.rs

@@ -63,7 +63,7 @@ pub(crate) struct SearchInstance<'m> {
     pub(crate) node_buffer: Vec<SearchNode>,
     pub(crate) root_history: HashMap<Root, f32>,
     #[cfg(feature = "detailed-layers")]
-    pub(crate) from: Vec2,
+    pub(crate) from: (Vec2, u8),
     pub(crate) to: Vec2,
     pub(crate) polygon_to: u32,
     pub(crate) mesh: &'m Mesh,
@@ -133,7 +133,7 @@ impl<'m> SearchInstance<'m> {
             node_buffer: Vec::with_capacity(10),
             root_history: HashMap::with_capacity(10),
             #[cfg(feature = "detailed-layers")]
-            from: from.0,
+            from: (from.0, from.1.layer()),
             to: to.0,
             polygon_to: to.1,
             mesh,
@@ -275,7 +275,7 @@ impl<'m> SearchInstance<'m> {
                 #[cfg(feature = "detailed-layers")]
                 let path_with_layers = {
                     let mut path_with_layers = vec![];
-                    let mut from = self.from;
+                    let mut from = self.from.0;
                     for (index, potential_point) in next.path_with_layers.iter().enumerate() {
                         if potential_point.0 == potential_point.1 {
                             from = potential_point.0;
@@ -311,9 +311,20 @@ impl<'m> SearchInstance<'m> {
                     }
                     path_with_layers
                 };
+
                 return InstanceStep::Found(Path {
                     path,
-                    length: next.distance_start_to_root + dbg!(next.heuristic),
+                    #[cfg(not(feature = "detailed-layers"))]
+                    length: next.distance_start_to_root + next.heuristic,
+                    #[cfg(feature = "detailed-layers")]
+                    length: {
+                        let a = path_with_layers.iter().fold((0.0, self.from), |acc, p| {
+                            let scale = self.mesh.layers[acc.1 .1 as usize].scale;
+                            let to_point = (acc.1 .0 * scale).distance(p.0 * scale);
+                            (acc.0 + to_point, *p)
+                        });
+                        a.0
+                    },
                     #[cfg(feature = "detailed-layers")]
                     path_with_layers,
                 });
@@ -556,8 +567,7 @@ impl<'m> SearchInstance<'m> {
         {
             heuristic_to_end = heuristic(
                 root,
-                // self.to, //* self.mesh.layers[other_side.layer() as usize].scale,
-                self.to, //* scale_of_path,
+                self.to,
                 (
                     start.0 * self.mesh.layers[start.1.layer() as usize].scale,
                     end.0 * self.mesh.layers[end.1.layer() as usize].scale,
@@ -811,7 +821,7 @@ impl<'m> SearchInstance<'m> {
                     (successor.interval.0, successor.edge[0]),
                     (successor.interval.1, successor.edge[1]),
                     &node,
-                )
+                );
             }
 
             if self.node_buffer.len() == 1 && self.node_buffer[0].polygon_to != self.polygon_to {

src/lib.rs

@@ -271,17 +271,36 @@ impl Mesh {
             start,
         );
 
+        let mut paths: Vec<Path> = vec![];
         // Limit search to avoid an infinite loop.
         for _ in 0..self.layers.iter().map(|l| l.polygons.len()).sum::<usize>() * 10 {
-            match search_instance.next() {
+            let potential_path = match search_instance.next() {
+                #[cfg(not(feature = "detailed-layers"))]
                 InstanceStep::Found(path) => return Some(path),
-                InstanceStep::NotFound => return None,
-                InstanceStep::Continue => (),
+                #[cfg(feature = "detailed-layers")]
+                InstanceStep::Found(path) => Some(path),
+                InstanceStep::NotFound => {
+                    if paths.is_empty() {
+                        None
+                    } else {
+                        Some(paths.remove(0))
+                    }
+                }
+                InstanceStep::Continue => None,
+            };
+            #[cfg(feature = "detailed-layers")]
+            if let Some(path) = potential_path {
+                paths.push(path);
             }
         }
-
-        error!("Search from {from} to {to} failed. Please check the mesh is valid as this should not happen.");
-        None
+        #[cfg(feature = "detailed-layers")]
+        paths.sort_by(|p1, p2| p1.length.partial_cmp(&p2.length).unwrap());
+        if paths.is_empty() {
+            error!("Search from {from} to {to} failed. Please check the mesh is valid as this should not happen.");
+            None
+        } else {
+            Some(paths.remove(0))
+        }
     }
 
     /// The delta set by [`Mesh::set_delta`]
@@ -312,7 +331,7 @@ impl Mesh {
             node_buffer: Vec::new(),
             root_history: HashMap::new(),
             #[cfg(feature = "detailed-layers")]
-            from: node.root,
+            from: (node.root, 0),
             to,
             polygon_to: self.get_point_location(to),
             mesh: self,
@@ -350,7 +369,7 @@ impl Mesh {
             node_buffer: Vec::new(),
             root_history: HashMap::new(),
             #[cfg(feature = "detailed-layers")]
-            from: Vec2::ZERO,
+            from: (Vec2::ZERO, 0),
             to: Vec2::ZERO,
             polygon_to: self.get_point_location(vec2(0.0, 0.0)),
             mesh: self,