diff --git a/src-tauri/src/serial_core/multi_dim_force.rs b/src-tauri/src/serial_core/multi_dim_force.rs index 3909baf..629e9a1 100644 --- a/src-tauri/src/serial_core/multi_dim_force.rs +++ b/src-tauri/src/serial_core/multi_dim_force.rs @@ -18,8 +18,6 @@ const ACTIVE_CELL_PEAK_RATIO: f32 = 0.14; const MIN_ACTIVE_CELLS: usize = 3; const ANCHOR_LERP_ALPHA: f32 = 0.018; -const PATCH_IMMATURE_ANCHOR_ALPHA: f32 = 0.16; -const EDGE_GROWTH_ANCHOR_ALPHA: f32 = 0.42; const VECTOR_SMOOTHING_ALPHA: f32 = 0.16; const REPORT_MAGNITUDE_ENTER: f32 = 0.12; @@ -31,21 +29,6 @@ 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 LOCAL_GLOBAL_TREND_WEIGHT: f32 = 0.18; - -const PATCH_MATURE_STABLE_FRAMES: usize = 3; -const EDGE_SETTLE_FRAMES: usize = 12; -const EDGE_GROWTH_CELL_TOLERANCE: usize = 2; -const EDGE_GROWTH_SPAN_TOLERANCE: f32 = 0.08; -const EDGE_CLIP_COMPENSATION_WEIGHT: f32 = 0.62; -const EDGE_TRANSIENT_DRIFT_GAIN: f32 = 0.22; - -const FULL_SURFACE_ACTIVE_RATIO: f32 = 0.34; -const FULL_SURFACE_SPAN_RATIO: f32 = 0.74; -const FULL_SURFACE_LOCAL_WEIGHT: f32 = 0.18; -const FULL_SURFACE_TREND_WEIGHT: f32 = 1.25; -const FULL_SURFACE_DRIFT_WEIGHT: f32 = 0.22; -const FULL_SURFACE_MOTION_WEIGHT: f32 = 0.18; #[derive(Debug, Clone, Copy)] pub struct PztSpatialAnalysis { @@ -67,11 +50,6 @@ pub struct PztProcessor { anchor_cop_y: Option, last_cop_x: Option, last_cop_y: Option, - contact_frames: usize, - stable_patch_frames: usize, - last_active_cells: Option, - last_span_rows: Option, - last_span_cols: Option, smoothed_x: f32, smoothed_y: f32, report_active: bool, @@ -85,19 +63,14 @@ struct ContactStats { peak: f32, active_total: f32, active_cells: usize, + min_row: usize, + max_row: usize, + min_col: usize, + max_col: usize, cop_x: f32, cop_y: f32, asymmetry_x: f32, asymmetry_y: f32, - global_trend_x: f32, - global_trend_y: f32, - edge_bias_x: f32, - edge_bias_y: f32, - span_rows: f32, - span_cols: f32, - coverage: f32, - edge_contact: bool, - full_surface: bool, } impl PztProcessor { @@ -111,11 +84,6 @@ impl PztProcessor { anchor_cop_y: None, last_cop_x: None, last_cop_y: None, - contact_frames: 0, - stable_patch_frames: 0, - last_active_cells: None, - last_span_rows: None, - last_span_cols: None, smoothed_x: 0.0, smoothed_y: 0.0, report_active: false, @@ -132,11 +100,6 @@ impl PztProcessor { self.anchor_cop_y = None; self.last_cop_x = None; self.last_cop_y = None; - self.contact_frames = 0; - self.stable_patch_frames = 0; - self.last_active_cells = None; - self.last_span_rows = None; - self.last_span_cols = None; self.smoothed_x = 0.0; self.smoothed_y = 0.0; } @@ -212,31 +175,6 @@ impl PztProcessor { } } - fn edge_clip_bias(min_index: usize, max_index: usize, size: usize) -> f32 { - let touches_min = min_index == 0; - let touches_max = max_index + 1 == size; - if touches_min == touches_max { - return 0.0; - } - - let span_ratio = (max_index - min_index + 1) as f32 / size as f32; - let strength = (1.0 - span_ratio).clamp(0.18, 0.9); - if touches_min { - strength - } else { - -strength - } - } - - fn damp_same_direction_bias(value: f32, bias: f32, weight: f32) -> f32 { - if bias == 0.0 || value == 0.0 || value.signum() != bias.signum() { - return value; - } - - let adjusted = (value.abs() - bias.abs() * weight).max(0.0); - adjusted * value.signum() - } - fn compute_contact_stats(frame: &[f32]) -> Option { let total = frame.iter().sum::(); if total <= 0.0 { @@ -254,24 +192,15 @@ impl PztProcessor { let mut active_cells = 0usize; let mut weighted_col_sum = 0.0; let mut weighted_row_sum = 0.0; - let mut global_col_trend = 0.0; - let mut global_row_trend = 0.0; let mut min_row = SENSOR_ROWS; let mut max_row = 0usize; let mut min_col = SENSOR_COLS; let mut max_col = 0usize; - let center_col = (SENSOR_COLS - 1) as f32 * 0.5; - let center_row = (SENSOR_ROWS - 1) as f32 * 0.5; - let half_cols = center_col.max(1.0); - let half_rows = center_row.max(1.0); for row in 0..SENSOR_ROWS { for col in 0..SENSOR_COLS { let index = row * SENSOR_COLS + col; let value = frame[index]; - global_col_trend += value * ((col as f32 - center_col) / half_cols); - global_row_trend += value * ((row as f32 - center_row) / half_rows); - if value < active_threshold { continue; } @@ -297,18 +226,6 @@ impl PztProcessor { let bbox_center_y = (min_row + max_row) as f32 * 0.5; let half_width = ((max_col - min_col).max(1) as f32) * 0.5; let half_height = ((max_row - min_row).max(1) as f32) * 0.5; - let span_rows = (max_row - min_row + 1) as f32 / SENSOR_ROWS as f32; - let span_cols = (max_col - min_col + 1) as f32 / SENSOR_COLS as f32; - let coverage = active_cells as f32 / SENSOR_COUNT as f32; - let edge_bias_x = Self::edge_clip_bias(min_col, max_col, SENSOR_COLS); - let edge_bias_y = Self::edge_clip_bias(min_row, max_row, SENSOR_ROWS); - let edge_contact = min_row == 0 - || max_row + 1 == SENSOR_ROWS - || min_col == 0 - || max_col + 1 == SENSOR_COLS; - let full_surface = coverage >= FULL_SURFACE_ACTIVE_RATIO - && span_rows >= FULL_SURFACE_SPAN_RATIO - && span_cols >= FULL_SURFACE_SPAN_RATIO; let mut asymmetry_x = 0.0; let mut asymmetry_y = 0.0; @@ -331,19 +248,14 @@ impl PztProcessor { peak, active_total, active_cells, + min_row, + max_row, + min_col, + max_col, cop_x, cop_y, asymmetry_x: asymmetry_x / active_total, asymmetry_y: asymmetry_y / active_total, - global_trend_x: global_col_trend / total, - global_trend_y: global_row_trend / total, - edge_bias_x, - edge_bias_y, - span_rows, - span_cols, - coverage, - edge_contact, - full_surface, }) } @@ -395,40 +307,6 @@ impl PztProcessor { false } - fn update_patch_dynamics(&mut self, stats: &ContactStats) -> bool { - self.contact_frames += 1; - - let cell_growth = self - .last_active_cells - .map(|last| stats.active_cells > last + EDGE_GROWTH_CELL_TOLERANCE) - .unwrap_or(false); - let row_growth = self - .last_span_rows - .map(|last| stats.span_rows > last + EDGE_GROWTH_SPAN_TOLERANCE) - .unwrap_or(false); - let col_growth = self - .last_span_cols - .map(|last| stats.span_cols > last + EDGE_GROWTH_SPAN_TOLERANCE) - .unwrap_or(false); - let expanding = cell_growth || row_growth || col_growth; - - if expanding { - self.stable_patch_frames = 0; - } else { - self.stable_patch_frames = self.stable_patch_frames.saturating_add(1); - } - - self.last_active_cells = Some(stats.active_cells); - self.last_span_rows = Some(stats.span_rows); - self.last_span_cols = Some(stats.span_cols); - - expanding - } - - fn patch_maturity(&self) -> f32 { - (self.stable_patch_frames as f32 / PATCH_MATURE_STABLE_FRAMES as f32).clamp(0.0, 1.0) - } - fn store_report(&mut self, mut analysis: PztSpatialAnalysis) -> PztSpatialAnalysis { analysis.reportable = true; self.report_active = true; @@ -492,11 +370,6 @@ impl PztProcessor { let Some(stats) = Self::compute_contact_stats(&baseline_subtracted) else { return Ok(self.stabilize_report(Self::weak_contact_analysis())); }; - let patch_expanding = self.update_patch_dynamics(&stats); - let patch_maturity = self.patch_maturity(); - let edge_settling = - stats.edge_contact && !stats.full_surface && self.contact_frames <= EDGE_SETTLE_FRAMES; - let edge_transient = edge_settling && patch_expanding; let Some(anchor_x) = self.anchor_cop_x else { self.anchor_cop_x = Some(stats.cop_x); @@ -515,75 +388,18 @@ impl PztProcessor { let motion_x = stats.cop_x - last_x; let motion_y = stats.cop_y - last_y; - let edge_compensation = if edge_settling { - EDGE_CLIP_COMPENSATION_WEIGHT - } else { - EDGE_CLIP_COMPENSATION_WEIGHT * 0.65 - }; - let corrected_asymmetry_x = - Self::damp_same_direction_bias(stats.asymmetry_x, stats.edge_bias_x, edge_compensation); - let corrected_asymmetry_y = - Self::damp_same_direction_bias(stats.asymmetry_y, stats.edge_bias_y, edge_compensation); - - let half_cols = ((SENSOR_COLS - 1) as f32 * 0.5).max(1.0); - let half_rows = ((SENSOR_ROWS - 1) as f32 * 0.5).max(1.0); - let drift_x_norm = drift_x / half_cols; - let drift_y_norm = drift_y / half_rows; - let motion_x_norm = motion_x / half_cols; - let motion_y_norm = motion_y / half_rows; - - let (combined_x, combined_y) = if stats.full_surface { - ( - corrected_asymmetry_x * FULL_SURFACE_LOCAL_WEIGHT - + stats.global_trend_x * FULL_SURFACE_TREND_WEIGHT - + drift_x_norm * FULL_SURFACE_DRIFT_WEIGHT - + motion_x_norm * FULL_SURFACE_MOTION_WEIGHT, - corrected_asymmetry_y * FULL_SURFACE_LOCAL_WEIGHT - + stats.global_trend_y * FULL_SURFACE_TREND_WEIGHT - + drift_y_norm * FULL_SURFACE_DRIFT_WEIGHT - + motion_y_norm * FULL_SURFACE_MOTION_WEIGHT, - ) - } else { - let drift_gain = if edge_settling { - EDGE_TRANSIENT_DRIFT_GAIN - } else { - patch_maturity.max(0.35) - }; - let motion_gain = if edge_transient { - EDGE_TRANSIENT_DRIFT_GAIN.max(0.35) - } else { - 1.0 - }; - let local_trend_gain = if stats.edge_contact { - 0.0 - } else { - ((stats.coverage - 0.16) / 0.18).clamp(0.0, 1.0) - }; - - ( - corrected_asymmetry_x * ASYMMETRY_WEIGHT - + drift_x * DRIFT_WEIGHT * drift_gain - + motion_x * MOTION_WEIGHT * motion_gain - + stats.global_trend_x * LOCAL_GLOBAL_TREND_WEIGHT * local_trend_gain, - corrected_asymmetry_y * ASYMMETRY_WEIGHT - + drift_y * DRIFT_WEIGHT * drift_gain - + motion_y * MOTION_WEIGHT * motion_gain - + stats.global_trend_y * LOCAL_GLOBAL_TREND_WEIGHT * local_trend_gain, - ) - }; + 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; self.smoothed_x += (combined_x - self.smoothed_x) * VECTOR_SMOOTHING_ALPHA; self.smoothed_y += (combined_y - self.smoothed_y) * VECTOR_SMOOTHING_ALPHA; - let anchor_alpha = if edge_settling { - EDGE_GROWTH_ANCHOR_ALPHA - } else if patch_maturity < 1.0 && !stats.full_surface { - PATCH_IMMATURE_ANCHOR_ALPHA - } else { - ANCHOR_LERP_ALPHA - }; - self.anchor_cop_x = Some(anchor_x + drift_x * anchor_alpha); - self.anchor_cop_y = Some(anchor_y + drift_y * anchor_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); @@ -591,33 +407,15 @@ impl PztProcessor { let planar_y = -self.smoothed_y; let (angle_deg, magnitude) = Self::compute_vector_angle(planar_x, planar_y); - let activity = stats.coverage.clamp(0.0, 1.0); - let span = ((stats.span_rows + stats.span_cols) * 0.5).clamp(0.0, 1.0); + let active_span_rows = (stats.max_row - stats.min_row + 1) as f32 / SENSOR_ROWS as f32; + let active_span_cols = (stats.max_col - stats.min_col + 1) as f32 / SENSOR_COLS as f32; + let activity = (stats.active_cells as f32 / SENSOR_COUNT as f32).clamp(0.0, 1.0); + let span = ((active_span_rows + active_span_cols) * 0.5).clamp(0.0, 1.0); let pressure_ratio = (stats.active_total / stats.total.max(1.0)).clamp(0.0, 1.0); let peak_ratio = (stats.peak / (stats.total / stats.active_cells as f32 + 1.0)).clamp(0.0, 1.0); - let trend_strength = (stats.global_trend_x * stats.global_trend_x - + stats.global_trend_y * stats.global_trend_y) - .sqrt() - .clamp(0.0, 1.0); - let edge_penalty = if edge_settling { - 0.72 - } else if stats.edge_contact && !stats.full_surface { - 0.9 - } else { - 1.0 - }; - let full_surface_bonus = if stats.full_surface { 0.12 } else { 0.0 }; - let trend_bonus = if stats.full_surface { - trend_strength * 0.28 - } else { - 0.0 - }; let confidence = - (((activity * 0.35) + (span * 0.2) + (pressure_ratio * 0.3) + (peak_ratio * 0.15)) - * edge_penalty - + full_surface_bonus - + trend_bonus) + ((activity * 0.35) + (span * 0.2) + (pressure_ratio * 0.3) + (peak_ratio * 0.15)) .clamp(0.0, 1.0); Ok(self.stabilize_report(PztSpatialAnalysis { @@ -662,32 +460,6 @@ mod tests { frame } - fn make_surface_frame( - mut value_at: impl FnMut(usize, usize) -> f32, - ) -> [f32; SENSOR_ROWS * SENSOR_COLS] { - let mut frame = [0.0; SENSOR_ROWS * SENSOR_COLS]; - for row in 0..SENSOR_ROWS { - for col in 0..SENSOR_COLS { - frame[index(row, col)] = value_at(row, col); - } - } - frame - } - - fn make_left_edge_patch(width: usize, value: f32) -> [f32; SENSOR_ROWS * SENSOR_COLS] { - let mut frame = [0.0; SENSOR_ROWS * SENSOR_COLS]; - for row in 4..=7 { - for col in 0..width.min(SENSOR_COLS) { - frame[index(row, col)] = value; - } - } - frame - } - - fn is_rightward(angle_deg: f32) -> bool { - angle_deg <= 45.0 || angle_deg >= 315.0 - } - #[test] fn idle_frame_does_not_report_contact() { let mut processor = PztProcessor::new(); @@ -724,7 +496,7 @@ mod tests { assert!(analysis.contact_active); assert!(analysis.reportable); assert!(analysis.magnitude > 0.0); - assert!(is_rightward(analysis.angle_deg)); + assert!(analysis.angle_deg <= 45.0 || analysis.angle_deg >= 315.0); } #[test] @@ -752,76 +524,4 @@ mod tests { let analysis = processor.get_pzt_analysis(&weak).unwrap(); assert!(analysis.reportable); } - - #[test] - fn edge_patch_growth_does_not_create_false_inward_force() { - let mut processor = PztProcessor::new(); - let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; - let _ = processor.get_pzt_analysis(&baseline).unwrap(); - - let mut analysis = processor - .get_pzt_analysis(&make_left_edge_patch(1, 180.0)) - .unwrap(); - for width in [1, 2, 3, 4, 4, 4, 4, 4, 4, 4] { - analysis = processor - .get_pzt_analysis(&make_left_edge_patch(width, 180.0)) - .unwrap(); - } - - assert!(analysis.contact_active); - assert!(!analysis.reportable || analysis.magnitude < 0.12); - } - - #[test] - fn edge_to_inner_motion_can_still_report_inward_direction() { - let mut processor = PztProcessor::new(); - let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; - let _ = processor.get_pzt_analysis(&baseline).unwrap(); - for _ in 0..8 { - let _ = processor - .get_pzt_analysis(&make_left_edge_patch(2, 180.0)) - .unwrap(); - } - - let inward = make_frame(&[ - (4, 1, 120.0), - (4, 2, 180.0), - (4, 3, 280.0), - (5, 1, 120.0), - (5, 2, 180.0), - (5, 3, 280.0), - (6, 1, 120.0), - (6, 2, 180.0), - (6, 3, 280.0), - (7, 1, 120.0), - (7, 2, 180.0), - (7, 3, 280.0), - ]); - - let mut analysis = processor.get_pzt_analysis(&inward).unwrap(); - for _ in 0..8 { - analysis = processor.get_pzt_analysis(&inward).unwrap(); - } - - assert!(analysis.contact_active); - assert!(analysis.reportable); - assert!(is_rightward(analysis.angle_deg)); - } - - #[test] - fn full_surface_press_uses_global_pressure_trend() { - let mut processor = PztProcessor::new(); - let baseline = [0.0; SENSOR_ROWS * SENSOR_COLS]; - let full_surface = make_surface_frame(|_, col| 70.0 + col as f32 * 16.0); - - let _ = processor.get_pzt_analysis(&baseline).unwrap(); - let mut analysis = processor.get_pzt_analysis(&full_surface).unwrap(); - for _ in 0..8 { - analysis = processor.get_pzt_analysis(&full_surface).unwrap(); - } - - assert!(analysis.contact_active); - assert!(analysis.reportable); - assert!(is_rightward(analysis.angle_deg)); - } }