sva_samples/physics/
willemsen_bilbao_serafin.rs

1// Concern: one bowed-string call site's finite-difference state | Non-concern: argument evaluation, builtin dispatch | IO: (params, sr) -> a site; () -> a sample
2
3//! Elasto-plastic friction: Dupont/Hayward/Armstrong/Altpeter, IEEE TAC 47(5) (2002). First
4//! bowed use: Serafin/Avanzini/Rocchesso, SMAC-03. Corrected eqs. (7)-(9)/FD coupling:
5//! Willemsen/Bilbao/Serafin, DAFx-19 pp. 40-46. Reuses `stiff_string`'s FD string.
6
7use crate::error::SampleError;
8use crate::physics::Solver;
9
10use crate::physics::bound::Bound::*;
11use crate::physics::bound::all;
12use crate::physics::stiff_string::{
13    StringGrid, Wire, dispersive_grid, grid_tension, point_weights, read_at, spread, stencil_update,
14};
15
16#[derive(Clone, Debug, PartialEq)]
17pub struct WillemsenBilbaoSerafinParams {
18    pub f0: f64,
19    pub b: f64,
20    pub bow_pos: f64,
21    pub bow_vel: f64,
22    pub bow_force: f64,
23    pub mu_s: f64,
24    pub mu_c: f64,
25    pub stribeck_vel: f64,
26    pub bristle_stiffness: f64,
27    pub bristle_damping: f64,
28    pub viscous_friction: f64,
29    pub damp_dc: f64,
30    pub damp_freq: f64,
31}
32
33impl WillemsenBilbaoSerafinParams {
34    /// DAFx-19 Table 1's reference values, at the fundamental asked for.
35    pub fn at(f0: f64) -> WillemsenBilbaoSerafinParams {
36        WillemsenBilbaoSerafinParams {
37            f0,
38            b: 1e-4,
39            bow_pos: 0.25,
40            bow_vel: 0.1,
41            bow_force: 10.0,
42            mu_s: 0.8,
43            mu_c: 0.3,
44            stribeck_vel: 0.1,
45            bristle_stiffness: 1e4,
46            bristle_damping: 0.1,
47            viscous_friction: 0.4,
48            damp_dc: 1.0,
49            damp_freq: 5e-3,
50        }
51    }
52}
53
54impl WillemsenBilbaoSerafinParams {
55    pub fn valid(&self) -> bool {
56        all(&[
57            (self.f0, Positive),
58            (self.b, NonNegative),
59            (self.bow_pos, OpenUnit),
60            (self.bow_vel, Finite),
61            (self.bow_force, Positive),
62            (self.mu_s, Positive),
63            (self.mu_c, Positive),
64            (self.stribeck_vel, Positive),
65            (self.bristle_stiffness, Positive),
66            (self.bristle_damping, NonNegative),
67            (self.viscous_friction, NonNegative),
68            (self.damp_dc, NonNegative),
69            (self.damp_freq, NonNegative),
70        ])
71    }
72}
73
74const WIRE_DENSITY_KG_M3: f64 = 7850.0;
75const WIRE_RADIUS_M: f64 = 5e-4;
76/// Fixed per Table 1; `c = 2 f0 L` scales instead (`chaigne_askenfelt` inverts that).
77const WIRE_LENGTH_M: f64 = 1.0;
78const NR_MAX_ITERATIONS: usize = 50;
79const NR_TOLERANCE: f64 = 1e-7;
80
81/// Table 1's wire at `c = 2 f0 L`.
82fn build_grid(params: &WillemsenBilbaoSerafinParams, sr: f64) -> Option<StringGrid> {
83    let rho = std::f64::consts::PI * WIRE_RADIUS_M * WIRE_RADIUS_M * WIRE_DENSITY_KG_M3;
84    let wire = Wire {
85        rho,
86        c: 2.0 * params.f0 * WIRE_LENGTH_M,
87        length: WIRE_LENGTH_M,
88    };
89    dispersive_grid(
90        wire,
91        params.f0,
92        params.b,
93        params.damp_dc,
94        params.damp_freq,
95        sr,
96    )
97}
98
99fn sgn(x: f64) -> f64 {
100    if x > 0.0 {
101        1.0
102    } else if x < 0.0 {
103        -1.0
104    } else {
105        0.0
106    }
107}
108
109/// Eq. (7): `abs()` around `z_ss`, missing in the pre-DAFx-19 literature.
110fn steady_state(v: f64, s0: f64, f_c: f64, f_s: f64, stribeck_vel: f64) -> f64 {
111    sgn(v) / s0 * (f_c + (f_s - f_c) * (-(v / stribeck_vel).powi(2)).exp())
112}
113
114/// Eq. (8)-(9): `sgn(z)` on the sine offset; `|z|=z_ba`/`|z_ss|` resolved, not undefined.
115fn adhesion(v: f64, z: f64, z_ba: f64, zss: f64) -> f64 {
116    if sgn(v) != sgn(z) {
117        return 0.0;
118    }
119    let (az, azss) = (z.abs(), zss.abs());
120    if az <= z_ba {
121        0.0
122    } else if az >= azss {
123        1.0
124    } else {
125        let span = (azss - z_ba).max(1e-300);
126        let s = sgn(z);
127        0.5 * (1.0 + s * (std::f64::consts::PI * (z - s * 0.5 * (azss + z_ba)) / span).sin())
128    }
129}
130
131/// Eq. (6)/(15). `v == 0` avoids dividing by `z_ss(0) = 0`.
132fn bristle_rate(v: f64, z: f64, s0: f64, f_c: f64, f_s: f64, stribeck_vel: f64, z_ba: f64) -> f64 {
133    if v == 0.0 {
134        return 0.0;
135    }
136    let zss = steady_state(v, s0, f_c, f_s, stribeck_vel);
137    let a = adhesion(v, z, z_ba, zss);
138    v * (1.0 - a * z / zss)
139}
140
141/// Eq. (4), noise term `s3*w` dropped (disclosed v1 gap).
142fn friction_force(v: f64, z: f64, r: f64, s0: f64, s1: f64, s2: f64) -> f64 {
143    s0 * z + s1 * r + s2 * v
144}
145
146#[derive(Clone)]
147struct Coupling {
148    coeff: f64,
149    b_known: f64,
150    s0: f64,
151    s1: f64,
152    s2: f64,
153    f_c: f64,
154    f_s: f64,
155    stribeck_vel: f64,
156    z_ba: f64,
157    z_prev: f64,
158    r_prev: f64,
159    dt: f64,
160}
161
162impl Coupling {
163    /// `g1` re-derives eq. (17)-(19): theirs assume centred damping, the FD string's is
164    /// backward. `g2` keeps their eq. (20)-(21).
165    fn residual(&self, v: f64, z: f64) -> (f64, f64) {
166        let r = bristle_rate(
167            v,
168            z,
169            self.s0,
170            self.f_c,
171            self.f_s,
172            self.stribeck_vel,
173            self.z_ba,
174        );
175        let f = friction_force(v, z, r, self.s0, self.s1, self.s2);
176        let g1 = v + self.coeff * f - self.b_known;
177        let a = 2.0 * (z - self.z_prev) / self.dt - self.r_prev;
178        (g1, r - a)
179    }
180
181    /// A central-difference Jacobian, not Algorithm 1's analytic one.
182    fn newton_step(&self, v: f64, z: f64) -> Option<(f64, f64)> {
183        let (g1, g2) = self.residual(v, z);
184        let hv = v.abs().max(1e-3) * 1e-6;
185        let hz = z.abs().max(self.z_ba).max(1e-9) * 1e-6;
186        let (g1_vp, g2_vp) = self.residual(v + hv, z);
187        let (g1_vm, g2_vm) = self.residual(v - hv, z);
188        let (g1_zp, g2_zp) = self.residual(v, z + hz);
189        let (g1_zm, g2_zm) = self.residual(v, z - hz);
190        let dg1_dv = (g1_vp - g1_vm) / (2.0 * hv);
191        let dg2_dv = (g2_vp - g2_vm) / (2.0 * hv);
192        let dg1_dz = (g1_zp - g1_zm) / (2.0 * hz);
193        let dg2_dz = (g2_zp - g2_zm) / (2.0 * hz);
194        let det = dg1_dv * dg2_dz - dg1_dz * dg2_dv;
195        if !det.is_finite() || det.abs() < 1e-300 {
196            return None;
197        }
198        let dv = (g1 * dg2_dz - g2 * dg1_dz) / det;
199        let dz = (dg1_dv * g2 - dg2_dv * g1) / det;
200        let (new_v, new_z) = (v - dv, z - dz);
201        if !new_v.is_finite() || !new_z.is_finite() {
202            return None;
203        }
204        Some((new_v, new_z))
205    }
206}
207
208#[derive(Clone)]
209pub struct WillemsenBilbaoSerafinSite {
210    strings: Vec<StringGrid>,
211    /// Read and spread alike.
212    bow: Vec<f64>,
213    bow_self: f64,
214    tension: f64,
215    dt: f64,
216    bow_vel: f64,
217    f_c: f64,
218    f_s: f64,
219    stribeck_vel: f64,
220    s0: f64,
221    s1: f64,
222    s2: f64,
223    z_ba: f64,
224    z: f64,
225    z_prev: f64,
226    /// `r^{n-1}`, eq. (20)-(21)'s extra trapezoidal state.
227    r_prev: f64,
228    last_v: f64,
229    last_f: f64,
230    /// Samples stepped, which a refusal names.
231    steps: usize,
232}
233
234impl WillemsenBilbaoSerafinSite {
235    pub fn new(params: &WillemsenBilbaoSerafinParams, sr: f64) -> Result<Self, SampleError> {
236        let grid = build_grid(params, sr).ok_or(SampleError::StringPastRate {
237            model: "willemsen_bilbao_serafin",
238        })?;
239        let bow = point_weights(grid.n, params.bow_pos);
240        let bow_self = read_at(&bow, &bow);
241        let tension = grid_tension(&grid, 1.0 / sr);
242        let f_c = params.mu_c * params.bow_force;
243        let f_s = params.mu_s * params.bow_force;
244        // Table 1: z_ba = 0.7*f_C/s0 — off the kinetic (mu_c) force, not the static one.
245        let z_ba = 0.7 * f_c / params.bristle_stiffness;
246        Ok(WillemsenBilbaoSerafinSite {
247            strings: vec![grid],
248            bow,
249            bow_self,
250            tension,
251            dt: 1.0 / sr,
252            bow_vel: params.bow_vel,
253            f_c,
254            f_s,
255            stribeck_vel: params.stribeck_vel,
256            s0: params.bristle_stiffness,
257            s1: params.bristle_damping,
258            s2: params.viscous_friction,
259            z_ba,
260            z: 0.0,
261            z_prev: 0.0,
262            r_prev: 0.0,
263            last_v: 0.0,
264            last_f: 0.0,
265            steps: 0,
266        })
267    }
268}
269
270impl Solver for WillemsenBilbaoSerafinSite {
271    fn step(&mut self) -> Result<f64, SampleError> {
272        let dt = self.dt;
273        let coupling_prev = (self.z, self.r_prev);
274
275        let grid = &mut self.strings[0];
276        let n = grid.n;
277        for i in 1..n {
278            grid.y_next[i] = stencil_update(grid, i, 0.0, 0.0);
279        }
280        grid.y_next[0] = 0.0;
281        grid.y_next[n] = 0.0;
282        let free_next = read_at(&self.bow, &grid.y_next);
283        let bow_prev = read_at(&self.bow, &grid.y_prev);
284
285        let coupling = Coupling {
286            coeff: dt * self.bow_self / (2.0 * grid.rho * grid.dx),
287            b_known: (free_next - bow_prev) / (2.0 * dt) - self.bow_vel,
288            s0: self.s0,
289            s1: self.s1,
290            s2: self.s2,
291            f_c: self.f_c,
292            f_s: self.f_s,
293            stribeck_vel: self.stribeck_vel,
294            z_ba: self.z_ba,
295            z_prev: coupling_prev.0,
296            r_prev: coupling_prev.1,
297            dt,
298        };
299
300        let (mut v, mut z) = (self.last_v, self.z);
301        let mut converged = false;
302        for _ in 0..NR_MAX_ITERATIONS {
303            let Some((new_v, new_z)) = coupling.newton_step(v, z) else {
304                break;
305            };
306            let step_norm = ((new_v - v).powi(2) + (new_z - z).powi(2)).sqrt();
307            v = new_v;
308            z = new_z;
309            if step_norm < NR_TOLERANCE {
310                converged = true;
311                break;
312            }
313        }
314        if !converged {
315            return Err(SampleError::ContactUnsettled {
316                model: "willemsen_bilbao_serafin",
317                sample: self.steps,
318            });
319        }
320        self.steps += 1;
321
322        let r = bristle_rate(
323            v,
324            z,
325            self.s0,
326            self.f_c,
327            self.f_s,
328            self.stribeck_vel,
329            self.z_ba,
330        );
331        let f = friction_force(v, z, r, self.s0, self.s1, self.s2);
332        let grid = &mut self.strings[0];
333        spread(grid, &self.bow, -f, dt);
334
335        self.z_prev = self.z;
336        self.z = z;
337        self.r_prev = r;
338        self.last_v = v;
339        self.last_f = f;
340
341        let sample = self.tension * (grid.y_now[n] - grid.y_now[n - 1]) / grid.dx;
342
343        std::mem::swap(&mut grid.y_prev, &mut grid.y_now);
344        std::mem::swap(&mut grid.y_now, &mut grid.y_next);
345
346        Ok(sample)
347    }
348}
sva_samples/physics/willemsen_bilbao_serafin.rs

sva_samples/physics/
willemsen_bilbao_serafin.rs
