--- a/rust/fpnum/src/lib.rs	Thu Nov 08 22:00:43 2018 +0100
+++ b/rust/fpnum/src/lib.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -364,6 +364,11 @@
     }
 
     #[inline]
+    pub fn is_zero(&self) -> bool {
+        self.x().is_zero() && self.y().is_zero()
+    }
+
+    #[inline]
     pub fn max_norm(&self) -> FPNum {
         std::cmp::max(self.x().abs(), self.y().abs())
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/collision.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -0,0 +1,111 @@
+use std::{
+    ops::RangeInclusive
+};
+
+use crate::{
+    common::GearId,
+    physics::PhysicsData,
+    grid::Grid
+};
+
+use fpnum::*;
+use integral_geometry::{
+    Point, Size, GridIndex
+};
+use land2d::Land2D;
+
+pub fn fppoint_round(point: &FPPoint) -> Point {
+    Point::new(point.x().round() as i32, point.y().round() as i32)
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub struct CircleBounds {
+    pub center: FPPoint,
+    pub radius: FPNum
+}
+
+impl CircleBounds {
+    pub fn intersects(&self, other: &CircleBounds) -> bool {
+        (other.center - self.center).is_in_range(self.radius + other.radius)
+    }
+
+    pub fn rows(&self) -> impl Iterator<Item = (usize, RangeInclusive<usize>)> {
+        let radius = self.radius.abs_round() as usize;
+        let center = Point::from_fppoint(&self.center);
+        (center.y as usize - radius..=center.y as usize + radius)
+            .map(move |row| (row, center.x as usize - radius..=center.x as usize + radius))
+    }
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub struct CollisionData {
+    pub bounds: CircleBounds
+}
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub struct ContactData {
+    pub elasticity: FPNum,
+    pub friction: FPNum
+}
+
+struct EnabledCollisionsCollection {
+    gear_ids: Vec<GearId>,
+    collisions: Vec<CollisionData>
+}
+
+impl EnabledCollisionsCollection {
+    fn push(&mut self, gear_id: GearId, collision: CollisionData) {
+        self.gear_ids.push(gear_id);
+        self.collisions.push(collision);
+    }
+
+    fn iter(&self) -> impl Iterator<Item = (GearId, &CollisionData)> {
+        self.gear_ids.iter().cloned().zip(self.collisions.iter())
+    }
+}
+
+pub struct CollisionProcessor {
+    grid: Grid,
+    enabled_collisions: EnabledCollisionsCollection,
+
+    detected_collisions: DetectedCollisions,
+}
+
+pub struct DetectedCollisions {
+    pub pairs: Vec<(GearId, GearId)>,
+    pub positions: Vec<Point>
+}
+
+impl DetectedCollisions {
+    pub fn new(capacity: usize) -> Self {
+        Self {
+            pairs: Vec::with_capacity(capacity),
+            positions: Vec::with_capacity(capacity),
+        }
+    }
+
+    pub fn push(&mut self, contact_gear_id1: GearId, contact_gear_id2: GearId, position: &FPPoint) {
+        self.pairs.push((contact_gear_id1, contact_gear_id2));
+        self.positions.push(fppoint_round(position));
+    }
+}
+
+impl CollisionProcessor {
+    pub fn process(&mut self, land: &Land2D<u32>, updates: &crate::physics::PositionUpdate) {
+        self.grid.check_collisions(&mut self.detected_collisions);
+
+        for (gear_id, collision) in self.enabled_collisions.iter() {
+            if collision.bounds.rows().any(|(y, r)| (&land[y][r]).iter().any(|v| *v != 0)) {
+                self.detected_collisions.push(0, 0, &collision.bounds.center)
+            }
+        }
+    }
+
+    pub fn push(&mut self, gear_id: GearId, physics_data: PhysicsData, collision_data: CollisionData) {
+        if physics_data.velocity.is_zero() {
+            self.grid.insert_static(0, &physics_data.position, &collision_data.bounds);
+        } else {
+            self.grid.insert_dynamic(0, &physics_data.position, &collision_data.bounds);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/common.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -0,0 +1,1 @@
+pub type GearId = u16;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/grid.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -0,0 +1,91 @@
+use crate::{
+    common::GearId,
+    collision::{
+        fppoint_round,
+        CircleBounds,
+        DetectedCollisions
+    }
+};
+
+use integral_geometry::{
+    Point,
+    Size,
+    GridIndex
+};
+use fpnum::FPPoint;
+
+struct GridBin {
+    refs: Vec<GearId>,
+    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;
+
+pub struct Grid {
+    bins: Vec<GridBin>,
+    space_size: Size,
+    bins_count: Size,
+    index: GridIndex
+}
+
+impl Grid {
+    pub 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]
+    }
+
+    pub fn insert_static(&mut self, gear_id: GearId, position: &FPPoint, bounds: &CircleBounds) {
+        self.lookup_bin(position).static_entries.push(*bounds)
+    }
+
+    pub fn insert_dynamic(&mut self, gear_id: GearId, position: &FPPoint, bounds: &CircleBounds) {
+        self.lookup_bin(position).dynamic_entries.push(*bounds)
+    }
+
+    pub fn check_collisions(&self, collisions: &mut DetectedCollisions) {
+        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, &bounds.center)
+                    }
+                }
+
+                for other in &bin.static_entries {
+                    if bounds.intersects(other) {
+                        collisions.push(0, 0, &bounds.center)
+                    }
+                }
+            }
+        }
+    }
+}
\ No newline at end of file

