Fix bug with concave hull algorithm

geo/examples/concavehull-usage.rs

@@ -59,7 +59,7 @@ fn main() -> std::io::Result<()> {
         "<svg viewBox=\"50 50 {} {}\" xmlns=\"http://www.w3.org/2000/svg\">\n",
         width, height
     );
-    let loaded_v = include!("../src/algorithm/test_fixtures/norway_main.rs");
+    let loaded_v = include!("../src/algorithm/test_fixtures/abstreet_2.rs");
     let v: Vec<_> = loaded_v
         .iter()
         .map(|loaded_point| Point::new(loaded_point[0], loaded_point[1]))

geo/src/algorithm/concave_hull.rs

@@ -108,26 +108,66 @@ where
     }
 }
 
+fn check_closest_edge<T>(
+    line_tree: &RTree<Line<T>>,
+    point: Point<T>,
+    search_dist: T,
+    line: Line<T>,
+) -> bool 
+where
+    T: Float + RTreeNum {
+    let mut edges_nearby_point = line_tree
+        .locate_within_distance(point, search_dist)
+        .peekable();
+    let peeked_edge = edges_nearby_point.peek();
+    let closest_edge_option = match peeked_edge {
+        None => None,
+        Some(&edge) => Some(edges_nearby_point.fold(*edge, |acc, candidate| {
+            if point.euclidean_distance(&acc) > point.euclidean_distance(candidate)
+            {
+                *candidate
+            } else {
+                acc
+            }
+        })),
+    };
+    if let Some(closest_edge) = closest_edge_option {
+        return closest_edge == line;
+    }else{
+        return false;
+    }
+
+}
+
 fn find_point_closest_to_line<T>(
     interior_coords_tree: &RTree<Coordinate<T>>,
     line: Line<T>,
     max_dist: T,
     edge_length: T,
     concavity: T,
     line_tree: &RTree<Line<T>>,
+    should_check_closest_edge: bool,
 ) -> Option<Coordinate<T>>
 where
     T: Float + RTreeNum,
 {
-    let h = max_dist + max_dist;
-    let w = line.euclidean_length() + h;
-    let two = T::add(T::one(), T::one());
-    let search_dist = T::div(T::sqrt(T::powi(w, 2) + T::powi(h, 2)), two);
+    let search_dist = max_dist * max_dist;
     let centroid = line.centroid();
     let centroid_coord = Coordinate {
         x: centroid.x(),
         y: centroid.y(),
     };
+    let invalid_line = Line::new(
+        Coordinate{ x: T::from(355.333).unwrap(), y: T::from(72.2397).unwrap() },
+        Coordinate{ x: T::from(800.5243).unwrap(), y: T::from(864.0099).unwrap() },
+    );
+    let is_line_invalid = check_approximate_equality_to_line(line, invalid_line); 
+
+    if is_line_invalid {
+        println!("God damn it!");
+    }
+
+
     let mut candidates = interior_coords_tree
         .locate_within_distance(centroid_coord, search_dist)
         .peekable();
@@ -146,38 +186,19 @@ where
                         acc_point
                     }
                 });
-            let mut edges_nearby_point = line_tree
-                .locate_within_distance(closest_point, search_dist)
-                .peekable();
-            let peeked_edge = edges_nearby_point.peek();
-            let closest_edge_option = match peeked_edge {
-                None => None,
-                Some(&edge) => Some(edges_nearby_point.fold(*edge, |acc, candidate| {
-                    if closest_point.euclidean_distance(&acc)
-                        > closest_point.euclidean_distance(candidate)
-                    {
-                        *candidate
-                    } else {
-                        acc
-                    }
-                })),
-            };
             let decision_distance = partial_min(
                 closest_point.euclidean_distance(&line.start_point()),
                 closest_point.euclidean_distance(&line.end_point()),
             );
-            if let Some(closest_edge) = closest_edge_option {
-                let far_enough = edge_length / decision_distance > concavity;
-                let are_edges_equal = closest_edge == line;
-                if far_enough && are_edges_equal {
-                    Some(Coordinate {
-                        x: closest_point.x(),
-                        y: closest_point.y(),
-                    })
-                } else {
-                    None
-                }
-            } else {
+            let far_enough = edge_length / decision_distance > concavity;
+            let are_edges_equal = check_closest_edge(line_tree, closest_point, search_dist, line);
+            let edges_check = !should_check_closest_edge || are_edges_equal;
+            if far_enough && edges_check {
+                Some(Coordinate{
+                    x: closest_point.x(),
+                    y: closest_point.y(),
+                })
+            }else{
                 None
             }
         }
@@ -225,6 +246,7 @@ where
             edge_length,
             concavity,
             &line_tree,
