115 lines
4.1 KiB
C++
115 lines
4.1 KiB
C++
#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
|