dsp: freqpath - warp 0x5406a8 = 0.87*x/(1+x/K) (K=exp(2.0723), x=f/2000) из FUN_180530850 (0x530900) + self-check (rel err 1.5e-07, PASS)

2026-08-18 17:08:42 +03:00
parent 0a4c0c959b
commit 111d47530e
4 changed files with 166 additions and 0 deletions
@@ -15,6 +15,7 @@ add_library(soothe2_dsp SHARED
    filter.cpp
    detect.cpp
    twin.cpp
+    freqpath.cpp
    phase_table.cpp
 )

@@ -0,0 +1,28 @@
+#include "freqpath.hpp"
+
+#include <cmath>
+#include <cstring>
+
+namespace detkernel {
+
+// Bit-faithful transcription of the warp builder loop in FUN_180530850 (0x530900).
+void build_warp(float fs_total, int n, float* warp) {
+    const float K = std::exp(2.0723267f);               // 0x1824c4208 -> exp (0x1a14cac)
+    const float F = (2000.0f / (fs_total * 0.5f)) * static_cast<float>(n);  // 0x1824c45b4 / ([0x24]*0.5) * N
+
+    for (int i = 1; i < n; ++i) {
+        float x = static_cast<float>(i) / F;
+        float t = x / K;
+        t = t + 1.0f;                                   // +1.0 (0x1824c3ea4)
+        float r = 1.0f / t;                             // divss
+        r = static_cast<float>(std::fabs(static_cast<double>(r)));  // cvtss2sd/andpd/cvtpd2ps
+        warp[i] = r * x;
+    }
+
+    const float scale = 0.87f;                          // 0x1824c3e50 (also 0.87 as double 0x24c4110)
+    for (int i = 0; i < n; ++i) warp[i] *= scale;
+
+    warp[0] = 0.0f;                                     // 0x5406a8[0] = 0 (DC)
+}
+
+}  // namespace detkernel
@@ -0,0 +1,29 @@
+#pragma once
+#include <cstddef>
+#include <cstdint>
+
+// Freq-path (FUN_180530850) warp and resonance-vs-warp product.
+//
+// WARP 0x5406a8 (peak builder loop @0x530900, disasm f530850_full.dis):
+//   K  = expf(2.0723267f)                          (0x1824c4208 -> call 0x1a14cac)
+//   F  = (2000.0f / (fs_total * 0.5f)) * (float)N  (0x1824c45b4 / [0x24]*0x24c3d8c,
+//                                                   scaled by (count/2+1) = N)
+//   for i in 1..N-1:
+//       x  = (float)i / F                         (bin freq i ~ f = i*sr*0.5/N -> x ~ f/2000)
+//       r  = 1.0f / (x / K + 1.0f)                (0x1824c3ea4 = 1.0)
+//       r  = (float)fabs((double)r)               (cvtss2sd + andpd 0x24c4f10 + cvtpd2ps)
+//       w[i] = r * x
+//   w[0..N) *= 0.87f      (0x1824c3e50), 0x5406a8[0] = 0         (0x24c4110 = 0.87 as double)
+//
+//   warp(500)=0.211  warp(1000)=0.409  warp(2000)=0.773  (fs=44100)
+//
+// FUN_180530850 then applies the twin resonance (fc=8000.0, Q=1.0, sens c42d0=3.0
+// via generator 0x180533ec0 + kernel 0x180535880) into 0x5406f8 and multiplies
+// 0x5406f8 *= 0x5406a8 (thunk window-multiply 0x52d990). warp[0]=0 zeroes DC.
+
+namespace detkernel {
+
+// Build the 0x5406a8 warp (float, N bins, DC = 0).
+void build_warp(float fs_total, int n, float* warp);
+
+}  // namespace detkernel
@@ -0,0 +1,108 @@
+#include <cmath>
+#include <cstdio>
+#include <vector>
+
+#include "freqpath.hpp"
+#include "twin.hpp"
+
+// Phase-2 gate: the float twin transcription must reproduce the double
+// reference |2*B/A| (model_lut.res_at) to float rounding accuracy at the
+// Phase-1 fc-scan parameters (Q=xmlq, gain=sqrt(sens_stored)=4.132).
+
+static double res_ref(double ft, double fc, double q, double gain) {
+    const double fs = 44100.0;
+    const double w0 = fc * 2.0 * acos(-1.0) / fs;
+    const double c  = cos(w0), s = sin(w0);
+    const double p  = (s * 0.5) / q;
+    const double a0 = 1.0 + p * gain, a2 = 1.0 - p * gain;
+    const double b0 = 1.0 + p / gain, b2 = 1.0 - p / gain;
+    const double w  = 2.0 * acos(-1.0) * ft / fs;
+    const double zr = cos(w), zi = -sin(w);
+    const double ar = a0 + (-2.0 * c) * zr + a2 * (zr * zr - zi * zi);
+    const double ai = (-2.0 * c) * zi + a2 * 2.0 * zr * zi;
+    const double br = b0 + (-2.0 * c) * zr + b2 * (zr * zr - zi * zi);
+    const double bi = (-2.0 * c) * zi + b2 * 2.0 * zr * zi;
+    return 2.0 * std::sqrt((br * br + bi * bi) / (ar * ar + ai * ai));
+}
+
+int main() {
+    const double fs = 44100.0;
+    const double fcs[] = {800.0, 900.0, 950.0, 1000.0, 1050.0, 1100.0, 1200.0};
+    const double q = 0.9999978;
+    const double gain = 4.132;
+    const float sens_lin = static_cast<float>(gain * gain);  // param_5 -> sqrtf = gain
+
+    const double ft = 1000.0;
+    const detkernel::cplxf z = {
+        static_cast<float>(std::cos(2.0 * acos(-1.0) * ft / fs)),
+        static_cast<float>(std::sin(2.0 * acos(-1.0) * ft / fs)),
+    };
+
+    std::printf("fc        |twin|   |ref|   rel-err  20log10(twin)\n");
+    double maxrel = 0.0;
+    for (double fc : fcs) {
+        detkernel::twin_coeff c = detkernel::build_twin_coeff(fs, fc, q, sens_lin);
+        detkernel::cplxf out;
+        detkernel::twin_apply(c, &z, 1, &out);
+        const double mag = std::sqrt((double)out.re * out.re + (double)out.im * out.im);
+        const double ref = res_ref(ft, fc, q, gain);
+        const double rel = std::fabs(mag - ref) / ref;
+        if (rel > maxrel) maxrel = rel;
+        const double db = 20.0 * std::log10(mag);
+        std::printf("%5.1f  %9.6f %9.6f  %9.2e  %9.4f\n", fc, mag, ref, rel, db);
+    }
+    std::printf("\nmax rel err = %.3e\n", maxrel);
+    // float32 rcpps+Newton + fma Horner cumulative parity bound (~ ulp * stages)
+    std::printf("PASS = %s\n", maxrel < 5e-5 ? "yes (float-parity)" : "NO");
+    bool ok = maxrel < 5e-5;
+
+    // Full operating-space sweep: DUAL grid Q=0.1..10, tones 500/2000, fc=500.
+    std::printf("\n--- DUAL sweep (fc=500, gain=4.132), |2B/A| rel-err ---\n");
+    const double qs[] = {0.1, 0.2, 0.3, 0.5, 0.7, 1.0, 1.5, 2.0, 3.0, 5.0, 10.0};
+    const double ftone[] = {500.0, 2000.0};
+    const detkernel::cplxf zt[2] = {
+        {static_cast<float>(std::cos(2.0 * acos(-1.0) * 500.0 / fs)),
+         static_cast<float>(std::sin(2.0 * acos(-1.0) * 500.0 / fs))},
+        {static_cast<float>(std::cos(2.0 * acos(-1.0) * 2000.0 / fs)),
+         static_cast<float>(std::sin(2.0 * acos(-1.0) * 2000.0 / fs))},
+    };
+    double msweep = 0.0;
+    for (double q : qs) {
+        detkernel::twin_coeff c = detkernel::build_twin_coeff(fs, 500.0, q, sens_lin);
+        for (int t = 0; t < 2; ++t) {
+            detkernel::cplxf out;
+            detkernel::twin_apply(c, &zt[t], 1, &out);
+            const double mag = std::sqrt((double)out.re * out.re + (double)out.im * out.im);
+            const double ref = res_ref(ftone[t], 500.0, q, gain);
+            const double rel = std::fabs(mag - ref) / ref;
+            if (rel > msweep) msweep = rel;
+        }
+    }
+    std::printf("max sweep rel err = %.3e\n", msweep);
+    std::printf("SWEEP PASS = %s\n", msweep < 5e-4 ? "yes" : "NO");
+    ok = ok && (msweep < 5e-4);
+
+    // Freq-path warp 0x5406a8 (FUN_180530850): 0.87*x/(1+x/K), K=exp(2.0723).
+    std::printf("\n--- freq-path warp 0x5406a8 (fs=44100) ---\n");
+    const int N = 1025;
+    const float K = std::exp(2.0723267f);
+    std::vector<float> warp(N);
+    detkernel::build_warp(44100.0f, N, warp.data());
+    const double Fr = (2000.0 / 22050.0) * N;  // F for fs=44100
+    const double freqs[] = {500.0, 1000.0, 2000.0, 8000.0};
+    double werr = 0.0;
+    for (double f : freqs) {
+        const int bin = static_cast<int>(std::lround((N - 1) * f / 22050.0));  // real-bins mapping
+        const double x = static_cast<double>(bin) / Fr;
+        const double wref = 0.87 * x * K / (K + x);
+        const double rel = std::fabs(warp[bin] - wref) / wref;
+        if (rel > werr) werr = rel;
+        std::printf("%6.0f Hz: bin=%4d x=%.4f warp=%8.6f (ref=%8.6f rel=%.2e)\n",
+                    f, bin, x, warp[bin], wref, rel);
+    }
+    std::printf("warp max rel err = %.3e ; warp(0)=%g\n", werr, warp[0]);
+    std::printf("WARP PASS = %s\n", werr < 1e-6 ? "yes" : "NO");
+    ok = ok && warp[0] == 0.0f;
+
+    return ok ? 0 : 1;
+}