49 lines
2.0 KiB
C++
49 lines
2.0 KiB
C++
#pragma once
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
|
|
// Bit-exact transcription of the soothe2 "twin" resonance filter 0x180535880
|
|
// (float sibling of the resonator 2nd-order detector core, NLS .sdk plugin).
|
|
//
|
|
// Pipeline per complex bin (all float32, op-order faithful to the disassembly):
|
|
// z1 = conj(e^{i*theta}) (rotor -> interleave, then 0x1800018b0 conj)
|
|
// z2 = cplx_mul(z1, z1) (0x18000ad60 scalar body @0x18000ae00)
|
|
// accA = A0; accA = fma(A1, z1, accA); accA = fma(A2, z2, accA) (vfmadd213ss)
|
|
// accB = B0; accB = fma(B1, z1, accB); accB = fma(B2, z2, accB)
|
|
// out = 2 * cplx_div(accB, accA) (0x181a77520: rcpps + 1 Newton step)
|
|
//
|
|
// Coefficients come from FUN_180533ec0 (double pipeline) + cvtpd2ps packing
|
|
// as done in the twin prologue (stack slots +0x28..+0x40).
|
|
//
|
|
// Constants locked in Phase 1:
|
|
// A0=B0=1+d, A1=B1=-2*cos(w0), A2=1-d, B2=1-d2,
|
|
// d = p*sqrtf(param_5), d2 = p/sqrtf(param_5),
|
|
// p = sin(w0)*0.5/Q, w0 = max(freq,2.0)*2*pi/fs_total.
|
|
// param_5 = 10^(sens_stored/20) with sens_stored~=24.65 dB (host-scaled ~=2*XML 12.0).
|
|
|
|
namespace detkernel {
|
|
|
|
struct cplxf {
|
|
float re, im;
|
|
};
|
|
|
|
struct twin_coeff {
|
|
float A[3]; // A0,A1,A2 (float32 after cvtpd2ps)
|
|
float B[3]; // B0,B1,B2
|
|
};
|
|
|
|
// FUN_180533ec0 coefficients, packed to float32 like the twin prologue.
|
|
// sens_lin = param_5 (linear, before sqrtf) e.g. 10^(24.65/20).
|
|
twin_coeff build_twin_coeff(double fs_total, double freq, double q, float sens_lin);
|
|
|
|
// 0x181a77520 complex division, scalar form (rcpps + Newton, NaN guard).
|
|
void cplx_div_exact(const cplxf& num, const cplxf& den, cplxf& out);
|
|
|
|
// 0x18000ad60 scalar complex multiply (vfmaddsub213ps form).
|
|
void cplx_mul_exact(const cplxf& a, const cplxf& b, cplxf& out);
|
|
|
|
// Full twin evaluation for `n` bins. `z` carries unit-magnitude twiddles
|
|
// exp(+i*theta_k) (the function applies the conjugate itself).
|
|
void twin_apply(const twin_coeff& c, const cplxf* z, size_t n, cplxf* out);
|
|
|
|
} // namespace detkernel
|