rust/hwphysics/src/lib.rs
changeset 14080 c6745a1c827a
child 14165 165e43c3ed59
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/lib.rs	Thu Nov 01 21:30:21 2018 +0300
@@ -0,0 +1,173 @@
+use fpnum::*;
+use integral_geometry::{
+    Point, Size, GridIndex
+};
+
+type Index = u16;
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+struct PhysicsData {
+    position: FPPoint,
+    velocity: FPPoint,
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+struct CollisionData {
+    bounds: CircleBounds
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+struct ContactData {
+    elasticity: FPNum,
+    friction: FPNum
+}
+
+pub struct PhysicsCollection {
+    positions: Vec<FPPoint>,
+    velocities: Vec<FPPoint>
+}
+
+impl PhysicsCollection {
+    fn push(&mut self, data: PhysicsData) {
+        self.positions.push(data.position);
+        self.velocities.push(data.velocity);
+    }
+
+    fn iter_mut_pos(&mut self) -> impl Iterator<Item = (&mut FPPoint, &FPPoint)> {
+        self.positions.iter_mut().zip(self.velocities.iter())
+    }
+}
+
+pub struct JoinedData {
+    physics: PhysicsData,
+    collision: CollisionData,
+    contact: ContactData
+}
+
+pub struct World {
+    enabled_physics: PhysicsCollection,
+    disabled_physics: Vec<PhysicsData>,
+
+    collision: Vec<CollisionData>,
+    grid: Grid,
+
+    physics_cleanup: Vec<PhysicsData>,
+    collision_output: Vec<(Index, Index)>
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+struct CircleBounds {
+    center: FPPoint,
+    radius: FPNum
+}
+
+impl CircleBounds {
+    fn intersects(&self, other: &CircleBounds) -> bool {
+        (other.center - self.center).is_in_range(self.radius + other.radius)
+    }
+}
+
+fn fppoint_round(point: &FPPoint) -> Point {
+    Point::new(point.x().round() as i32, point.y().round() as i32)
+}
+
+struct GridBin {
+    refs: Vec<Index>,
+    static_entries: Vec<CircleBounds>,
+    dynamic_entries: Vec<CircleBounds>
+}
+
+impl GridBin {
+    fn new() -> Self {
+        Self {
+            refs: vec![],
+            static_entries: vec![],
+            dynamic_entries: vec![]
+        }
+    }
+}
+
+const GRID_BIN_SIZE: usize = 256;
+
+struct Grid {
+    bins: Vec<GridBin>,
+    space_size: Size,
+    bins_count: Size,
+    index: GridIndex
+}
+
+impl Grid {
+    fn new(size: Size) -> Self {
+        assert!(size.is_power_of_two());
+        let bins_count =
+            Size::new(size.width / GRID_BIN_SIZE,
+                      size.height / GRID_BIN_SIZE);
+
+        Self {
+            bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(),
+            space_size: size,
+            bins_count,
+            index: Size::square(GRID_BIN_SIZE).to_grid_index()
+        }
+    }
+
+    fn bin_index(&self, position: &FPPoint) -> Point {
+        self.index.map(fppoint_round(position))
+    }
+
+    fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin {
+        let index = self.bin_index(position);
+        &mut self.bins[index.x as usize * self.bins_count.width + index.y as usize]
+    }
+
+    fn insert_static(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {
+        self.lookup_bin(position).static_entries.push(*bounds)
+    }
+
+    fn insert_dynamic(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {
+        self.lookup_bin(position).dynamic_entries.push(*bounds)
+    }
+
+    fn check_collisions(&self, collisions: &mut Vec<(Index, Index)>) {
+        for bin in &self.bins {
+            for bounds in &bin.dynamic_entries {
+                for other in &bin.dynamic_entries {
+                    if bounds.intersects(other) && bounds != other {
+                        collisions.push((0, 0))
+                    }
+                }
+
+                for other in &bin.static_entries {
+                    if bounds.intersects(other) {
+                        collisions.push((0, 0))
+                    }
+                }
+            }
+        }
+    }
+}
+
+impl World {
+    pub fn step(&mut self, time_step: FPNum) {
+        for (pos, vel) in self.enabled_physics.iter_mut_pos() {
+            *pos += *vel
+        }
+
+        self.grid.check_collisions(&mut self.collision_output);
+    }
+
+    pub fn add_gear(&mut self, data: JoinedData) {
+        if data.physics.velocity == FPPoint::zero() {
+            self.disabled_physics.push(data.physics);
+            self.grid.insert_static(0, &data.physics.position, &data.collision.bounds);
+        } else {
+            self.enabled_physics.push(data.physics);
+            self.grid.insert_dynamic(0, &data.physics.position, &data.collision.bounds);
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+}