265 lines
8.8 KiB
Rust
265 lines
8.8 KiB
Rust
//! Continuous phase-wheel analysis on the native-rate audio stream.
|
|
|
|
const HILBERT_TAPS: usize = 33;
|
|
const HILBERT_HALF: usize = (HILBERT_TAPS - 1) / 2;
|
|
const BANDPASS_LOW_HZ: f64 = 300.0;
|
|
const BANDPASS_HIGH_HZ: f64 = 5_000.0;
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct PhaseWheelSnapshot {
|
|
pub angle_rad: Option<f32>,
|
|
pub coherence: f32,
|
|
pub level: f32,
|
|
pub peak: f32,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
struct BandpassChannel {
|
|
hp_x: f64,
|
|
hp_y: f64,
|
|
lp_y: f64,
|
|
}
|
|
|
|
impl BandpassChannel {
|
|
fn process(&mut self, sample: f64, hp_alpha: f64, lp_alpha: f64) -> f64 {
|
|
let hp = hp_alpha * (self.hp_y + sample - self.hp_x);
|
|
self.hp_x = sample;
|
|
self.hp_y = hp;
|
|
self.lp_y = lp_alpha * hp + (1.0 - lp_alpha) * self.lp_y;
|
|
self.lp_y
|
|
}
|
|
}
|
|
|
|
pub struct PhaseWheelAnalyzer {
|
|
sample_rate: u32,
|
|
hp_alpha: f64,
|
|
lp_alpha: f64,
|
|
band_l: BandpassChannel,
|
|
band_r: BandpassChannel,
|
|
ring_l: [f64; HILBERT_TAPS],
|
|
ring_r: [f64; HILBERT_TAPS],
|
|
hilbert: [f64; HILBERT_TAPS],
|
|
write: usize,
|
|
fill: usize,
|
|
cross_re: f64,
|
|
cross_im: f64,
|
|
weight_sum: f64,
|
|
amplitude_sum: f64,
|
|
amplitude_peak: f64,
|
|
count: usize,
|
|
}
|
|
|
|
impl PhaseWheelAnalyzer {
|
|
pub fn new(sample_rate: u32) -> Self {
|
|
let mut analyzer = Self {
|
|
sample_rate: 0,
|
|
hp_alpha: 0.0,
|
|
lp_alpha: 0.0,
|
|
band_l: BandpassChannel::default(),
|
|
band_r: BandpassChannel::default(),
|
|
ring_l: [0.0; HILBERT_TAPS],
|
|
ring_r: [0.0; HILBERT_TAPS],
|
|
hilbert: build_hilbert_kernel(),
|
|
write: 0,
|
|
fill: 0,
|
|
cross_re: 0.0,
|
|
cross_im: 0.0,
|
|
weight_sum: 0.0,
|
|
amplitude_sum: 0.0,
|
|
amplitude_peak: 0.0,
|
|
count: 0,
|
|
};
|
|
analyzer.configure(sample_rate);
|
|
analyzer
|
|
}
|
|
|
|
pub fn configure(&mut self, sample_rate: u32) {
|
|
let rate = sample_rate.max(8_000);
|
|
if self.sample_rate == rate {
|
|
return;
|
|
}
|
|
self.sample_rate = rate;
|
|
self.hp_alpha = highpass_alpha(rate, BANDPASS_LOW_HZ);
|
|
self.lp_alpha = lowpass_alpha(rate, BANDPASS_HIGH_HZ);
|
|
self.band_l = BandpassChannel::default();
|
|
self.band_r = BandpassChannel::default();
|
|
self.ring_l.fill(0.0);
|
|
self.ring_r.fill(0.0);
|
|
self.write = 0;
|
|
self.fill = 0;
|
|
self.clear_accumulator();
|
|
}
|
|
|
|
pub fn process(&mut self, left: f32, right: f32) {
|
|
let filtered_l = self
|
|
.band_l
|
|
.process(left as f64, self.hp_alpha, self.lp_alpha);
|
|
let filtered_r = self
|
|
.band_r
|
|
.process(right as f64, self.hp_alpha, self.lp_alpha);
|
|
self.ring_l[self.write] = filtered_l;
|
|
self.ring_r[self.write] = filtered_r;
|
|
self.write = (self.write + 1) % HILBERT_TAPS;
|
|
self.fill = (self.fill + 1).min(HILBERT_TAPS);
|
|
if self.fill < HILBERT_TAPS {
|
|
return;
|
|
}
|
|
|
|
// `write` points at the oldest sample. The real component is delayed
|
|
// by half the FIR length, so it is aligned with the causal Hilbert FIR.
|
|
let real_index = (self.write + HILBERT_HALF) % HILBERT_TAPS;
|
|
let l_re = self.ring_l[real_index].clamp(-1.0, 1.0);
|
|
let r_re = self.ring_r[real_index].clamp(-1.0, 1.0);
|
|
let mut l_im = 0.0;
|
|
let mut r_im = 0.0;
|
|
// The ideal odd Hilbert kernel has zero coefficients at every even
|
|
// offset; with a 33-tap kernel those are the even tap indices.
|
|
for tap in (1..HILBERT_TAPS).step_by(2) {
|
|
let index = (self.write + tap) % HILBERT_TAPS;
|
|
l_im += self.ring_l[index] * self.hilbert[tap];
|
|
r_im += self.ring_r[index] * self.hilbert[tap];
|
|
}
|
|
|
|
let mag_l = l_re.hypot(l_im).min(1.0);
|
|
let mag_r = r_re.hypot(r_im).min(1.0);
|
|
let weight = mag_l * mag_r;
|
|
// zL * conj(zR): its argument is the energy-weighted L/R phase.
|
|
self.cross_re += l_re * r_re + l_im * r_im;
|
|
self.cross_im += l_im * r_re - l_re * r_im;
|
|
self.weight_sum += weight;
|
|
let amplitude = 0.5 * (mag_l + mag_r);
|
|
self.amplitude_sum += amplitude;
|
|
self.amplitude_peak = self.amplitude_peak.max(amplitude);
|
|
self.count += 1;
|
|
}
|
|
|
|
pub fn take_snapshot(&mut self) -> PhaseWheelSnapshot {
|
|
let level = if self.count > 0 {
|
|
(self.amplitude_sum / self.count as f64) as f32
|
|
} else {
|
|
0.0
|
|
};
|
|
let resultant = self.cross_re.hypot(self.cross_im);
|
|
let angle_rad = if self.weight_sum > 1e-12 && resultant > self.weight_sum * 1e-9 {
|
|
Some(self.cross_im.atan2(self.cross_re) as f32)
|
|
} else {
|
|
None
|
|
};
|
|
let coherence = if self.weight_sum > 1e-12 {
|
|
(resultant / self.weight_sum).clamp(0.0, 1.0) as f32
|
|
} else {
|
|
0.0
|
|
};
|
|
let snapshot = PhaseWheelSnapshot {
|
|
angle_rad,
|
|
coherence,
|
|
level,
|
|
peak: self.amplitude_peak as f32,
|
|
};
|
|
self.clear_accumulator();
|
|
snapshot
|
|
}
|
|
|
|
fn clear_accumulator(&mut self) {
|
|
self.cross_re = 0.0;
|
|
self.cross_im = 0.0;
|
|
self.weight_sum = 0.0;
|
|
self.amplitude_sum = 0.0;
|
|
self.amplitude_peak = 0.0;
|
|
self.count = 0;
|
|
}
|
|
}
|
|
|
|
fn highpass_alpha(sample_rate: u32, cutoff: f64) -> f64 {
|
|
let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff.max(1.0));
|
|
let dt = 1.0 / sample_rate.max(1) as f64;
|
|
(rc / (rc + dt)).clamp(0.0, 1.0)
|
|
}
|
|
|
|
fn lowpass_alpha(sample_rate: u32, cutoff: f64) -> f64 {
|
|
let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff.max(1.0));
|
|
let dt = 1.0 / sample_rate.max(1) as f64;
|
|
(dt / (rc + dt)).clamp(0.0, 1.0)
|
|
}
|
|
|
|
fn build_hilbert_kernel() -> [f64; HILBERT_TAPS] {
|
|
let mut kernel = [0.0; HILBERT_TAPS];
|
|
for (index, value) in kernel.iter_mut().enumerate() {
|
|
let offset = index as isize - HILBERT_HALF as isize;
|
|
if offset == 0 || offset % 2 == 0 {
|
|
continue;
|
|
}
|
|
let window = 0.54
|
|
- 0.46
|
|
* ((2.0 * std::f64::consts::PI * index as f64) / (HILBERT_TAPS - 1) as f64).cos();
|
|
*value = 2.0 / (std::f64::consts::PI * offset as f64) * window;
|
|
}
|
|
kernel
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn feed_tone(analyzer: &mut PhaseWheelAnalyzer, phase: f64, samples: usize) {
|
|
let omega = 2.0 * std::f64::consts::PI * 1_000.0 / 48_000.0;
|
|
for index in 0..samples {
|
|
let t = omega * index as f64;
|
|
analyzer.process((0.5 * t.sin()) as f32, (0.5 * (t - phase).sin()) as f32);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn continuous_analyzer_tracks_tone_phase() {
|
|
let mut analyzer = PhaseWheelAnalyzer::new(48_000);
|
|
feed_tone(&mut analyzer, std::f64::consts::FRAC_PI_2, 4_800);
|
|
let snapshot = analyzer.take_snapshot();
|
|
let angle = snapshot.angle_rad.expect("coherent tone has a phase");
|
|
assert!((angle.abs() - std::f32::consts::FRAC_PI_2).abs() < 0.03);
|
|
assert!(
|
|
snapshot.coherence > 0.9,
|
|
"coherence was {}",
|
|
snapshot.coherence
|
|
);
|
|
assert!(snapshot.level > 0.1);
|
|
assert!(snapshot.peak >= snapshot.level);
|
|
}
|
|
|
|
#[test]
|
|
fn snapshot_reset_does_not_reset_filter_or_hilbert_history() {
|
|
let mut analyzer = PhaseWheelAnalyzer::new(48_000);
|
|
feed_tone(&mut analyzer, 0.4, 2_400);
|
|
let first = analyzer.take_snapshot().angle_rad.unwrap();
|
|
feed_tone(&mut analyzer, 0.4, 800);
|
|
let second = analyzer.take_snapshot().angle_rad.unwrap();
|
|
assert!((first - second).abs() < 0.03);
|
|
}
|
|
|
|
#[test]
|
|
fn one_sided_signal_does_not_invent_a_phase() {
|
|
let mut analyzer = PhaseWheelAnalyzer::new(48_000);
|
|
for index in 0..2_400 {
|
|
let t = 2.0 * std::f64::consts::PI * 1_000.0 * index as f64 / 48_000.0;
|
|
analyzer.process((0.5 * t.sin()) as f32, 0.0);
|
|
}
|
|
let snapshot = analyzer.take_snapshot();
|
|
assert!(snapshot.angle_rad.is_none());
|
|
assert_eq!(snapshot.coherence, 0.0);
|
|
}
|
|
|
|
#[test]
|
|
fn energetic_component_dominates_a_quiet_conflicting_tone() {
|
|
let mut analyzer = PhaseWheelAnalyzer::new(48_000);
|
|
for index in 0..9_600 {
|
|
let t = index as f64 / 48_000.0;
|
|
let strong = 2.0 * std::f64::consts::PI * 1_000.0 * t;
|
|
let quiet = 2.0 * std::f64::consts::PI * 2_000.0 * t;
|
|
let left = 0.5 * strong.sin() + 0.04 * quiet.sin();
|
|
let right = 0.5 * strong.sin() + 0.04 * (quiet - std::f64::consts::FRAC_PI_2).sin();
|
|
analyzer.process(left as f32, right as f32);
|
|
}
|
|
let angle = analyzer.take_snapshot().angle_rad.unwrap();
|
|
assert!(angle.abs() < 0.03, "quiet tone pulled phase to {angle}");
|
|
}
|
|
}
|