c5866daReimplement the algorithm in MATLAB syntax, run it with numblJeremy Magland 1function [Qdd, Xdd, force, helmholtzHz, contactMs, peakForceN, maxRealLam] = woodblock_solve(P, basis, oversample)
2% WOODBLOCK_SOLVE Integrate the coupled mallet / body / cavity system.
3%
4% The system is nonlinear only while the mallet is in contact (a fraction of
5% a millisecond), so the solve is split in two:
6% contact phase: RK4 on the full nonlinear system, until the mallet has
7% rebounded far enough that no future contact is possible;
8% free phase: the remaining coupled body+cavity system is linear, so it is
9% evolved in closed form from one eigendecomposition of the (2N+2)
10% system matrix, evaluated vectorized over the whole time axis.
11% This departs from the reference woodblock.py, which runs RK4 throughout;
12% the free-phase eigen solution is exact in time, so the two agree to the
13% reference's own integration tolerance.
14%
15% Returns modal accelerations Qdd (N x T), slit-plug acceleration Xdd (1 x T),
16% contact force (1 x T), and scalar diagnostics. maxRealLam should be
17% strictly negative; anything else means the coupling was assembled wrongly.
19N = basis.n;
20fsHi = P.sample_rate * oversample;
21dt = 1 / fsHi;
22nSteps = floor(P.duration_s * fsHi);
24om = basis.omega;
25om2 = om.^2;
26tzo = 2 * basis.zeta .* om;
27phiC = basis.phiC;
28A = basis.A;
30rho0 = P.air_density; c = P.speed_of_sound;
31V = P.cavity_volume_m3; S = P.slit_area_m2;
32% End correction on the slit: ~0.85 * equivalent radius per open end.
33Leff = P.slit_length_m + 2 * 0.85 * sqrt(S / pi);
34omH = c * sqrt(S / (V * Leff));
35tzoH = 2 * P.cavity_damping * omH;
36pScale = rho0 * c^2 / V;
37cavDrive = c^2 / (V * Leff);
38K = P.mallet_stiffness;
39alpha = P.contact_exponent;
40mu = P.contact_damping;
41invMh = 1 / P.mallet_mass;
43% ---- contact phase: RK4 on [q; qd; xi; xid; y; yd] ----
44maxContact = min(nSteps, ceil(0.02 * fsHi)); % 20 ms hard cap
45z = zeros(2 * N + 4, 1);
46z(2 * N + 4) = P.mallet_velocity;
47Qc = zeros(N, maxContact);
48Xc = zeros(1, maxContact);
49forceC = zeros(1, maxContact);
50Tc = 0;
51for k = 1:maxContact
52 [d1, qdd1, xidd1, fc1] = wb_deriv(z, N, om2, tzo, phiC, A, S, pScale, omH, tzoH, cavDrive, K, alpha, mu, invMh);
53 Qc(:, k) = qdd1;
54 Xc(k) = xidd1;
55 forceC(k) = fc1;
56 Tc = k;
57 [d2, ~, ~, ~] = wb_deriv(z + 0.5 * dt * d1, N, om2, tzo, phiC, A, S, pScale, omH, tzoH, cavDrive, K, alpha, mu, invMh);
58 [d3, ~, ~, ~] = wb_deriv(z + 0.5 * dt * d2, N, om2, tzo, phiC, A, S, pScale, omH, tzoH, cavDrive, K, alpha, mu, invMh);
59 [d4, ~, ~, ~] = wb_deriv(z + dt * d3, N, om2, tzo, phiC, A, S, pScale, omH, tzoH, cavDrive, K, alpha, mu, invMh);
60 z = z + (dt / 6) * (d1 + 2 * d2 + 2 * d3 + d4);
62 % No-recontact exit: after separation the mallet coasts at constant
63 % velocity while the surface stays within B (a conservative bound from
64 % the current modal energy), so once the mallet is receding and further
65 % than 1.5*B below the surface rest plane, contact can never resume.
66 y = z(2 * N + 3);
67 yd = z(2 * N + 4);
68 if fc1 == 0 && yd <= 0
69 q = z(1:N);
70 qd = z(N + 1:2 * N);
71 B = sum(abs(phiC) .* sqrt(q.^2 + (qd ./ om).^2));
72 if y <= -1.5 * B
73 break;
74 end
75 end
76end
78% ---- free phase: eigendecomposition of the linear body+cavity system ----
79% state x = [q; qd; xi; xid], xdot = MM x
80T2 = nSteps - Tc;
81MM = zeros(2 * N + 2);
82MM(1:N, N + 1:2 * N) = eye(N);
83MM(N + 1:2 * N, 1:N) = -(diag(om2) + pScale * (A * A.'));
84MM(N + 1:2 * N, N + 1:2 * N) = -diag(tzo);
85MM(N + 1:2 * N, 2 * N + 1) = -pScale * S * A;
86MM(2 * N + 1, 2 * N + 2) = 1;
87MM(2 * N + 2, 1:N) = -cavDrive * A.';
88MM(2 * N + 2, 2 * N + 1) = -omH^2;
89MM(2 * N + 2, 2 * N + 2) = -tzoH;
91[VV, DD] = eig(MM);
92lam = diag(DD);
93maxRealLam = max(real(lam));
94cvec = VV \ z(1:2 * N + 2);
95w = lam .* cvec; % acceleration weights per eigenmode
96t2 = (0:T2 - 1) * dt; % t2 = 0 is the state right after contact
97E = exp(lam * t2); % (2N+2) x T2, complex
98Qf = real((VV(N + 1:2 * N, :) * diag(w)) * E);
99Xf = real((VV(2 * N + 2, :) * diag(w)) * E);
101Qdd = [Qc(:, 1:Tc) Qf];
102Xdd = [Xc(1:Tc) Xf];
103force = [forceC(1:Tc) zeros(1, T2)];
105helmholtzHz = omH / (2 * pi);
106contactMs = sum(force > 0) * dt * 1e3;
107peakForceN = max(force);
108end
110function [d, qdd, xidd, fc] = wb_deriv(z, N, om2, tzo, phiC, A, S, pScale, omH, tzoH, cavDrive, K, alpha, mu, invMh)
111% Derivative of the coupled system with Hunt-Crossley mallet contact.
112q = z(1:N);
113qd = z(N + 1:2 * N);
114xi = z(2 * N + 1);
115xid = z(2 * N + 2);
116y = z(2 * N + 3);
117yd = z(2 * N + 4);
119wsurf = phiC.' * q;
120wdsurf = phiC.' * qd;
121delta = y - wsurf;
122if delta > 0
123 % f = K d^a (1 + lambda * ddot); lambda in s/m keeps the correction O(1)
124 fc = K * delta^alpha * (1 + mu * (yd - wdsurf));
125 if fc < 0
126 fc = 0;
127 end
128else
129 fc = 0;
130end
132Aq = A.' * q;
133pc = pScale * (S * xi + Aq);
135qdd = -tzo .* qd - om2 .* q + fc * phiC - A * pc;
136xidd = -tzoH * xid - omH^2 * xi - cavDrive * Aq;
138d = zeros(2 * N + 4, 1);
139d(1:N) = qd;
140d(N + 1:2 * N) = qdd;
141d(2 * N + 1) = xid;
142d(2 * N + 2) = xidd;
143d(2 * N + 3) = yd;
144d(2 * N + 4) = -fc * invMh;
145end