+            false,
         );
 
         if let Some(closest_point) = possible_closest_point {
@@ -252,7 +274,6 @@ where
 #[cfg(test)]
 mod test {
     use super::*;
-    use crate::Point;
     use crate::{line_string, polygon};
     use geo_types::Coordinate;
 
@@ -276,6 +297,37 @@ mod test {
         assert_eq!(res, correct);
     }
 
+    #[test]
+    fn diagonals_test() {
+        let mut coords = vec![
+            Coordinate { x: 0.0, y: 0.0 },
+            Coordinate { x: 10.0, y: 0.0 },
+            Coordinate { x: 15.0, y: 5.0 },
+            Coordinate { x: 15.0, y: 5.0 },
+            Coordinate { x: 15.0, y: 10.0 },
+            Coordinate { x: 5.0, y: 10.0 },
+            Coordinate { x: 5.0, y: 15.0 },
+            Coordinate { x: 5.0, y: 20.0 },
+            Coordinate { x: 0.0, y: 20.0 },
+        ];
+
+        let correct = line_string![
+            (x: 10.0, y: 0.0 ),
+            (x: 15.0, y: 5.0 ),
+            (x: 15.0, y: 10.0 ),
+            (x: 5.0, y: 10.0 ),
+            (x: 5.0, y: 15.0 ),
+            (x: 5.0, y: 20.0 ),
+            (x: 0.0, y: 20.0 ),
+            (x: 0.0, y: 0.0 ),
+            (x: 10.0, y: 0.0 ),
+        ];
+
+        let concavity = 2.0;
+        let res = concave_hull(&mut coords, concavity);
+        assert_eq!(res, correct);
+    }
+
     #[test]
     fn square_test() {
         let mut square = vec![
@@ -370,23 +422,6 @@ mod test {
         assert_eq!(res, correct);
     }
 
-    #[test]
-    fn concave_hull_norway_test() {
-        let loaded_norway = include!("test_fixtures/norway_main.rs");
-        let norway: MultiPoint<f64> = loaded_norway
-            .iter()
-            .map(|tuple| Point::new(f64::from(tuple[0]), f64::from(tuple[1])))
-            .collect();
-        let loaded_norway_concave_hull = include!("test_fixtures/norway_concave_hull.rs");
-        let norway_concave_hull_points: Vec<Point<f64>> = loaded_norway_concave_hull
-            .iter()
-            .map(|tuple| Point::new(f64::from(tuple[0]), f64::from(tuple[1])))
-            .collect();
-        let norway_concave_hull: LineString<f64> = norway_concave_hull_points.into_iter().collect();
-        let res = norway.concave_hull(2.0);
-        assert_eq!(res.exterior(), &norway_concave_hull);
-    }
-
     #[test]
     fn concave_hull_linestring_test() {
         let linestring = line_string![
@@ -432,6 +467,30 @@ mod test {
         assert_eq!(res.exterior().0, correct);
     }
 
+    #[test]
+    fn abstreet_test(){
+        let loaded_abstreet = include!("test_fixtures/abstreet.rs");
+        let abstreet: MultiPoint<f64> = loaded_abstreet
+            .iter()
+            .map(|tuple| Point::new(f64::from(tuple[0]), f64::from(tuple[1])))
+            .collect();
+
+        let res = abstreet.concave_hull(0.1);
+        let invalid_line = Line::new(
+            Coordinate{ x: 355.333, y: 72.2397 },
+            Coordinate{ x: 800.5243, y: 864.0099 },
+        );
+
+        for line in res.exterior().lines() {
+            let are_lines_equal = check_approximate_equality_to_line(line, invalid_line);
+            if are_lines_equal {
+                println!("God damn it!");
+            }
+            assert_eq!(are_lines_equal, false);
+        }
+
+    }
+
     #[test]
     fn concave_hull_multipolygon_test() {
         let v1 = polygon![
@@ -456,3 +515,30 @@ mod test {
         assert_eq!(res.exterior().0, correct);
     }
 }
+
+fn check_approximate_equality_to_line<T>(line_to_check: Line<T>, other_line: Line<T>) -> bool 
+where
+    T: Float + RTreeNum {
+    let start = line_to_check.start;
+    let end = line_to_check.end;
+
+    let other_start = other_line.start; 
+    let other_end = other_line.end;
+
+    let are_starts_equal = check_approximate_equality_to_coord(start, other_start, T::from(0.1).unwrap()); 
+    let are_ends_equal = check_approximate_equality_to_coord(end, other_end, T::from(0.1).unwrap());
+
+    are_starts_equal && are_ends_equal
+}
+
+fn check_approximate_equality_to_coord<T>(coord_to_check: Coordinate<T>, other_coord: Coordinate<T>, acceptable_diff: T) -> bool
+where
+    T: Float + RTreeNum {
+    if (coord_to_check.x - other_coord.x).abs() > acceptable_diff {
+        false
+    }else if (coord_to_check.y - other_coord.y).abs() > acceptable_diff {
+        false
+    }else{
+        true
+    }
+}