--- a/rust/hwphysics/src/lib.rs	Thu Nov 08 22:00:43 2018 +0100
+++ b/rust/hwphysics/src/lib.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -1,191 +1,45 @@
-use std::{
-    ops::RangeInclusive
-};
+mod common;
+mod physics;
+mod grid;
+mod collision;
 
-use fpnum::*;
-use integral_geometry::{
-    Point, Size, GridIndex
-};
+use fpnum::FPNum;
 use land2d::Land2D;
 
-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);
+use crate::{
+    common::GearId,
+    physics::{
+        PhysicsProcessor,
+        PhysicsData
+    },
+    collision::{
+        CollisionProcessor,
+        CollisionData,
+        ContactData
     }
-
-    fn iter_mut_pos(&mut self) -> impl Iterator<Item = (&mut FPPoint, &FPPoint)> {
-        self.positions.iter_mut().zip(self.velocities.iter())
-    }
-}
+};
 
 pub struct JoinedData {
+    gear_id: GearId,
     physics: PhysicsData,
     collision: CollisionData,
     contact: ContactData
 }
 
 pub struct World {
-    enabled_physics: PhysicsCollection,
-    disabled_physics: Vec<PhysicsData>,
-
-    enabled_collision: Vec<CollisionData>,
-    disabled_collision: Vec<CollisionData>,
-    grid: Grid,
-
-    physics_cleanup: Vec<PhysicsData>,
-    collision_output: Vec<(Index, Index)>,
-    land_collision_output: Vec<Index>
-}
-
-#[derive(PartialEq, Eq, Clone, Copy, Debug)]
-struct CircleBounds {
-    center: FPPoint,
-    radius: FPNum
-}
-
-impl CircleBounds {
-    pub fn intersects(&self, other: &CircleBounds) -> bool {
-        (other.center - self.center).is_in_range(self.radius + other.radius)
-    }
-
-    pub fn rows(&self) -> impl Iterator<Item = (usize, RangeInclusive<usize>)> {
-        let radius = self.radius.abs_round() as usize;
-        let center = Point::from_fppoint(&self.center);
-        (center.y as usize - radius..=center.y as usize + radius)
-            .map(move |row| (row, center.x as usize - radius..=center.x as usize + 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))
-                    }
-                }
-            }
-        }
-    }
+    physics: PhysicsProcessor,
+    collision: CollisionProcessor,
 }
 
 impl World {
     pub fn step(&mut self, time_step: FPNum, land: &Land2D<u32>) {
-        for (pos, vel) in self.enabled_physics.iter_mut_pos() {
-            *pos += *vel
-        }
-
-        self.grid.check_collisions(&mut self.collision_output);
-    }
-
-    fn check_land_collisions(&mut self, land: &Land2D<u32>) {
-        for collision in &self.enabled_collision {
-            if collision.bounds.rows().any(|(y, r)| (&land[y][r]).iter().any(|v| *v != 0)) {
-                self.land_collision_output.push(0)
-            }
-        }
+        let updates = self.physics.process(time_step);
+        self.collision.process(land, &updates);
     }
 
     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);
