fft.c

浏览该文件的文档.

#include <math.h>
#include <stdint.h>
#include <string.h>
#include "memory_bridge.h"
#include "internal.h"
#include "lmmc/config.h"
#include "lmmc/fft.h"
#include "lmmc/numeric_scalar.h"

static int fft_is_power_of_two(size_t n) {
    return (n > 0) && ((n & (n - 1)) == 0);
}
 

static size_t fft_next_power_of_two(size_t n) {
    size_t p = 1;
    if (n == 0) { return 1; }
    while (p < n) {
        if (p > SIZE_MAX / 2) { return 0; }
        p <<= 1;
    }
    return p;
}

static void fft_bit_reverse(lmmc_real_t* real, lmmc_real_t* imag, size_t n) {
    size_t i, j, bit;
    lmmc_real_t tmp;
 
    j = 0;
    for (i = 1; i < n; ++i) {
        bit = n >> 1;
        while (j & bit) {
            j ^= bit;
            bit >>= 1;
        }
        j ^= bit;
 
        if (i < j) {
            tmp = real[i]; real[i] = real[j]; real[j] = tmp;
            tmp = imag[i]; imag[i] = imag[j]; imag[j] = tmp;
        }
    }
}

static lmmc_status_t fft_radix2(lmmc_real_t* real, lmmc_real_t* imag, size_t n, int inverse) {
    size_t len, i, j;
    double angle_sign;
 
    if (n <= 1) { return LMMC_STATUS_OK; }
 
    fft_bit_reverse(real, imag, n);
 
    angle_sign = inverse ? 1.0 : -1.0;
 
    for (len = 2; len <= n; len <<= 1) {
        double angle = angle_sign * 2.0 * LMMC_PI / (double)len;
        double wlen_r = cos(angle);
        double wlen_i = sin(angle);
 
        for (i = 0; i < n; i += len) {
            double w_r = 1.0, w_i = 0.0;
            size_t half = len >> 1;
 
            for (j = 0; j < half; ++j) {
                size_t u_idx = i + j;
                size_t v_idx = i + j + half;

                double t_r = w_r * real[v_idx] - w_i * imag[v_idx];
                double t_i = w_r * imag[v_idx] + w_i * real[v_idx];

                real[v_idx] = real[u_idx] - t_r;
                imag[v_idx] = imag[u_idx] - t_i;
                real[u_idx] = real[u_idx] + t_r;
                imag[u_idx] = imag[u_idx] + t_i;

                {
                    double new_w_r = w_r * wlen_r - w_i * wlen_i;
                    double new_w_i = w_r * wlen_i + w_i * wlen_r;
                    w_r = new_w_r;
                    w_i = new_w_i;
                }
            }
        }
        if (len == n) { break; }
    }
 
    if (inverse) {
        double scale = 1.0 / (double)n;
        for (i = 0; i < n; ++i) {
            real[i] *= scale;
            imag[i] *= scale;
        }
    }
 
    return LMMC_STATUS_OK;
}
 
static void fft_chirp_input(
    const lmmc_real_t* real, const lmmc_real_t* imag,
    lmmc_real_t* a_r, lmmc_real_t* a_i, size_t n, double sign)
{
    size_t k;
    for (k = 0; k < n; ++k) {
        const double kd = (double)k;
        const double phase = sign * LMMC_PI * kd * kd / (double)n;
        const double c = cos(phase);
        const double s = sin(phase);
        a_r[k] = real[k] * c - imag[k] * s;
        a_i[k] = imag[k] * c + real[k] * s;
    }
}
 
static void fft_chirp_kernel(
    lmmc_real_t* b_r, lmmc_real_t* b_i,
    size_t n, size_t m, double sign)
{
    size_t k;
    for (k = 0; k < n; ++k) {
        const double kd = (double)k;
        const double phase = -sign * LMMC_PI * kd * kd / (double)n;
        b_r[k] = cos(phase);
        b_i[k] = sin(phase);
    }
    for (k = 1; k < n; ++k) {
        b_r[m - k] = b_r[k];
        b_i[m - k] = b_i[k];
    }
}
 
