143 lines
3.3 KiB
Rust
143 lines
3.3 KiB
Rust
pub struct Hysteresis {
|
|
fs: f32,
|
|
t: f32,
|
|
|
|
m_s: f32,
|
|
a: f32,
|
|
c: f32,
|
|
|
|
// Save calculations
|
|
nc: f32,
|
|
m_s_oa: f32,
|
|
m_s_oa_tc: f32,
|
|
m_s_oa_tc_talpha: f32,
|
|
|
|
// state variables
|
|
m_n1: f32,
|
|
h_n1: f32,
|
|
h_d_n1: f32,
|
|
}
|
|
|
|
impl Hysteresis {
|
|
const ALPHA: f32 = 1.6e-3;
|
|
const D_ALPHA: f32 = 0.9;
|
|
const UPPER_LIM: f32 = 20.0;
|
|
const K: f32 = 0.47875;
|
|
|
|
pub fn new() -> Self {
|
|
let m_s = 1.0;
|
|
let a = m_s / 4.0;
|
|
let c = 1.7e-1;
|
|
|
|
let fs = 48000.0;
|
|
let t = 1.0 / fs;
|
|
Self {
|
|
fs,
|
|
t,
|
|
|
|
m_s,
|
|
a,
|
|
c,
|
|
|
|
// Save calculations
|
|
nc: 1.0 - c,
|
|
m_s_oa: m_s / a,
|
|
m_s_oa_tc: c * m_s / a,
|
|
m_s_oa_tc_talpha: Self::ALPHA * c * m_s / a,
|
|
|
|
// state variables
|
|
m_n1: 0.0,
|
|
h_n1: 0.0,
|
|
h_d_n1: 0.0,
|
|
}
|
|
}
|
|
|
|
pub fn set_sample_rate(&mut self, sample_rate: f32) {
|
|
// Sample rate
|
|
self.fs = sample_rate;
|
|
self.t = 1.0 / self.fs;
|
|
}
|
|
|
|
pub fn process(&mut self, drive: f32, width: f32, sat: f32, h: f32) -> f32 {
|
|
self.m_s = 0.5 + 1.5 * (1.0 - sat);
|
|
self.a = self.m_s / (0.01 + 6.0 * drive);
|
|
self.c = (1.0 - width).sqrt() - 0.01;
|
|
|
|
self.nc = 1.0 - self.c;
|
|
self.m_s_oa = self.m_s / self.a;
|
|
self.m_s_oa_tc = self.c * self.m_s_oa;
|
|
self.m_s_oa_tc_talpha = Self::ALPHA * self.m_s_oa_tc;
|
|
|
|
let h_d = self.deriv(h, self.h_n1, self.h_d_n1);
|
|
let mut m = self.solver(h, h_d);
|
|
|
|
if m.is_nan() || m > Self::UPPER_LIM {
|
|
m = 0.0;
|
|
}
|
|
|
|
self.m_n1 = m;
|
|
self.m_n1 = h;
|
|
self.h_d_n1 = h_d;
|
|
|
|
m
|
|
}
|
|
|
|
fn deriv(&self, x_n: f32, x_n1: f32, x_d_n1: f32) -> f32 {
|
|
(((1.0 + Self::D_ALPHA) / self.t) * (x_n - x_n1)) - Self::D_ALPHA * x_d_n1
|
|
}
|
|
|
|
fn solver(&self, h: f32, h_d: f32) -> f32 {
|
|
// Corresponds to RK2
|
|
let k1 = self.t * self.hysteresis_func(self.m_n1, self.h_n1, self.h_d_n1);
|
|
let k2 = self.t
|
|
* self.hysteresis_func(
|
|
self.m_n1 + (k1 / 2.0),
|
|
(h + self.h_n1) / 2.0,
|
|
(h_d + self.h_d_n1) / 2.0,
|
|
);
|
|
|
|
self.m_n1 + k2
|
|
}
|
|
|
|
fn hysteresis_func(&self, m: f32, h: f32, h_d: f32) -> f32 {
|
|
let q = (h + Self::ALPHA * m) / self.a;
|
|
let coth = 1.0 / q.tanh();
|
|
let near_zero = q.abs() < 0.001;
|
|
|
|
let m_diff = self.m_s * langevin(coth, q, near_zero) - m;
|
|
|
|
let delta: f32 = if h_d >= 0.0 { 1.0 } else { -1.0 };
|
|
let delta_m = if delta.signum() == m_diff.signum() {
|
|
1.0
|
|
} else {
|
|
0.0
|
|
};
|
|
|
|
let l_prime = langevin_d(coth, q, near_zero);
|
|
|
|
let kap1 = self.nc * delta_m;
|
|
let f1_denom = self.nc * delta * Self::K - Self::ALPHA * m_diff;
|
|
let f1 = kap1 * m_diff / f1_denom;
|
|
let f2 = self.m_s_oa_tc * l_prime;
|
|
let f3 = 1.0 - (self.m_s_oa_tc_talpha * l_prime);
|
|
|
|
h_d * (f1 + f2) / f3
|
|
}
|
|
}
|
|
|
|
fn langevin(coth: f32, x: f32, near_zero: bool) -> f32 {
|
|
if near_zero {
|
|
x / 3.0
|
|
} else {
|
|
coth - (1.0 / x)
|
|
}
|
|
}
|
|
|
|
fn langevin_d(coth: f32, x: f32, near_zero: bool) -> f32 {
|
|
if near_zero {
|
|
1.0 / 3.0
|
|
} else {
|
|
(1.0 / (x * x)) - (coth * coth) + 1.0
|
|
}
|
|
}
|