1 use fpnum::*;

     2 use integral_geometry::{

     3     Point, Size, GridIndex

     4 };

     5 

     6 type Index = u16;

     7 

     8 #[derive(PartialEq, Eq, Clone, Copy, Debug)]

     9 struct PhysicsData {

    10     position: FPPoint,

    11     velocity: FPPoint,

    12 }

    13 

    14 #[derive(PartialEq, Eq, Clone, Copy, Debug)]

    15 struct CollisionData {

    16     bounds: CircleBounds

    17 }

    18 

    19 #[derive(PartialEq, Eq, Clone, Copy, Debug)]

    20 struct ContactData {

    21     elasticity: FPNum,

    22     friction: FPNum

    23 }

    24 

    25 pub struct PhysicsCollection {

    26     positions: Vec<FPPoint>,

    27     velocities: Vec<FPPoint>

    28 }

    29 

    30 impl PhysicsCollection {

    31     fn push(&mut self, data: PhysicsData) {

    32         self.positions.push(data.position);

    33         self.velocities.push(data.velocity);

    34     }

    35 

    36     fn iter_mut_pos(&mut self) -> impl Iterator<Item = (&mut FPPoint, &FPPoint)> {

    37         self.positions.iter_mut().zip(self.velocities.iter())

    38     }

    39 }

    40 

    41 pub struct JoinedData {

    42     physics: PhysicsData,

    43     collision: CollisionData,

    44     contact: ContactData

    45 }

    46 

    47 pub struct World {

    48     enabled_physics: PhysicsCollection,

    49     disabled_physics: Vec<PhysicsData>,

    50 

    51     collision: Vec<CollisionData>,

    52     grid: Grid,

    53 

    54     physics_cleanup: Vec<PhysicsData>,

    55     collision_output: Vec<(Index, Index)>

    56 }

    57 

    58 #[derive(PartialEq, Eq, Clone, Copy, Debug)]

    59 struct CircleBounds {

    60     center: FPPoint,

    61     radius: FPNum

    62 }

    63 

    64 impl CircleBounds {

    65     fn intersects(&self, other: &CircleBounds) -> bool {

    66         (other.center - self.center).is_in_range(self.radius + other.radius)

    67     }

    68 }

    69 

    70 fn fppoint_round(point: &FPPoint) -> Point {

    71     Point::new(point.x().round() as i32, point.y().round() as i32)

    72 }

    73 

    74 struct GridBin {

    75     refs: Vec<Index>,

    76     static_entries: Vec<CircleBounds>,

    77     dynamic_entries: Vec<CircleBounds>

    78 }

    79 

    80 impl GridBin {

    81     fn new() -> Self {

    82         Self {

    83             refs: vec![],

    84             static_entries: vec![],

    85             dynamic_entries: vec![]

    86         }

    87     }

    88 }

    89 

    90 const GRID_BIN_SIZE: usize = 256;

    91 

    92 struct Grid {

    93     bins: Vec<GridBin>,

    94     space_size: Size,

    95     bins_count: Size,

    96     index: GridIndex

    97 }

    98 

    99 impl Grid {

   100     fn new(size: Size) -> Self {

   101         assert!(size.is_power_of_two());

   102         let bins_count =

   103             Size::new(size.width / GRID_BIN_SIZE,

   104                       size.height / GRID_BIN_SIZE);

   105 

   106         Self {

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

   108             space_size: size,

   109             bins_count,

   110             index: Size::square(GRID_BIN_SIZE).to_grid_index()

   111         }

   112     }

   113 

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

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

   116     }

   117 

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

   119         let index = self.bin_index(position);

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

   121     }

   122 

   123     fn insert_static(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {

   124         self.lookup_bin(position).static_entries.push(*bounds)

   125     }

   126 

   127     fn insert_dynamic(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {

   128         self.lookup_bin(position).dynamic_entries.push(*bounds)

   129     }

   130 

   131     fn check_collisions(&self, collisions: &mut Vec<(Index, Index)>) {

   132         for bin in &self.bins {

   133             for bounds in &bin.dynamic_entries {

   134                 for other in &bin.dynamic_entries {

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

   136                         collisions.push((0, 0))

   137                     }

   138                 }

   139 

   140                 for other in &bin.static_entries {

   141                     if bounds.intersects(other) {

   142                         collisions.push((0, 0))

   143                     }

   144                 }

   145             }

   146         }

   147     }

   148 }

   149 

   150 impl World {

   151     pub fn step(&mut self, time_step: FPNum) {

   152         for (pos, vel) in self.enabled_physics.iter_mut_pos() {

   153             *pos += *vel

   154         }

   155 

   156         self.grid.check_collisions(&mut self.collision_output);

   157     }

   158 

   159     pub fn add_gear(&mut self, data: JoinedData) {

   160         if data.physics.velocity == FPPoint::zero() {

   161             self.disabled_physics.push(data.physics);

   162             self.grid.insert_static(0, &data.physics.position, &data.collision.bounds);

   163         } else {

   164             self.enabled_physics.push(data.physics);

   165             self.grid.insert_dynamic(0, &data.physics.position, &data.collision.bounds);

   166         }

   167     }

   168 }

   169 

   170 #[cfg(test)]

   171 mod tests {

   172 

   173 }