static void fft_pointwise_product(
    lmmc_real_t* a_r, lmmc_real_t* a_i,
    const lmmc_real_t* b_r, const lmmc_real_t* b_i, size_t n)
{
    size_t k;
    for (k = 0; k < n; ++k) {
        const double tr = a_r[k] * b_r[k] - a_i[k] * b_i[k];
        const double ti = a_r[k] * b_i[k] + a_i[k] * b_r[k];
        a_r[k] = tr;
        a_i[k] = ti;
    }
}

static lmmc_status_t fft_bluestein(lmmc_real_t* real, lmmc_real_t* imag, size_t n, int inverse) {
    size_t m;           /* padded power-of-2 length */
    size_t convolution_length;
    size_t k;
    double sign;
    lmmc_real_t *a_r = NULL, *a_i = NULL;
    lmmc_real_t *b_r = NULL, *b_i = NULL;
    lmmc_status_t st = LMMC_STATUS_OK;
 
    if (n <= 1) {
        return LMMC_STATUS_OK;
    }

    if (!lmmc_safe_mul_size(2, n, &convolution_length)) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
    --convolution_length;
    m = fft_next_power_of_two(convolution_length);
    if (m == 0) { return LMMC_STATUS_INVALID_ARGUMENT; }

    a_r = (lmmc_real_t*)lmmc_memory_alloc_array(m, sizeof(lmmc_real_t));
    a_i = (lmmc_real_t*)lmmc_memory_alloc_array(m, sizeof(lmmc_real_t));
    b_r = (lmmc_real_t*)lmmc_memory_alloc_array(m, sizeof(lmmc_real_t));
    b_i = (lmmc_real_t*)lmmc_memory_alloc_array(m, sizeof(lmmc_real_t));
 
    if (a_r == NULL || a_i == NULL || b_r == NULL || b_i == NULL) {
        st = LMMC_STATUS_ALLOCATION_FAILED;
        goto cleanup;
    }

    memset(a_r, 0, m * sizeof(lmmc_real_t));
    memset(a_i, 0, m * sizeof(lmmc_real_t));
    memset(b_r, 0, m * sizeof(lmmc_real_t));
    memset(b_i, 0, m * sizeof(lmmc_real_t));

    sign = inverse ? 1.0 : -1.0;
 
    fft_chirp_input(real, imag, a_r, a_i, n, sign);
    fft_chirp_kernel(b_r, b_i, n, m, sign);

    st = fft_radix2(a_r, a_i, m, 0);
    if (st != LMMC_STATUS_OK) { goto cleanup; }
 
    st = fft_radix2(b_r, b_i, m, 0);
    if (st != LMMC_STATUS_OK) { goto cleanup; }

    fft_pointwise_product(a_r, a_i, b_r, b_i, m);
 
    /* Inverse FFT of the product */
    st = fft_radix2(a_r, a_i, m, 1);
    if (st != LMMC_STATUS_OK) { goto cleanup; }

    fft_chirp_input(a_r, a_i, real, imag, n, sign);
 
    /* For inverse FFT, normalize by 1/N */
    if (inverse) {
        double scale = 1.0 / (double)n;
        for (k = 0; k < n; ++k) {
            real[k] *= scale;
            imag[k] *= scale;
        }
    }
 
cleanup:
    lmmc_memory_free(a_r);
    lmmc_memory_free(a_i);
    lmmc_memory_free(b_r);
    lmmc_memory_free(b_i);
    return st;
}
 
lmmc_status_t lmmc_fft(lmmc_real_t* real, lmmc_real_t* imag, size_t n, int inverse) {
    lmmc_storage_envelope_t real_envelope;
    lmmc_storage_envelope_t imag_envelope;
 
    if (real == NULL || imag == NULL || n == 0 ||
        !lmmc_storage_envelope_checked(
            real, 1, n, n, sizeof(lmmc_real_t), &real_envelope) ||
        !lmmc_storage_envelope_checked(
            imag, 1, n, n, sizeof(lmmc_real_t), &imag_envelope) ||
        lmmc_storage_envelopes_overlap(&real_envelope, &imag_envelope)) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
    if (n == 1) {
        /* DFT of length 1 is identity */
        return LMMC_STATUS_OK;
    }
 
    /* Dispatch: power-of-4 -> radix-4, power-of-2 -> radix-2, else -> Bluestein */
    if (lmmc_is_power_of_four(n, NULL)) {
        return lmmc_fft_radix4(real, imag, n, inverse ? 1 : 0);
    } else if (fft_is_power_of_two(n)) {
        return fft_radix2(real, imag, n, inverse ? 1 : 0);
    } else {
        return fft_bluestein(real, imag, n, inverse ? 1 : 0);
    }
}
 
