P0: track twin.cpp/twin.hpp/rotor_kernel.hpp (were shadowed by .gitignore)

dsp/twin.cpp

@@ -0,0 +1,115 @@
+#include "twin.hpp"
+
+#include <cmath>
+#include <cstring>
+#include <xmmintrin.h>
+
+#include "rotor_kernel.hpp"
+
+namespace detkernel {
+
+// ---------------------------------------------------------------------------
+// Generator FUN_180533ec0 (double), then cvtpd2ps packing as in the twin prologue.
+// ---------------------------------------------------------------------------
+twin_coeff build_twin_coeff(double fs_total, double freq, double q, float sens_lin) {
+    float f1 = std::sqrt(sens_lin);
+    if (f1 <= 0.0f) f1 = 0.0f;
+    const double f1d = static_cast<double>(f1);
+
+    const double w0 = (freq < 2.0 ? 2.0 : freq) * 6.283185307179586 / fs_total;
+    const double s  = std::sin(w0);
+    const double c2 = std::cos(w0) * -2.0;
+    const double p  = (s * 0.5) / q;
+
+    const double a0 = 1.0 + p * f1d;
+    const double a2 = 1.0 - p * f1d;
+    const double b0 = 1.0 + p / f1d;
+    const double b2 = 1.0 - p / f1d;
+
+    twin_coeff c;
+    c.A[0] = static_cast<float>(a0);
+    c.A[1] = static_cast<float>(c2);
+    c.A[2] = static_cast<float>(a2);
+    c.B[0] = static_cast<float>(b0);
+    c.B[1] = static_cast<float>(c2);
+    c.B[2] = static_cast<float>(b2);
+    return c;
+}
+
+// ---------------------------------------------------------------------------
+// 0x181a77520: complex division  num/den = (num*conj(den)) * ref
+//   per-lane: shufps-0x88/0xdd -> |den|^2; mulps bith; rcpps + 1 Newton step.
+// ---------------------------------------------------------------------------
+void cplx_div_exact(const cplxf& num, const cplxf& den, cplxf& out) {
+    const float ar = den.re, ai = den.im;
+    const float br = num.re, bi = num.im;
+
+    const float ar2 = ar * ar;
+    const float ai2 = ai * ai;
+    const float den2 = ar2 + ai2;
+
+    const float re = ar * br + ai * bi;   // Re{num*conj(den)}
+    const float im = ar * bi - ai * br;   // Im{num*conj(den)}
+
+    if (den2 == 0.0f) {
+        const float qnan = 0.0f / 0.0f;
+        out.re = qnan;
+        out.im = qnan;
+        return;
+    }
+
+    const float r0  = _mm_cvtss_f32(_mm_rcp_ss(_mm_set_ss(den2)));
+    const float t1  = den2 * r0;
+    const float t2  = 2.0f - t1;
+    const float ref = r0 * t2;
+
+    out.re = re * ref;
+    out.im = im * ref;
+}
+
+// ---------------------------------------------------------------------------
+// 0x18000ad60 scalar complex multiply (body @0x18000ae00):
+//   t0=bi*ai; t1=bi*ar; re=fma(br,ar,-t0); im=fma(br,ai,+t1)
+// ---------------------------------------------------------------------------
+void cplx_mul_exact(const cplxf& a, const cplxf& b, cplxf& out) {
+    const float ar = a.re, ai = a.im;
+    const float br = b.re, bi = b.im;
+
+    const float t0 = bi * ai;   // mulps
+    const float t1 = bi * ar;   // mulps
+    out.re = fma_f(br, ar, -t0);  // vfmaddsub213ps lane0 (subtract)
+    out.im = fma_f(br, ai, t1);   // lane1 (add)
+}
+
+// ---------------------------------------------------------------------------
+// 0x180535880 hot loop: seed A/B into acc, 2 Horner FMA stages, cplx-div, x2.
+// `z` carries exp(+i*theta_k); the twin conjugates before use (0x1800018b0).
+// ---------------------------------------------------------------------------
+void twin_apply(const twin_coeff& c, const cplxf* z, size_t n, cplxf* out) {
+    for (size_t i = 0; i < n; ++i) {
+        // conj(z1) = conj(exp(+i*theta))  (0x1800018b0 negates imag)
+        const cplxf z1 = { z[i].re, -z[i].im };
+        cplxf z2;
+        cplx_mul_exact(z1, z1, z2);   // conj(z1)^2
+
+        // A accumulator (vfmadd213ss per scalar lane)
+        cplxf accA = { c.A[0], 0.0f };
+        accA.re = fma_f(c.A[1], z1.re, accA.re);
+        accA.im = fma_f(c.A[1], z1.im, accA.im);
+        accA.re = fma_f(c.A[2], z2.re, accA.re);
+        accA.im = fma_f(c.A[2], z2.im, accA.im);
+
+        // B accumulator
+        cplxf accB = { c.B[0], 0.0f };
+        accB.re = fma_f(c.B[1], z1.re, accB.re);
+        accB.im = fma_f(c.B[1], z1.im, accB.im);
+        accB.re = fma_f(c.B[2], z2.re, accB.re);
+        accB.im = fma_f(c.B[2], z2.im, accB.im);
+
+        cplx_div_exact(accB, accA, out[i]);   // B/A
+        out[i].re *= 2.0f;                    // 2*B/A (caller-side scale, exact)
+        out[i].im *= 2.0f;
+    }
+}
+
+}  // namespace detkernel