minidsp_gcc.c

Go to the documentation of this file.

 
#include "minidsp.h"
#include "minidsp_internal.h"
 
/* -----------------------------------------------------------------------
 * Static (file-scope) variables for FFT caching
 *
 * Creating FFTW plans is expensive, so we cache them.  As long as the
 * signal length N stays the same between calls, we reuse the existing
 * plans and buffers.  If N changes, we tear everything down and rebuild.
 * -----------------------------------------------------------------------*/
 
static unsigned      _N        = 0;    /* Current cached signal length        */
static double       *siga_loc  = nullptr; /* Local copy of input signal A        */
static double       *sigb_loc  = nullptr; /* Local copy of input signal B        */
static double       *lags_loc  = nullptr; /* Shifted cross-correlation output    */
static fftw_complex *ffta      = nullptr; /* FFT of signal A                     */
static fftw_complex *fftb      = nullptr; /* FFT of signal B                     */
static fftw_complex *xspec     = nullptr; /* Cross-spectrum (FFT_B * conj(FFT_A))*/
static double       *xcorr     = nullptr; /* Raw cross-correlation (time domain) */
 
static fftw_plan     pa = nullptr;        /* FFTW plan: signal A -> FFT          */
static fftw_plan     pb = nullptr;        /* FFTW plan: signal B -> FFT          */
static fftw_plan     px = nullptr;        /* FFTW plan: cross-spectrum -> IFFT   */
 
/* -----------------------------------------------------------------------
 * Internal helpers for FFT buffer management
 * -----------------------------------------------------------------------*/
 
static void _xcorr_free(void)
{
    if (xspec)    fftw_free(xspec);
    if (fftb)     fftw_free(fftb);
    if (ffta)     fftw_free(ffta);
 
    if (lags_loc) free(lags_loc);
    if (xcorr)    free(xcorr);
    if (sigb_loc) free(sigb_loc);
    if (siga_loc) free(siga_loc);
 
    xspec = nullptr;   fftb = nullptr;    ffta = nullptr;
    lags_loc = nullptr; xcorr = nullptr;  sigb_loc = nullptr; siga_loc = nullptr;
}
 
static void _xcorr_malloc(void)
{
    siga_loc = calloc(_N, sizeof(double));
    sigb_loc = calloc(_N, sizeof(double));
    xcorr    = calloc(_N + 1, sizeof(double)); /* FFTW c2r needs N+1 */
    lags_loc = calloc(_N, sizeof(double));
 
    /* FFTW provides its own allocator that guarantees proper alignment
     * for SIMD instructions (SSE2, AVX, etc.).  Always use it for
     * fftw_complex arrays. */
    ffta  = fftw_alloc_complex(_N);
    fftb  = fftw_alloc_complex(_N);
    xspec = fftw_alloc_complex(_N);
}
 
static void _xcorr_teardown(void)
{
    if (pa) fftw_destroy_plan(pa);
    if (pb) fftw_destroy_plan(pb);
    if (px) fftw_destroy_plan(px);
    pa = nullptr; pb = nullptr; px = nullptr;
 
    _xcorr_free();
}
 