lmmc_status_t lmmc_fft_forward(lmmc_real_t* real, lmmc_real_t* imag, size_t n) {
    return lmmc_fft(real, imag, n, 0);
}
 
lmmc_status_t lmmc_fft_inverse(lmmc_real_t* real, lmmc_real_t* imag, size_t n) {
    return lmmc_fft(real, imag, n, 1);
}
 
static int fft_padding_overlaps(
    const lmmc_storage_envelope_t* real_in,
    const lmmc_storage_envelope_t* imag_in,
    const lmmc_storage_envelope_t* real_out,
    const lmmc_storage_envelope_t* imag_out,
    const lmmc_storage_envelope_t* nfft)
{
    if (lmmc_storage_envelopes_overlap(real_out, imag_out)) {
        return 1;
    }
    if (lmmc_storage_envelopes_overlap(real_out, real_in) ||
        lmmc_storage_envelopes_overlap(real_out, imag_in)) {
        return 1;
    }
    if (lmmc_storage_envelopes_overlap(imag_out, real_in) ||
        lmmc_storage_envelopes_overlap(imag_out, imag_in)) {
        return 1;
    }
    if (lmmc_storage_envelopes_overlap(nfft, real_in) ||
        lmmc_storage_envelopes_overlap(nfft, imag_in)) {
        return 1;
    }
    return lmmc_storage_envelopes_overlap(nfft, real_out) ||
           lmmc_storage_envelopes_overlap(nfft, imag_out);
}
 
lmmc_status_t lmmc_fft_radix4_pad_into(
    const lmmc_real_t* real_in, const lmmc_real_t* imag_in, size_t n,
    lmmc_real_t* real_out, lmmc_real_t* imag_out, size_t* out_nfft)
{
    size_t nfft = 0;
    lmmc_status_t st;
    lmmc_storage_envelope_t real_in_envelope;
    lmmc_storage_envelope_t imag_in_envelope;
    lmmc_storage_envelope_t real_out_envelope;
    lmmc_storage_envelope_t imag_out_envelope;
    lmmc_storage_envelope_t nfft_envelope;
 
    if (real_in == NULL || imag_in == NULL || n == 0) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
    if (real_out == NULL || imag_out == NULL || out_nfft == NULL) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
 
    st = lmmc_fft_radix4_next_size(n, &nfft);
    if (st != LMMC_STATUS_OK) {
        return st;
    }
    if (!lmmc_storage_envelope_checked(
            real_in, 1, n, n, sizeof(lmmc_real_t), &real_in_envelope) ||
        !lmmc_storage_envelope_checked(
            imag_in, 1, n, n, sizeof(lmmc_real_t), &imag_in_envelope) ||
        !lmmc_storage_envelope_checked(
            real_out, 1, nfft, nfft, sizeof(lmmc_real_t), &real_out_envelope) ||
        !lmmc_storage_envelope_checked(
            imag_out, 1, nfft, nfft, sizeof(lmmc_real_t), &imag_out_envelope) ||
        !lmmc_storage_envelope_checked(
            out_nfft, 1, 1, 1, sizeof(size_t), &nfft_envelope)) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
    if (fft_padding_overlaps(&real_in_envelope, &imag_in_envelope,
                             &real_out_envelope, &imag_out_envelope,
                             &nfft_envelope)) {
        return LMMC_STATUS_INVALID_ARGUMENT;
    }
 
    /* Copy input data */
    memcpy(real_out, real_in, n * sizeof(lmmc_real_t));
    memcpy(imag_out, imag_in, n * sizeof(lmmc_real_t));
 
    /* Zero-pad the remainder */
    if (nfft > n) {
        memset(real_out + n, 0, (nfft - n) * sizeof(lmmc_real_t));
        memset(imag_out + n, 0, (nfft - n) * sizeof(lmmc_real_t));
    }
 
    *out_nfft = nfft;
    return LMMC_STATUS_OK;
}