--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rust/hwphysics/src/physics.rs	Wed Jul 31 23:14:27 2019 +0200
@@ -0,0 +1,173 @@
+use crate::common::{GearData, GearDataProcessor, GearId, Millis};
+use fpnum::*;
+use integral_geometry::{GridIndex, Point, Size};
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub struct PhysicsData {
+    pub position: FPPoint,
+    pub velocity: FPPoint,
+}
+
+impl GearData for PhysicsData {}
+
+impl PhysicsData {
+    pub fn new(position: FPPoint, velocity: FPPoint) -> Self {
+        Self { position, velocity }
+    }
+}
+
+pub struct DynamicPhysicsCollection {
+    gear_ids: Vec<GearId>,
+    positions: Vec<FPPoint>,
+    velocities: Vec<FPPoint>,
+}
+
+impl DynamicPhysicsCollection {
+    fn new() -> Self {
+        Self {
+            gear_ids: Vec::new(),
+            positions: Vec::new(),
+            velocities: Vec::new(),
+        }
+    }
+
+    fn len(&self) -> usize {
+        self.gear_ids.len()
+    }
+
+    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {
+        self.gear_ids.push(gear_id);
+        self.positions.push(physics.position);
+        self.velocities.push(physics.velocity);
+    }
+
+    fn remove(&mut self, gear_id: GearId) {
+        if let Some(index) = self.gear_ids.iter().position(|id| *id == gear_id) {
+            self.gear_ids.swap_remove(index);
+            self.positions.swap_remove(index);
+            self.velocities.swap_remove(index);
+        }
+    }
+
+    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 new() -> Self {
+        Self {
+            gear_ids: Vec::new(),
+            positions: Vec::new(),
+        }
+    }
+
+    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {
+        self.gear_ids.push(gear_id);
+        self.positions.push(physics.position);
+    }
+
+    fn remove(&mut self, gear_id: GearId) {
+        if let Some(index) = self.gear_ids.iter().position(|id| *id == gear_id) {
+            self.gear_ids.swap_remove(index);
+            self.positions.swap_remove(index);
+        }
+    }
+}
+
+pub struct PhysicsProcessor {
+    dynamic_physics: DynamicPhysicsCollection,
+    static_physics: StaticPhysicsCollection,
+
+    physics_cleanup: Vec<GearId>,
+    position_updates: PositionUpdates,
+}
+
+pub struct PositionUpdates {
+    pub gear_ids: Vec<GearId>,
+    pub shifts: Vec<(FPPoint, FPPoint)>,
+}
+
+impl PositionUpdates {
+    pub fn new(capacity: usize) -> Self {
+        Self {
+            gear_ids: Vec::with_capacity(capacity),
+            shifts: Vec::with_capacity(capacity),
+        }
+    }
+
+    pub fn push(&mut self, gear_id: GearId, old_position: &FPPoint, new_position: &FPPoint) {
+        self.gear_ids.push(gear_id);
+        self.shifts.push((*old_position, *new_position));
+    }
+
+    pub fn iter(&self) -> impl Iterator<Item = (GearId, &FPPoint, &FPPoint)> {
+        self.gear_ids
+            .iter()
+            .cloned()
+            .zip(self.shifts.iter())
+            .map(|(id, (from, to))| (id, from, to))
+    }
+
+    pub fn clear(&mut self) {
+        self.gear_ids.clear();
+        self.shifts.clear();
+    }
+}
+
+impl PhysicsProcessor {
+    pub fn new() -> Self {
+        Self {
+            dynamic_physics: DynamicPhysicsCollection::new(),
+            static_physics: StaticPhysicsCollection::new(),
+            physics_cleanup: Vec::new(),
+            position_updates: PositionUpdates::new(0),
+        }
+    }
+
+    pub fn process(&mut self, time_step: Millis) -> &PositionUpdates {
+        let fp_step = time_step.to_fixed();
+        self.position_updates.clear();
+        for (gear_id, (pos, vel)) in self.dynamic_physics.iter_pos_update() {
+            let old_pos = *pos;
+            *pos += *vel * fp_step;
+            if !vel.is_zero() {
+                self.position_updates.push(gear_id, &old_pos, 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);
+        }
+    }
+}
+
+impl GearDataProcessor<PhysicsData> for PhysicsProcessor {
+    fn add(&mut self, gear_id: GearId, gear_data: PhysicsData) {
+        if gear_data.velocity.is_zero() {
+            self.static_physics.push(gear_id, gear_data);
+        } else {
+            self.dynamic_physics.push(gear_id, gear_data);
+        }
+    }
+
+    fn remove(&mut self, gear_id: GearId) {
+        self.static_physics.remove(gear_id);
+        self.dynamic_physics.remove(gear_id)
+    }
+}