1 use crate::{

     2     collision::{fppoint_round, CircleBounds, DetectedCollisions},

     3     common::GearId,

     4 };

     5 

     6 use fpnum::FPPoint;

     7 use integral_geometry::{GridIndex, Point, Size};

     8 

     9 struct GridBin {

    10     static_refs: Vec<GearId>,

    11     static_entries: Vec<CircleBounds>,

    12 

    13     dynamic_refs: Vec<GearId>,

    14     dynamic_entries: Vec<CircleBounds>,

    15 }

    16 

    17 impl GridBin {

    18     fn new() -> Self {

    19         Self {

    20             static_refs: vec![],

    21             static_entries: vec![],

    22             dynamic_refs: vec![],

    23             dynamic_entries: vec![],

    24         }

    25     }

    26 }

    27 

    28 const GRID_BIN_SIZE: usize = 128;

    29 

    30 pub struct Grid {

    31     bins: Vec<GridBin>,

    32     space_size: Size,

    33     bins_count: Size,

    34     index: GridIndex,

    35 }

    36 

    37 impl Grid {

    38     pub fn new(size: Size) -> Self {

    39         assert!(size.is_power_of_two());

    40         let bins_count = Size::new(size.width / GRID_BIN_SIZE, size.height / GRID_BIN_SIZE);

    41 

    42         Self {

    43             bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(),

    44             space_size: size,

    45             bins_count,

    46             index: Size::square(GRID_BIN_SIZE).to_grid_index(),

    47         }

    48     }

    49 

    50     fn bin_index(&self, position: &FPPoint) -> Point {

    51         self.index.map(fppoint_round(position))

    52     }

    53 

    54     fn get_bin(&mut self, index: Point) -> &mut GridBin {

    55         &mut self.bins[index.x as usize * self.bins_count.width + index.y as usize]

    56     }

    57 

    58     fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin {

    59         self.get_bin(self.bin_index(position))

    60     }

    61 

    62     pub fn insert_static(&mut self, gear_id: GearId, bounds: &CircleBounds) {

    63         let bin = self.lookup_bin(&bounds.center);

    64         bin.static_refs.push(gear_id);

    65         bin.static_entries.push(*bounds)

    66     }

    67 

    68     pub fn insert_dynamic(&mut self, gear_id: GearId, bounds: &CircleBounds) {

    69         let bin = self.lookup_bin(&bounds.center);

    70         bin.dynamic_refs.push(gear_id);

    71         bin.dynamic_entries.push(*bounds);

    72     }

    73 

    74     pub fn remove(&mut self, gear_id: GearId) {}

    75 

    76     pub fn update_position(

    77         &mut self,

    78         gear_id: GearId,

    79         old_position: &FPPoint,

    80         new_position: &FPPoint,

    81     ) {

    82         let old_bin_index = self.bin_index(old_position);

    83         let new_bin_index = self.bin_index(new_position);

    84 

    85         let old_bin = self.lookup_bin(old_position);

    86         if let Some(index) = old_bin.dynamic_refs.iter().position(|id| *id == gear_id) {

    87             if old_bin_index == new_bin_index {

    88                 old_bin.dynamic_entries[index].center = *new_position

    89             } else {

    90                 let bounds = old_bin.dynamic_entries.swap_remove(index);

    91                 let new_bin = self.get_bin(new_bin_index);

    92 

    93                 new_bin.dynamic_refs.push(gear_id);

    94                 new_bin.dynamic_entries.push(CircleBounds {

    95                     center: *new_position,

    96                     ..bounds

    97                 });

    98             }

    99         } else if let Some(index) = old_bin.static_refs.iter().position(|id| *id == gear_id) {

   100             let bounds = old_bin.static_entries.swap_remove(index);

   101             old_bin.static_refs.swap_remove(index);

   102 

   103             let new_bin = if old_bin_index == new_bin_index {

   104                 old_bin

   105             } else {

   106                 self.get_bin(new_bin_index)

   107             };

   108 

   109             new_bin.dynamic_refs.push(gear_id);

   110             new_bin.dynamic_entries.push(CircleBounds {

   111                 center: *new_position,

   112                 ..bounds

   113             });

   114         }

   115     }

   116 

   117     pub fn check_collisions(&self, collisions: &mut DetectedCollisions) {

   118         for bin in &self.bins {

   119             for (index, bounds) in bin.dynamic_entries.iter().enumerate() {

   120                 for (other_index, other) in bin.dynamic_entries.iter().enumerate().skip(index + 1) {

   121                     if bounds.intersects(other) && bounds != other {

   122                         collisions.push(

   123                             bin.dynamic_refs[index],

   124                             Some(bin.dynamic_refs[other_index]),

   125                             &((bounds.center + other.center) / 2),

   126                         )

   127                     }

   128                 }

   129 

   130                 for (other_index, other) in bin.static_entries.iter().enumerate() {

   131                     if bounds.intersects(other) {

   132                         collisions.push(

   133                             bin.dynamic_refs[index],

   134                             Some(bin.static_refs[other_index]),

   135                             &((bounds.center + other.center) / 2),

   136                         )

   137                     }

   138                 }

   139             }

   140         }

   141     }

   142 }