mpush

src-tauri/src/serial_core/multi_dim_force.rs

@@ -17,7 +17,6 @@ const ACTIVE_CELL_MIN_VALUE: f32 = 18.0;
 const ACTIVE_CELL_PEAK_RATIO: f32 = 0.14;
 const MIN_ACTIVE_CELLS: usize = 3;
 
-const ANCHOR_LERP_ALPHA: f32 = 0.018;
 const VECTOR_SMOOTHING_ALPHA: f32 = 0.16;
 
 const REPORT_MAGNITUDE_ENTER: f32 = 0.12;
@@ -29,6 +28,12 @@ const REPORT_HOLD_FRAMES: usize = 10;
 const ASYMMETRY_WEIGHT: f32 = 1.1;
 const DRIFT_WEIGHT: f32 = 0.65;
 const MOTION_WEIGHT: f32 = 0.25;
+const EDGE_ASYMMETRY_DAMPING: f32 = 0.35;
+const EDGE_INWARD_ROLLING_BIAS: f32 = 0.55;
+const EDGE_START_COP_THRESHOLD: f32 = 0.45;
+const EDGE_START_BIAS_WEIGHT: f32 = 1.1;
+const ROLLING_FRICTION_ALPHA: f32 = 0.68;
+const ROLLING_FRICTION_MIN_MAGNITUDE: f32 = 0.05;
 
 #[derive(Debug, Clone, Copy)]
 pub struct PztSpatialAnalysis {
@@ -50,6 +55,8 @@ pub struct PztProcessor {
     anchor_cop_y: Option<f32>,
     last_cop_x: Option<f32>,
     last_cop_y: Option<f32>,
+    edge_start_bias_x: f32,
+    edge_start_bias_y: f32,
     smoothed_x: f32,
     smoothed_y: f32,
     report_active: bool,
@@ -84,6 +91,8 @@ impl PztProcessor {
             anchor_cop_y: None,
             last_cop_x: None,
             last_cop_y: None,
+            edge_start_bias_x: 0.0,
+            edge_start_bias_y: 0.0,
             smoothed_x: 0.0,
             smoothed_y: 0.0,
             report_active: false,
@@ -100,6 +109,8 @@ impl PztProcessor {
         self.anchor_cop_y = None;
         self.last_cop_x = None;
         self.last_cop_y = None;
+        self.edge_start_bias_x = 0.0;
+        self.edge_start_bias_y = 0.0;
         self.smoothed_x = 0.0;
         self.smoothed_y = 0.0;
     }
@@ -273,6 +284,112 @@ impl PztProcessor {
         (angle, magnitude)
     }
 
+    fn contact_touches_edge(stats: &ContactStats) -> bool {
+        stats.min_row == 0
+            || stats.max_row == SENSOR_ROWS - 1
+            || stats.min_col == 0
+            || stats.max_col == SENSOR_COLS - 1
+    }
+
+    fn damp_edge_asymmetry(
+        stats: &ContactStats,
+        kinematic_x: f32,
+        kinematic_y: f32,
+    ) -> (f32, f32) {
+        let mut asymmetry_x = stats.asymmetry_x * ASYMMETRY_WEIGHT;
+        let mut asymmetry_y = stats.asymmetry_y * ASYMMETRY_WEIGHT;
+
+        if stats.min_col == 0 && asymmetry_x < 0.0 {
+            asymmetry_x = -asymmetry_x * EDGE_INWARD_ROLLING_BIAS;
+        }
+        if stats.max_col == SENSOR_COLS - 1 && asymmetry_x > 0.0 {
+            asymmetry_x = -asymmetry_x * EDGE_INWARD_ROLLING_BIAS;
+        }
+        if stats.min_row == 0 && asymmetry_y < 0.0 {
+            asymmetry_y = -asymmetry_y * EDGE_INWARD_ROLLING_BIAS;
+        }
+        if stats.max_row == SENSOR_ROWS - 1 && asymmetry_y > 0.0 {
+            asymmetry_y = -asymmetry_y * EDGE_INWARD_ROLLING_BIAS;
+        }
+
+        if Self::contact_touches_edge(stats) {
+            let opposing_dot = asymmetry_x * kinematic_x + asymmetry_y * kinematic_y;
+            let kinematic_mag = (kinematic_x * kinematic_x + kinematic_y * kinematic_y).sqrt();
+            if opposing_dot < 0.0 && kinematic_mag >= ROLLING_FRICTION_MIN_MAGNITUDE {
+                asymmetry_x *= EDGE_ASYMMETRY_DAMPING;
+                asymmetry_y *= EDGE_ASYMMETRY_DAMPING;
+            }
+        }
+
+        (asymmetry_x, asymmetry_y)
+    }
+
+    fn edge_start_bias(stats: &ContactStats) -> (f32, f32) {
+        let center_x = (SENSOR_COLS - 1) as f32 * 0.5;
+        let center_y = (SENSOR_ROWS - 1) as f32 * 0.5;
+        let normalized_x = ((stats.cop_x - center_x) / center_x.max(1.0)).clamp(-1.0, 1.0);
+        let normalized_y = ((stats.cop_y - center_y) / center_y.max(1.0)).clamp(-1.0, 1.0);
+
+        let mut bias_x = 0.0;
+        let mut bias_y = 0.0;
+
+        if stats.min_col == 0 || stats.max_col == SENSOR_COLS - 1 {
+            bias_x = Self::edge_start_axis_bias(normalized_x);
+        }
+        if stats.min_row == 0 || stats.max_row == SENSOR_ROWS - 1 {
+            bias_y = Self::edge_start_axis_bias(normalized_y);
+        }
+
+        (bias_x, bias_y)
+    }
+
+    fn edge_start_axis_bias(normalized_axis: f32) -> f32 {
+        let distance = normalized_axis.abs();
+        if distance <= EDGE_START_COP_THRESHOLD {
+            return 0.0;
+        }
+
+        let strength = ((distance - EDGE_START_COP_THRESHOLD) / (1.0 - EDGE_START_COP_THRESHOLD))
+            .clamp(0.0, 1.0);
+        -normalized_axis.signum() * strength * EDGE_START_BIAS_WEIGHT
+    }
+
+    fn apply_rolling_friction(
+        previous_x: f32,
+        previous_y: f32,
+        current_x: f32,
+        current_y: f32,
+    ) -> (f32, f32) {
+        let previous_mag = (previous_x * previous_x + previous_y * previous_y).sqrt();
+        let current_mag = (current_x * current_x + current_y * current_y).sqrt();
+
+        if previous_mag < ROLLING_FRICTION_MIN_MAGNITUDE
+            || current_mag < ROLLING_FRICTION_MIN_MAGNITUDE
+        {
+            return (current_x, current_y);
+        }
+
+        let dot = previous_x * current_x + previous_y * current_y;
+        if dot >= 0.0 {
+            return (current_x, current_y);
+        }
+
+        let mixed_x = current_x * (1.0 - ROLLING_FRICTION_ALPHA)
+            + previous_x * ROLLING_FRICTION_ALPHA;
+        let mixed_y = current_y * (1.0 - ROLLING_FRICTION_ALPHA)
+            + previous_y * ROLLING_FRICTION_ALPHA;
+
+        if mixed_x * previous_x + mixed_y * previous_y >= 0.0 {
+            return (mixed_x, mixed_y);
+        }
+
+        let keep_mag = previous_mag.min(current_mag) * 0.5;
+        (
+            previous_x / previous_mag * keep_mag,
+            previous_y / previous_mag * keep_mag,
+        )
+    }
+
     fn update_contact_state(&mut self, raw_frame: &[f32], frame: &[f32]) -> bool {
         if self.contact_active {
             if Self::is_contact_exit_frame(frame) {
@@ -376,6 +493,9 @@ impl PztProcessor {
             self.anchor_cop_y = Some(stats.cop_y);
             self.last_cop_x = Some(stats.cop_x);
             self.last_cop_y = Some(stats.cop_y);
+            let (edge_start_bias_x, edge_start_bias_y) = Self::edge_start_bias(&stats);
+            self.edge_start_bias_x = edge_start_bias_x;
+            self.edge_start_bias_y = edge_start_bias_y;
 
             return Ok(self.stabilize_report(Self::weak_contact_analysis()));
         };
@@ -388,18 +508,25 @@ impl PztProcessor {
         let motion_x = stats.cop_x - last_x;
         let motion_y = stats.cop_y - last_y;
 
-        let combined_x = stats.asymmetry_x * ASYMMETRY_WEIGHT
-            + drift_x * DRIFT_WEIGHT
-            + motion_x * MOTION_WEIGHT;
-        let combined_y = stats.asymmetry_y * ASYMMETRY_WEIGHT
-            + drift_y * DRIFT_WEIGHT
-            + motion_y * MOTION_WEIGHT;
+        let kinematic_x = drift_x * DRIFT_WEIGHT + motion_x * MOTION_WEIGHT;
+        let kinematic_y = drift_y * DRIFT_WEIGHT + motion_y * MOTION_WEIGHT;
+        let edge_bias_x = self.edge_start_bias_x;
+        let edge_bias_y = self.edge_start_bias_y;
+        let (asymmetry_x, asymmetry_y) =
+            Self::damp_edge_asymmetry(&stats, kinematic_x + edge_bias_x, kinematic_y + edge_bias_y);
+
+        let combined_x = asymmetry_x + kinematic_x + edge_bias_x;
+        let combined_y = asymmetry_y + kinematic_y + edge_bias_y;
+        let (combined_x, combined_y) = Self::apply_rolling_friction(
+            self.smoothed_x,
+            self.smoothed_y,
+            combined_x,
+            combined_y,
+        );
 
         self.smoothed_x += (combined_x - self.smoothed_x) * VECTOR_SMOOTHING_ALPHA;
         self.smoothed_y += (combined_y - self.smoothed_y) * VECTOR_SMOOTHING_ALPHA;
 
-        self.anchor_cop_x = Some(anchor_x + drift_x * ANCHOR_LERP_ALPHA);
-        self.anchor_cop_y = Some(anchor_y + drift_y * ANCHOR_LERP_ALPHA);
         self.last_cop_x = Some(stats.cop_x);
         self.last_cop_y = Some(stats.cop_y);
 
@@ -446,7 +573,7 @@ impl PztProcessor {
 
 #[cfg(test)]
 mod tests {
-    use super::{PztProcessor, SENSOR_COLS, SENSOR_ROWS};
+    use super::{ContactStats, PztProcessor, SENSOR_COLS, SENSOR_ROWS};
 
     fn index(row: usize, col: usize) -> usize {
         row * SENSOR_COLS + col
@@ -460,6 +587,23 @@ mod tests {
         frame
     }
 
+    fn stats_touching_bottom_edge() -> ContactStats {
+        ContactStats {
+            total: 1000.0,
+            peak: 300.0,
+            active_total: 900.0,
+            active_cells: 6,
+            min_row: SENSOR_ROWS - 2,
+            max_row: SENSOR_ROWS - 1,
+            min_col: 2,
+            max_col: 4,
+            cop_x: 3.0,
+            cop_y: 10.5,
+            asymmetry_x: 0.0,
+            asymmetry_y: 1.0,
+        }
+    }
+
     #[test]
     fn idle_frame_does_not_report_contact() {
         let mut processor = PztProcessor::new();
@@ -524,4 +668,29 @@ mod tests {
         let analysis = processor.get_pzt_analysis(&weak).unwrap();
         assert!(analysis.reportable);
     }
+
+    #[test]
+    fn bottom_edge_outward_gradient_is_turned_inward() {
+        let stats = stats_touching_bottom_edge();
+        let (_asymmetry_x, asymmetry_y) = PztProcessor::damp_edge_asymmetry(&stats, 0.0, -0.2);
+
+        assert!(asymmetry_y < 0.0);
+        assert!(asymmetry_y > -1.1);
+    }
+
+    #[test]
+    fn bottom_edge_start_adds_fixed_upward_bias() {
+        let stats = stats_touching_bottom_edge();
+        let (_bias_x, bias_y) = PztProcessor::edge_start_bias(&stats);
+
+        assert!(bias_y < 0.0);
+    }
+
+    #[test]
+    fn rolling_friction_resists_one_frame_reversal() {
+        let (x, y) = PztProcessor::apply_rolling_friction(0.4, 0.0, -0.6, 0.0);
+
+        assert!(x > 0.0);
+        assert_eq!(y, 0.0);
+    }
 }