static void _xcorr_setup(void)
{
    _xcorr_teardown();
    _xcorr_malloc();
 
    /* FFTW_ESTIMATE: pick a reasonable plan quickly (don't benchmark).
     * FFTW_DESTROY_INPUT: FFTW may overwrite the input array -- that's
     * fine because we always copy the data into our local buffers first. */
    pa = fftw_plan_dft_r2c_1d(_N, siga_loc, ffta,  FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    pb = fftw_plan_dft_r2c_1d(_N, sigb_loc, fftb,  FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    px = fftw_plan_dft_c2r_1d(_N, xspec,    xcorr, FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
}
 
static void _gcc_setup(unsigned N)
{
    if (_N != N) {
        _N = N;
        _xcorr_setup();
    }
}
 
/* -----------------------------------------------------------------------
 * Cross-file teardown wrapper (declared in minidsp_internal.h)
 *
 * MD_shutdown() in minidsp_spectrum.c calls this to tear down the GCC
 * cache.  Unlike _xcorr_teardown() (which must NOT reset _N because
 * _xcorr_setup sets _N before calling it), this wrapper safely resets
 * _N because it is only called at shutdown time.
 * -----------------------------------------------------------------------*/
 
void md_gcc_teardown(void)
{
    _xcorr_teardown();
    _N = 0;
}
 
/* -----------------------------------------------------------------------
 * Internal helpers for cross-correlation post-processing
 * -----------------------------------------------------------------------*/
 
static void _fftshift(const double *in, double *out, unsigned N)
{
    /* The split point: how many elements are in the "second half" */
    unsigned half = (unsigned)floor((double)N / 2.0);
 
    /* Copy the second half of in[] (negative lags) to the start of out[] */
    memcpy(out, &in[half], sizeof(double) * (N - half));
 
    /* Copy the first half of in[] (positive lags) to the end of out[] */
    memcpy(&out[N - half], in, sizeof(double) * half);
}
 
static void _max_index(const double *a, unsigned N,
                       double *max, unsigned *maxi)
{
    assert(a != nullptr);
    assert(N >= 1);
 
    unsigned best_i = 0;
    double   best_v = a[0];
 
    for (unsigned i = 1; i < N; i++) {
        if (a[i] > best_v) {
            best_v = a[i];
            best_i = i;
        }
    }
 
    *max  = best_v;
    *maxi = best_i;
}
 
/* -----------------------------------------------------------------------
 * Public API: GCC-PHAT delay estimation
 * -----------------------------------------------------------------------*/
 
void MD_get_multiple_delays(const double **sigs, unsigned M, unsigned N,
                            unsigned margin, int weightfunc,
                            int *outdelays)
{
    /* Static buffers are reused across calls for efficiency.
     * They are only re-allocated when the signal length changes. */
    static unsigned last_N   = 0;
    static double  *hann_win = nullptr;
    static double  *t_ref    = nullptr;
    static double  *t_sig    = nullptr;
 
    if (M < 2) return;
 
    /* Re-allocate if the signal length has changed */
    if (last_N != N) {
        free(hann_win);  /* free(nullptr) is safe in C */
        free(t_ref);
        free(t_sig);
 
        hann_win = malloc(N * sizeof(double));
        assert(hann_win != nullptr);
        MD_Gen_Hann_Win(hann_win, N);
 
        t_ref = malloc(N * sizeof(double));
        assert(t_ref != nullptr);
 
        t_sig = malloc(N * sizeof(double));
        assert(t_sig != nullptr);
 
        last_N = N;
    }
 
    /* Apply the Hanning window to the reference signal */
    for (unsigned i = 0; i < N; i++) {
        t_ref[i] = sigs[0][i] * hann_win[i];
    }
 
    /* Compute the delay for each non-reference signal */
    for (unsigned i = 0; i < M - 1; i++) {
        /* Window the comparison signal */
        for (unsigned j = 0; j < N; j++) {
            t_sig[j] = sigs[i + 1][j] * hann_win[j];
        }
        outdelays[i] = MD_get_delay(t_ref, t_sig, N, nullptr, margin, weightfunc);
    }
}
 
int MD_get_delay(const double *siga, const double *sigb, unsigned N,
                 double *ent, unsigned margin, int weightfunc)
{
    _gcc_setup(N);
 
    /* Compute the full cross-correlation */
    MD_gcc(siga, sigb, N, lags_loc, weightfunc);
 
    /* The zero-lag position in the shifted output */
    unsigned center = (unsigned)ceil((double)N / 2.0);
 
    /* Clamp the margin so we don't read outside the array */
    unsigned m = margin;
    if (center < m) {
        m = center;
    }
    if (center + m >= N) {
        m = (N - 1) - center;
    }
 
    /* Search the window [center-m, center+m] for the peak */
    unsigned start = center - m;
    unsigned len   = 2 * m + 1;
    double   peak_val;
    unsigned peak_i;
 
    _max_index(lags_loc + start, len, &peak_val, &peak_i);
 
    /* Optionally compute the entropy of the search window */
    if (ent != nullptr) {
        *ent = MD_entropy(lags_loc + start, len, true);
    }
 
    /* Convert the peak index to a signed delay relative to zero-lag */
    return (int)(peak_i) - (int)(m);
}
 
void MD_gcc(const double *siga, const double *sigb, unsigned N,
            double *lagvals, int weightfunc)
{
    _gcc_setup(N);
 
    /* Copy input into local buffers (FFTW may overwrite them) */
    memcpy(siga_loc, siga, _N * sizeof(double));
    memcpy(sigb_loc, sigb, _N * sizeof(double));
 
    /* Forward FFT of both signals.
     *
     * For a real-valued input of length N, the FFT output has only
     * N/2 + 1 unique complex values (the rest are conjugate-symmetric).
     * FFTW's r2c transform exploits this and only writes N/2+1 values. */
    fftw_execute(pa);
    fftw_execute(pb);
 
    /* Compute the weighted cross-spectrum.
     *
     * The cross-spectrum is:  X[k] = FFT_B[k] * conj(FFT_A[k])
     *
     * For PHAT weighting, we divide by the magnitude of the cross-spectrum.
     * This keeps only the phase information, which produces a much sharper
     * peak in the time-domain correlation.
     *
     * We only loop over the N/2+1 non-redundant frequency bins. */
    unsigned num_bins = _N / 2 + 1;
 
    switch (weightfunc) {
    case PHAT:
        for (unsigned i = 0; i < num_bins; i++) {
            double complex cs = fftb[i] * conj(ffta[i]);
            /* Divide by magnitude; add DBL_MIN to prevent division by zero */
            xspec[i] = cs / (cabs(cs) + DBL_MIN);
        }
        break;
 
    default: /* SIMP: simple 1/N weighting */
        for (unsigned i = 0; i < num_bins; i++) {
            double complex cs = fftb[i] * conj(ffta[i]);
            xspec[i] = cs / (double)_N;
        }
        break;
    }
 
    /* Inverse FFT of the cross-spectrum -> time-domain cross-correlation */
    fftw_execute(px);
 
    /* Rearrange so the zero-lag is at the centre of the output array */
    _fftshift(xcorr, lagvals, _N);
}