feat(spacial_indec): Seems to be working nicely at this point, next step use it and see how fast we got

src/space_colonization/spacial_index.rs

@@ -3,11 +3,18 @@ use super::Point;
 #[derive(Debug)]
 struct SpatialIndex<T> {
     elements: Vec<Vec<T>>,
+    /// The number of cells between a cell and the cell right below it.
+    ///
+    /// For a table of 100x100 with cell_size = 10, number_cells_x is 11 (100/10+1)
     number_cells_x: i32,
     cell_size: i32,
+    max_point: Point,
 }
 
-impl<T> SpatialIndex<T> {
+impl<T> SpatialIndex<T>
+where
+    T: std::fmt::Debug,
+{
     pub fn new(max_point: Point, cell_size: i32) -> SpatialIndex<T> {
         // Here we add 1 to number cells x and y to handle the right and bottom edges where
         // x = max_point.x or y = max_point.y
@@ -25,30 +32,84 @@ impl<T> SpatialIndex<T> {
             elements,
             cell_size,
             number_cells_x,
+            max_point,
         }
     }
 
     /// Adds an element in the cell of point
     ///
-    /// panics if the point is outside of
+    /// panics if the point is outside of the grid
     pub fn add(&mut self, point: &Point, element: T) {
         let element_index = self.get_index_from_position(point);
-        dbg!(element_index);
         self.elements
             .get_mut(element_index as usize)
             .unwrap()
             .push(element);
     }
 
-    pub fn get_surrounding_elements<'a>(&'a self, point: &Point) -> Vec<&'a Vec<T>> {
-        let element_index = self.get_index_from_position(point);
-        dbg!(element_index);
-        vec![&self.elements[element_index]]
+    pub fn get_surrounding_elements<'a>(&'a self, point: &Point) -> Vec<&'a T> {
+        let surrounding_indices: Vec<usize> = self.get_surrounding_indices(point);
+        let mut surrounding_elements = Vec::new();
+        dbg!(&surrounding_indices);
+        for i in surrounding_indices {
+            surrounding_elements.extend(self.elements.get(i).unwrap().iter());
+        }
+        dbg!(&self.elements[115]);
+        dbg!(&surrounding_elements);
+        dbg!(&self.elements.len());
+        surrounding_elements
     }
 
     fn get_index_from_position(&self, point: &Point) -> usize {
         ((point.x / self.cell_size) + ((point.y / self.cell_size) * self.number_cells_x)) as usize
     }
+
+    fn get_surrounding_indices(&self, point: &Point) -> Vec<usize> {
+        let element_index = self.get_index_from_position(point);
+        let mut indices = Vec::from([element_index]);
+        let row_offset = self.number_cells_x as usize;
+
+        // top row
+        if point.y >= self.cell_size {
+            // top left
+            if point.x >= self.cell_size {
+                indices.push(element_index - row_offset - 1);
+            }
+            // top middle
+            indices.push(element_index - row_offset);
+            // top right
+            if point.x < self.max_point.x {
+                indices.push(element_index - row_offset + 1);
+            }
+        }
+
+        // middle left
+        if point.x >= self.cell_size {
+            indices.push(element_index - 1);
+        }
+        // middle middle can be skipped, already added
+
+        // middle right
+        if point.x < self.max_point.x {
+            indices.push(element_index + 1);
+        }
+
+        if point.y < self.max_point.y {
+            // bottom left
+            if point.x >= self.cell_size {
+                indices.push(element_index + row_offset - 1);
+            }
+
+            // bottom middle
+            indices.push(element_index + row_offset);
+            // bottom right
+            if point.x < self.max_point.x {
+                indices.push(element_index + row_offset + 1);
+            }
+        }
+
+        indices
+    }
 }
 
 #[cfg(test)]
@@ -60,7 +121,7 @@ mod test {
         let index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
         assert_eq!(
             index.get_surrounding_elements(&Point::new((0, 0))),
-            vec![&Vec::<usize>::new()]
+            Vec::<&usize>::new()
         );
     }
 
@@ -70,32 +131,87 @@ mod test {
         index.add(&Point::new((50, 50)), 132);
         assert_eq!(
             index.get_surrounding_elements(&Point::new((50, 50))),
-            vec![&vec![132]]
+            vec![&132]
         );
     }
 
     #[test]
-    fn adding_point_outside_grid_panics() {
-        todo!();
+    fn multiple_points_in_every_surrounding_cell() {
+        let mut index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
+        index.add(&Point::new((40, 40)), 4040);
+        index.add(&Point::new((45, 45)), 4040);
+        index.add(&Point::new((50, 40)), 5040);
+        index.add(&Point::new((60, 40)), 6040);
+        index.add(&Point::new((40, 50)), 4050);
+        index.add(&Point::new((50, 50)), 5050);
+        index.add(&Point::new((60, 50)), 6050);
+        index.add(&Point::new((50, 50)), 5050);
+        index.add(&Point::new((51, 51)), 5151);
+        index.add(&Point::new((60, 60)), 6060);
+        index.add(&Point::new((40, 60)), 4060);
+        index.add(&Point::new((50, 60)), 5060);
+        index.add(&Point::new((60, 60)), 6060);
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((50, 50))).sort(),
+            vec![
+                &4040, &4545, &5040, &6040, &4050, &5050, &6050, &5050, &5151, &6060, &4060, &5060, &6060,
+            ].sort()
+        );
     }
 
     #[test]
     fn point_on_top_edge_is_close_to_first_cell() {
-        todo!();
+        let mut index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
+        index.add(&Point::new((50, 0)), 132);
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((50, 9))),
+            vec![&132]
+        );
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((50, 0))),
+            vec![&132]
+        );
     }
 
     #[test]
-    fn point_on_bottom_edge_is_close_to_first_cell() {
-        todo!();
+    fn point_on_bottom_edge_is_close_to_bottom_cell() {
+        let mut index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
+        index.add(&Point::new((50, 100)), 132);
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((50, 95))),
+            vec![&132]
+        );
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((50, 100))),
+            vec![&132]
+        );
     }
 
     #[test]
     fn point_on_right_edge_is_close_to_first_cell() {
-        todo!();
+        let mut index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
+        index.add(&Point::new((100, 50)), 132);
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((95, 50))),
+            vec![&132]
+        );
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((100, 50))),
+            vec![&132]
+        );
     }
     #[test]
     fn point_on_left_edge_is_close_to_first_cell() {
-        todo!();
+        let mut index: SpatialIndex<usize> = SpatialIndex::new(Point::new((100, 100)), 10);
+        index.add(&Point::new((0, 50)), 132);
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((9, 50))),
+            vec![&132]
+        );
+        assert_eq!(
+            index.get_surrounding_elements(&Point::new((0, 50))),
+            vec![&132]
+        );
     }
 
     #[test]
@@ -104,7 +220,7 @@ mod test {
         index.add(&Point::new((0, 0)), 132);
         assert_eq!(
             index.get_surrounding_elements(&Point::new((99, 99))),
-            vec![&Vec::<usize>::new()]
+            Vec::<&usize>::new()
         );
     }
 }