yanjie/JE-Skin
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

fix: compensate tangential force edge and full-surface cases

Browse Source
This commit is contained in:
lenn
2026-05-20 09:39:14 +08:00
parent 6187976b6b
commit aa08a75aef
1 changed files with 322 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
src-tauri/src/serial_core/multi_dim_force.rs
View File

@@ -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<f32>,
last_cop_x: Option<f32>,
last_cop_y: Option<f32>,
contact_frames: usize,
stable_patch_frames: usize,
last_active_cells: Option<usize>,
last_span_rows: Option<f32>,
last_span_cols: Option<f32>,
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<ContactStats> {
let total = frame.iter().sum::<f32>();
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));
}
}