391 lines
13 KiB
Rust
391 lines
13 KiB
Rust
//! Adaptiver 1€-Filter (One Euro Filter) für rauschfreie und latenzoptimierte Stylus-Eingaben.
|
|
//!
|
|
//! Filtert x, y und Druck (pressure p ∈ [0.0, 1.0]).
|
|
//! Implementiert numerische Härtung:
|
|
//! - dt-Clamping: dt <= 0 wird mit dt = 1e-4 s abgefangen, dt > 1.0 s löst ein reset() aus.
|
|
//! - Glättungsfaktor alpha wird strikt auf den Bereich [1e-5, 1.0] begrenzt.
|
|
//! - NaN/Inf-Schutz gegen Filtervergiftung.
|
|
|
|
use std::f32::consts::PI;
|
|
|
|
/// Standard-Minimal-Grenzfrequenz in Hz (reduziert Jitter bei niedrigen Geschwindigkeiten)
|
|
pub const DEFAULT_FC_MIN: f32 = 4.5;
|
|
/// Standard-Geschwindigkeitskoeffizient (reduziert Latenz bei schnellen Bewegungen)
|
|
pub const DEFAULT_BETA: f32 = 0.05;
|
|
/// Standard-Grenzfrequenz für die Ableitung in Hz
|
|
pub const DEFAULT_D_CUTOFF: f32 = 1.0;
|
|
|
|
/// Berechnet den Glättungsfaktor alpha = 1 / (1 + tau / dt) = 2*pi*fc*dt / (2*pi*fc*dt + 1).
|
|
///
|
|
/// Numerisch gehärtet: Garantiert Werte im Bereich [1e-5, 1.0] und schützt vor NaN/Inf.
|
|
pub fn smoothing_factor(dt: f32, cutoff: f32) -> f32 {
|
|
let r = 2.0 * PI * cutoff * dt;
|
|
if !r.is_finite() || r <= 0.0 {
|
|
1e-5
|
|
} else {
|
|
let alpha = r / (r + 1.0);
|
|
if !alpha.is_finite() {
|
|
1e-5
|
|
} else {
|
|
alpha.clamp(1e-5, 1.0)
|
|
}
|
|
}
|
|
}
|
|
|
|
/// 1D One-Euro-Filter für skalare Signale.
|
|
#[derive(Debug, Clone)]
|
|
pub struct OneEuroFilter1D {
|
|
pub fc_min: f32,
|
|
pub beta: f32,
|
|
pub d_cutoff: f32,
|
|
x_prev: Option<f32>,
|
|
dx_prev: f32,
|
|
t_prev: Option<f64>,
|
|
}
|
|
|
|
impl OneEuroFilter1D {
|
|
/// Erstellt einen neuen 1D-Filter mit den angegebenen Parametern.
|
|
pub fn new(fc_min: f32, beta: f32, d_cutoff: f32) -> Self {
|
|
Self {
|
|
fc_min,
|
|
beta,
|
|
d_cutoff,
|
|
x_prev: None,
|
|
dx_prev: 0.0,
|
|
t_prev: None,
|
|
}
|
|
}
|
|
|
|
/// Setzt den Filterzustand vollständig zurück.
|
|
pub fn reset(&mut self) {
|
|
self.x_prev = None;
|
|
self.dx_prev = 0.0;
|
|
self.t_prev = None;
|
|
}
|
|
|
|
/// Gibt zurück, ob der Filter bereits mindestens einen Messwert empfangen hat.
|
|
pub fn is_initialized(&self) -> bool {
|
|
self.x_prev.is_some()
|
|
}
|
|
|
|
/// Letzter gefilterter Wert, falls vorhanden.
|
|
pub fn last_value(&self) -> Option<f32> {
|
|
self.x_prev
|
|
}
|
|
|
|
/// Filtert einen Wert `x` zum Zeitstempel `t` (in Sekunden).
|
|
pub fn filter(&mut self, x: f32, t: f64) -> f32 {
|
|
// Schutz vor NaN- und Inf-Eingaben
|
|
if !x.is_finite() || !t.is_finite() {
|
|
return self.x_prev.unwrap_or(0.0);
|
|
}
|
|
|
|
let Some(prev_t) = self.t_prev else {
|
|
// Erster Messwert: Initialisierung
|
|
self.x_prev = Some(x);
|
|
self.dx_prev = 0.0;
|
|
self.t_prev = Some(t);
|
|
return x;
|
|
};
|
|
|
|
let raw_dt = t - prev_t;
|
|
|
|
// Numerische Härtung: dt > 1.0 s führt zu reset()
|
|
if raw_dt > 1.0 {
|
|
self.reset();
|
|
self.x_prev = Some(x);
|
|
self.dx_prev = 0.0;
|
|
self.t_prev = Some(t);
|
|
return x;
|
|
}
|
|
|
|
// Numerische Härtung: dt <= 0 wird mit dt = 1e-4 s abgefangen
|
|
let dt = if raw_dt <= 0.0 || !raw_dt.is_finite() {
|
|
1e-4_f32
|
|
} else {
|
|
raw_dt as f32
|
|
};
|
|
|
|
self.t_prev = Some(t);
|
|
|
|
let prev_x = self.x_prev.unwrap_or(x);
|
|
|
|
// 1. Ableitung (Geschwindigkeit) filtern
|
|
let mut dx = (x - prev_x) / dt;
|
|
if !dx.is_finite() {
|
|
dx = 0.0;
|
|
}
|
|
|
|
let a_d = smoothing_factor(dt, self.d_cutoff);
|
|
let mut dx_hat = a_d * dx + (1.0 - a_d) * self.dx_prev;
|
|
if !dx_hat.is_finite() {
|
|
dx_hat = self.dx_prev;
|
|
}
|
|
|
|
// 2. Adaptive Grenzfrequenz fc berechnen
|
|
let mut fc = self.fc_min + self.beta * dx_hat.abs();
|
|
if !fc.is_finite() || fc < self.fc_min {
|
|
fc = self.fc_min;
|
|
}
|
|
|
|
// 3. Signal mit adaptivem fc filtern
|
|
let a = smoothing_factor(dt, fc);
|
|
let mut x_hat = a * x + (1.0 - a) * prev_x;
|
|
if !x_hat.is_finite() {
|
|
x_hat = prev_x;
|
|
}
|
|
|
|
self.x_prev = Some(x_hat);
|
|
self.dx_prev = dx_hat;
|
|
|
|
x_hat
|
|
}
|
|
}
|
|
|
|
impl Default for OneEuroFilter1D {
|
|
fn default() -> Self {
|
|
Self::new(DEFAULT_FC_MIN, DEFAULT_BETA, DEFAULT_D_CUTOFF)
|
|
}
|
|
}
|
|
|
|
/// Gefilterter Eingabepunkt mit 2D-Position, Druck und Zeitstempel.
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct FilteredPoint {
|
|
pub position: glam::Vec2,
|
|
pub pressure: f32,
|
|
pub timestamp: f64,
|
|
}
|
|
|
|
/// Adaptiver 1€-Filter für 2D-Position (x, y) und Stylus-Druck p ∈ [0.0, 1.0].
|
|
#[derive(Debug, Clone)]
|
|
pub struct OneEuroFilter {
|
|
pub fc_min: f32,
|
|
pub beta: f32,
|
|
pub d_cutoff: f32,
|
|
x_filter: OneEuroFilter1D,
|
|
y_filter: OneEuroFilter1D,
|
|
p_filter: OneEuroFilter1D,
|
|
last_output: Option<(f32, f32, f32)>,
|
|
}
|
|
|
|
impl OneEuroFilter {
|
|
/// Erstellt einen neuen OneEuroFilter mit den angegebenen Parametern.
|
|
pub fn new(fc_min: f32, beta: f32, d_cutoff: f32) -> Self {
|
|
Self {
|
|
fc_min,
|
|
beta,
|
|
d_cutoff,
|
|
x_filter: OneEuroFilter1D::new(fc_min, beta, d_cutoff),
|
|
y_filter: OneEuroFilter1D::new(fc_min, beta, d_cutoff),
|
|
p_filter: OneEuroFilter1D::new(fc_min, beta, d_cutoff),
|
|
last_output: None,
|
|
}
|
|
}
|
|
|
|
/// Setzt den Filter vollständig zurück (z. B. bei neuem Stroke).
|
|
pub fn reset(&mut self) {
|
|
self.x_filter.reset();
|
|
self.y_filter.reset();
|
|
self.p_filter.reset();
|
|
self.last_output = None;
|
|
}
|
|
|
|
/// Filtert x, y und Druck p ∈ [0.0, 1.0] zum Zeitstempel `t` (in Sekunden).
|
|
///
|
|
/// Gibt das gefilterte Tupel `(x, y, pressure)` zurück.
|
|
pub fn filter(&mut self, x: f32, y: f32, pressure: f32, t: f64) -> (f32, f32, f32) {
|
|
// Schutz vor NaN- und Inf-Eingaben
|
|
if !x.is_finite() || !y.is_finite() || !pressure.is_finite() || !t.is_finite() {
|
|
return self.last_output.unwrap_or((0.0, 0.0, 0.0));
|
|
}
|
|
|
|
// Druck immer auf [0.0, 1.0] klemmen
|
|
let p_in = pressure.clamp(0.0, 1.0);
|
|
|
|
let x_out = self.x_filter.filter(x, t);
|
|
let y_out = self.y_filter.filter(y, t);
|
|
let p_out = self.p_filter.filter(p_in, t).clamp(0.0, 1.0);
|
|
|
|
let out = (x_out, y_out, p_out);
|
|
self.last_output = Some(out);
|
|
out
|
|
}
|
|
|
|
/// Filtert einen 2D-Punkt `pos` und Druck `pressure` zum Zeitstempel `t`.
|
|
pub fn filter_point(&mut self, pos: glam::Vec2, pressure: f32, t: f64) -> (glam::Vec2, f32) {
|
|
let (x, y, p) = self.filter(pos.x, pos.y, pressure, t);
|
|
(glam::Vec2::new(x, y), p)
|
|
}
|
|
|
|
/// Filtert einen `FilteredPoint` und gibt den geglätteten Punkt zurück.
|
|
pub fn filter_stroke_point(&mut self, point: FilteredPoint) -> FilteredPoint {
|
|
let (pos, p) = self.filter_point(point.position, point.pressure, point.timestamp);
|
|
FilteredPoint {
|
|
position: pos,
|
|
pressure: p,
|
|
timestamp: point.timestamp,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Default for OneEuroFilter {
|
|
fn default() -> Self {
|
|
Self::new(DEFAULT_FC_MIN, DEFAULT_BETA, DEFAULT_D_CUTOFF)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_zero_and_negative_dt() {
|
|
let mut filter = OneEuroFilter::default();
|
|
|
|
// 1. Erster gültiger Datenpunkt bei t = 1.0
|
|
let (x1, y1, p1) = filter.filter(10.0, 20.0, 0.5, 1.0);
|
|
assert_eq!(x1, 10.0);
|
|
assert_eq!(y1, 20.0);
|
|
assert_eq!(p1, 0.5);
|
|
|
|
// 2. dt == 0 (gleicher Zeitstempel t = 1.0): Sollte dt = 1e-4 abfangen
|
|
let (x2, y2, p2) = filter.filter(10.5, 20.5, 0.6, 1.0);
|
|
assert!(x2.is_finite(), "x2 muss endlich sein");
|
|
assert!(y2.is_finite(), "y2 muss endlich sein");
|
|
assert!(p2.is_finite(), "p2 muss endlich sein");
|
|
assert!((0.0..=1.0).contains(&p2), "Druck muss in [0, 1] liegen");
|
|
|
|
// 3. dt < 0 (Zeitstempel springt zurück auf t = 0.5): Sollte mit dt = 1e-4 abgefangen werden
|
|
let (x3, y3, p3) = filter.filter(11.0, 21.0, 0.7, 0.5);
|
|
assert!(x3.is_finite(), "x3 muss nach negativem dt endlich sein");
|
|
assert!(y3.is_finite(), "y3 muss nach negativem dt endlich sein");
|
|
assert!(p3.is_finite(), "p3 muss nach negativem dt endlich sein");
|
|
assert!((0.0..=1.0).contains(&p3));
|
|
|
|
// 4. dt > 1.0 s (großer Zeitsprung z.B. auf t = 5.0 s): Muss reset() auslösen
|
|
let (x4, y4, p4) = filter.filter(50.0, 100.0, 0.9, 5.0);
|
|
// Nach dem Reset muss der Filter sofort exakt den neuen Punkt als Startpunkt annehmen
|
|
assert_eq!(
|
|
x4, 50.0,
|
|
"Nach Reset muss der neue X-Wert übernommen werden"
|
|
);
|
|
assert_eq!(
|
|
y4, 100.0,
|
|
"Nach Reset muss der neue Y-Wert übernommen werden"
|
|
);
|
|
assert_eq!(
|
|
p4, 0.9,
|
|
"Nach Reset muss der neue Druckwert übernommen werden"
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn test_high_frequency_jitter() {
|
|
let mut filter = OneEuroFilter::default();
|
|
|
|
let base_x = 100.0_f32;
|
|
let base_y = 50.0_f32;
|
|
let base_p = 0.5_f32;
|
|
let jitter_amplitude = 2.0_f32;
|
|
|
|
let num_samples = 120;
|
|
let dt = 1.0 / 120.0; // 120 Hz Abtastrate
|
|
|
|
let mut raw_x_values = Vec::with_capacity(num_samples);
|
|
let mut filtered_x_values = Vec::with_capacity(num_samples);
|
|
|
|
for i in 0..num_samples {
|
|
let t = i as f64 * dt;
|
|
// Hochfrequentes Rauschen mit Vorzeichenwechsel
|
|
let sign = if i % 2 == 0 { 1.0 } else { -1.0 };
|
|
let raw_x = base_x + sign * jitter_amplitude;
|
|
let raw_y = base_y + sign * jitter_amplitude;
|
|
let raw_p = base_p + sign * 0.1;
|
|
|
|
raw_x_values.push(raw_x);
|
|
|
|
let (fx, fy, fp) = filter.filter(raw_x, raw_y, raw_p, t);
|
|
filtered_x_values.push(fx);
|
|
|
|
assert!(fx.is_finite());
|
|
assert!(fy.is_finite());
|
|
assert!(fp.is_finite());
|
|
assert!((0.0..=1.0).contains(&fp));
|
|
}
|
|
|
|
// Varianz nach der Einschwingphase (letzte 60 Samples) berechnen
|
|
let eval_range = 60..num_samples;
|
|
let count = eval_range.len() as f32;
|
|
|
|
let raw_mean: f32 = raw_x_values[eval_range.clone()].iter().sum::<f32>() / count;
|
|
let raw_variance: f32 = raw_x_values[eval_range.clone()]
|
|
.iter()
|
|
.map(|v| (v - raw_mean).powi(2))
|
|
.sum::<f32>()
|
|
/ count;
|
|
|
|
let filtered_mean: f32 = filtered_x_values[eval_range.clone()].iter().sum::<f32>() / count;
|
|
let filtered_variance: f32 = filtered_x_values[eval_range]
|
|
.iter()
|
|
.map(|v| (v - filtered_mean).powi(2))
|
|
.sum::<f32>()
|
|
/ count;
|
|
|
|
// Der 1€-Filter muss das hochfrequente Jitter massiv dämpfen
|
|
assert!(
|
|
filtered_variance < raw_variance * 0.05,
|
|
"Gefilterte Varianz ({filtered_variance}) muss deutlich kleiner sein als Raw-Varianz ({raw_variance})"
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn test_nan_inf_poisoning() {
|
|
let mut filter = OneEuroFilter::default();
|
|
|
|
// 1. Zuerst normale Werte einspeisen
|
|
let (x0, y0, p0) = filter.filter(10.0, 20.0, 0.5, 0.0);
|
|
assert_eq!((x0, y0, p0), (10.0, 20.0, 0.5));
|
|
|
|
let (x1, y1, p1) = filter.filter(11.0, 21.0, 0.55, 0.01);
|
|
assert!(x1.is_finite());
|
|
assert!(y1.is_finite());
|
|
assert!(p1.is_finite());
|
|
|
|
// 2. Vergiftungsversuch mit NaN und Infinity
|
|
let (x_nan, y_nan, p_nan) = filter.filter(f32::NAN, f32::INFINITY, f32::NEG_INFINITY, 0.02);
|
|
assert!(
|
|
x_nan.is_finite(),
|
|
"Filter darf kein NaN bei Vergiftung ausgeben"
|
|
);
|
|
assert!(
|
|
y_nan.is_finite(),
|
|
"Filter darf kein Inf bei Vergiftung ausgeben"
|
|
);
|
|
assert!(
|
|
p_nan.is_finite(),
|
|
"Filter darf kein -Inf bei Vergiftung ausgeben"
|
|
);
|
|
|
|
let (x_nan2, y_nan2, p_nan2) = filter.filter(12.0, 22.0, 0.6, f64::NAN);
|
|
assert!(x_nan2.is_finite());
|
|
assert!(y_nan2.is_finite());
|
|
assert!(p_nan2.is_finite());
|
|
|
|
// 3. Nach der Vergiftung wieder normale Werte einspeisen
|
|
let (x2, y2, p2) = filter.filter(13.0, 23.0, 0.65, 0.03);
|
|
assert!(x2.is_finite(), "x2 muss nach Vergiftung endlich sein");
|
|
assert!(y2.is_finite(), "y2 muss nach Vergiftung endlich sein");
|
|
assert!(p2.is_finite(), "p2 muss nach Vergiftung endlich sein");
|
|
|
|
// Sicherstellen, dass der Filter nicht bei einem fixen Wert 'steckenbleibt'
|
|
let (x3, y3, p3) = filter.filter(30.0, 40.0, 0.8, 0.04);
|
|
assert!(x3.is_finite());
|
|
assert!(y3.is_finite());
|
|
assert!(p3.is_finite());
|
|
assert!(
|
|
x3 > x2,
|
|
"Filter muss sich nach Vergiftung weiter normal anpassen"
|
|
);
|
|
}
|
|
}
|