-        }
+        self.physics.push(data.gear_id, data.physics);
+        self.collision.push(data.gear_id, data.physics, data.collision);
     }
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/physics.rs	Fri Nov 09 01:05:34 2018 +0300
@@ -0,0 +1,99 @@
+use crate::{
+    common::GearId
+};
+use fpnum::*;
+use integral_geometry::{
+    Point, Size, GridIndex
+};
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub struct PhysicsData {
+    pub position: FPPoint,
+    pub velocity: FPPoint,
+}
+
+
+pub struct DynamicPhysicsCollection {
+    gear_ids: Vec<GearId>,
+    positions: Vec<FPPoint>,
+    velocities: Vec<FPPoint>,
+}
+
+impl DynamicPhysicsCollection {
+    fn len(&self) -> usize {
+        self.gear_ids.len()
+    }
+
+    fn push(&mut self, id: GearId, physics: PhysicsData) {
+        self.gear_ids.push(id);
+        self.positions.push(physics.position);
+        self.velocities.push(physics.velocity);
+    }
+
+    fn iter_pos_update(&mut self) -> impl Iterator<Item = (GearId, (&mut FPPoint, &FPPoint))> {
+        self.gear_ids.iter().cloned()
+            .zip(self.positions.iter_mut()
+                .zip(self.velocities.iter()))
+    }
+}
+
+pub struct StaticPhysicsCollection {
+    gear_ids: Vec<GearId>,
+    positions: Vec<FPPoint>
+}
+
+impl StaticPhysicsCollection {
+    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {
+        self.gear_ids.push(gear_id);
+        self.positions.push(physics.position);
+    }
+}
+
+pub struct PhysicsProcessor {
+    dynamic_physics: DynamicPhysicsCollection,
+    static_physics: StaticPhysicsCollection,
+
+    physics_cleanup: Vec<GearId>,
+    position_updates: PositionUpdate
+}
+
+pub struct PositionUpdate {
+    pub gear_ids: Vec<GearId>,
+    pub positions: Vec<FPPoint>
+}
+
+impl PositionUpdate {
+    pub fn new(capacity: usize) -> Self {
+        Self {
+            gear_ids: Vec::with_capacity(capacity),
+            positions: Vec::with_capacity(capacity),
+        }
+    }
+
+    pub fn push(&mut self, gear_id: GearId, position: &FPPoint) {
+        self.gear_ids.push(gear_id);
+        self.positions.push(*position);
+    }
+}
+
+impl PhysicsProcessor {
+    pub fn process(&mut self, time_step: FPNum) -> &PositionUpdate {
+        for (gear_id, (pos, vel)) in self.dynamic_physics.iter_pos_update() {
+            *pos += *vel * time_step;
+            if !vel.is_zero() {
+                self.position_updates.push(gear_id, pos)
+            } else {
+                self.physics_cleanup.push(gear_id)
+            }
+        }
+        &self.position_updates
+    }
+
+    pub fn push(&mut self, gear_id: GearId, physics_data: PhysicsData) {
+        if physics_data.velocity.is_zero() {
+            self.static_physics.push(gear_id, physics_data);
+        } else {
+            self.dynamic_physics.push(gear_id, physics_data);
+        }
+    }
+}
\ No newline at end of file