# HG changeset patch
# User alfadur
# Date 1564505603 -10800
# Node ID 8095853811a60e07e61ee11d92338cbe0fb0e916
# Parent  b12f63054c94f564ef1b0bb5856febaf98893b1c
add gear lookup to the physics processor

diff -r b12f63054c94 -r 8095853811a6 rust/hwphysics/src/common.rs
--- a/rust/hwphysics/src/common.rs	Mon Jul 29 21:58:39 2019 +0200
+++ b/rust/hwphysics/src/common.rs	Tue Jul 30 19:53:23 2019 +0300
@@ -1,10 +1,15 @@
 use fpnum::{fp, FPNum};
-use std::{collections::BinaryHeap, num::NonZeroU16, ops::Add};
+use std::{
+    collections::BinaryHeap,
+    num::NonZeroU16,
+    ops::{Add, Index, IndexMut},
+};
 
 pub type GearId = NonZeroU16;
 pub trait GearData {}
 
 #[derive(PartialEq, Eq, PartialOrd, Ord, Clone, Copy, Debug)]
+#[repr(transparent)]
 pub struct Millis(u32);
 
 impl Millis {
@@ -67,3 +72,53 @@
         self.free_ids.push(gear_id)
     }
 }
+
+#[derive(Clone, Copy, Default)]
+pub struct LookupEntry<T> {
+    pub index: u16,
+    pub value: T,
+}
+
+impl<T> LookupEntry<T> {
+    pub fn new(index: u16, value: T) -> Self {
+        Self { index, value }
+    }
+}
+
+pub struct GearDataLookup<T> {
+    lookup: [LookupEntry<T>; u16::max_value() as usize],
+}
+
+impl<T: Default + Copy> GearDataLookup<T> {
+    pub fn new() -> Self {
+        Self {
+            lookup: [LookupEntry::<T>::default(); u16::max_value() as usize],
+        }
+    }
+}
+
+impl<T> GearDataLookup<T> {
+    pub fn get(&self, gear_id: GearId) -> &LookupEntry<T> {
+        // All possible Gear IDs are valid indices
+        unsafe { self.lookup.get_unchecked(gear_id.get() as usize - 1) }
+    }
+
+    pub fn get_mut(&mut self, gear_id: GearId) -> &mut LookupEntry<T> {
+        // All possible Gear IDs are valid indices
+        unsafe { self.lookup.get_unchecked_mut(gear_id.get() as usize - 1) }
+    }
+}
+
+impl<T> Index<GearId> for GearDataLookup<T> {
+    type Output = LookupEntry<T>;
+
+    fn index(&self, index: GearId) -> &Self::Output {
+        self.get(index)
+    }
+}
+
+impl<T> IndexMut<GearId> for GearDataLookup<T> {
+    fn index_mut(&mut self, index: GearId) -> &mut Self::Output {
+        self.get_mut(index)
+    }
+}
diff -r b12f63054c94 -r 8095853811a6 rust/hwphysics/src/lib.rs
--- a/rust/hwphysics/src/lib.rs	Mon Jul 29 21:58:39 2019 +0200
+++ b/rust/hwphysics/src/lib.rs	Tue Jul 30 19:53:23 2019 +0300
@@ -4,7 +4,6 @@
 pub mod physics;
 pub mod time;
 
-use fpnum::FPNum;
 use integral_geometry::Size;
 use land2d::Land2D;
 
diff -r b12f63054c94 -r 8095853811a6 rust/hwphysics/src/physics.rs
--- a/rust/hwphysics/src/physics.rs	Mon Jul 29 21:58:39 2019 +0200
+++ b/rust/hwphysics/src/physics.rs	Tue Jul 30 19:53:23 2019 +0300
@@ -1,4 +1,4 @@
-use crate::common::{GearData, GearDataProcessor, GearId, Millis};
+use crate::common::{GearData, GearDataLookup, GearDataProcessor, GearId, LookupEntry, Millis};
 use fpnum::*;
 use integral_geometry::{GridIndex, Point, Size};
 
@@ -35,18 +35,20 @@
         self.gear_ids.len()
     }
 
-    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {
+    fn push(&mut self, gear_id: GearId, physics: PhysicsData) -> u16 {
         self.gear_ids.push(gear_id);
         self.positions.push(physics.position);
         self.velocities.push(physics.velocity);
+
+        (self.gear_ids.len() - 1) as u16
     }
 
-    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 remove(&mut self, index: usize) -> Option<GearId> {
+        self.gear_ids.swap_remove(index);
+        self.positions.swap_remove(index);
+        self.velocities.swap_remove(index);
+
+        self.gear_ids.get(index).cloned()
     }
 
     fn iter_pos_update(&mut self) -> impl Iterator<Item = (GearId, (&mut FPPoint, &FPPoint))> {
@@ -70,20 +72,23 @@
         }
     }
 
-    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {
+    fn push(&mut self, gear_id: GearId, physics: PhysicsData) -> u16 {
         self.gear_ids.push(gear_id);
         self.positions.push(physics.position);
+
+        (self.gear_ids.len() - 1) as u16
     }
 
-    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);
-        }
+    fn remove(&mut self, index: usize) -> Option<GearId> {
+        self.gear_ids.swap_remove(index);
+        self.positions.swap_remove(index);
+
+        self.gear_ids.get(index).cloned()
     }
 }
 
 pub struct PhysicsProcessor {
+    gear_lookup: GearDataLookup<bool>,
     dynamic_physics: DynamicPhysicsCollection,
     static_physics: StaticPhysicsCollection,
 
@@ -126,6 +131,7 @@
 impl PhysicsProcessor {
     pub fn new() -> Self {
         Self {
+            gear_lookup: GearDataLookup::new(),
             dynamic_physics: DynamicPhysicsCollection::new(),
             static_physics: StaticPhysicsCollection::new(),
             physics_cleanup: Vec::new(),
@@ -147,27 +153,30 @@
         }
         &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);
+        let is_dynamic = !gear_data.velocity.is_zero();
+        let index = if is_dynamic {
+            self.dynamic_physics.push(gear_id, gear_data)
         } else {
-            self.dynamic_physics.push(gear_id, gear_data);
-        }
+            self.static_physics.push(gear_id, gear_data)
+        };
+
+        self.gear_lookup[gear_id] = LookupEntry::new(index, is_dynamic);
     }
 
     fn remove(&mut self, gear_id: GearId) {
-        self.static_physics.remove(gear_id);
-        self.dynamic_physics.remove(gear_id)
+        let location = self.gear_lookup[gear_id];
+        let relocated_gear_id = if location.value {
+            self.dynamic_physics.remove(location.index as usize)
+        } else {
+            self.static_physics.remove(location.index as usize)
+        };
+
+        if let Some(id) = relocated_gear_id {
+            self.gear_lookup[id].index = location.index;
+        }
     }
 }