From aa08a75aef6cff9fe3294e38bb1a8187a78d900d Mon Sep 17 00:00:00 2001 From: lenn Date: Wed, 20 May 2026 09:39:14 +0800 Subject: [PATCH] fix: compensate tangential force edge and full-surface cases --- src-tauri/src/serial_core/multi_dim_force.rs | 344 +++++++++++++++++-- 1 file changed, 322 insertions(+), 22 deletions(-) diff --git a/src-tauri/src/serial_core/multi_dim_force.rs b/src-tauri/src/serial_core/multi_dim_force.rs index 629e9a1..3909baf 100644 --- a/src-tauri/src/serial_core/multi_dim_force.rs +++ b/src-tauri/src/serial_core/multi_dim_force.rs @@ -18,6 +18,8 @@ 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; @@ -29,6 +31,21 @@ 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 { @@ -50,6 +67,11 @@ 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, @@ -63,14 +85,19 @@ 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 { @@ -84,6 +111,11 @@ 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, @@ -100,6 +132,11 @@ 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; } @@ -175,6 +212,31 @@ 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 { @@ -192,15 +254,24 @@ 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; } @@ -226,6 +297,18 @@ 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; @@ -248,14 +331,19 @@ 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, }) } @@ -307,6 +395,40 @@ 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; @@ -370,6 +492,11 @@ 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); @@ -388,18 +515,75 @@ 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 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, + ) + }; 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); + 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.last_cop_x = Some(stats.cop_x); self.last_cop_y = Some(stats.cop_y); @@ -407,15 +591,33 @@ impl PztProcessor { let planar_y = -self.smoothed_y; let (angle_deg, magnitude) = Self::compute_vector_angle(planar_x, planar_y); - 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 activity = stats.coverage.clamp(0.0, 1.0); + let span = ((stats.span_rows + stats.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)) + (((activity * 0.35) + (span * 0.2) + (pressure_ratio * 0.3) + (peak_ratio * 0.15)) + * edge_penalty + + full_surface_bonus + + trend_bonus) .clamp(0.0, 1.0); Ok(self.stabilize_report(PztSpatialAnalysis { @@ -460,6 +662,32 @@ 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(); @@ -496,7 +724,7 @@ mod tests { assert!(analysis.contact_active); assert!(analysis.reportable); assert!(analysis.magnitude > 0.0); - assert!(analysis.angle_deg <= 45.0 || analysis.angle_deg >= 315.0); + assert!(is_rightward(analysis.angle_deg)); } #[test] @@ -524,4 +752,76 @@ 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)); + } }