audacity/lib-src/libnyquist/nyquist/nyqsrc/convolve.c

/* convolve.c -- implements (non-"fast") convolution */
/* 
 * Note: this code is mostly generated by translate.lsp (see convole.tran
 * in the tran directory), but it has been modified by hand to extend the
 * stop time to include the "tail" of the convolution beyond the length
 * of the first parameter.
 */

/* Original convolve.c modified to do fast convolution. Here are some
 * notes:
 *    The first arg is arbitrary length. The second arg is the impulse
 * response, which is converted into a table. Tables have limited maximum
 * size, which is good because we're going to use a single FFT for the
 * whole impulse response.
 *
 * The fast convolution works like this: 
 *   inputs are x_snd and h_snd. 
 *   Make h_snd into a table ht of size N, where N is a power of 2. 
 *   Copy ht with zero fill into H of size 2N.
 *   Compute FFT of H in place.
 *   Iterate:
 *       Copy N samples of x_snd into X and zero fill to size 2N.
 *       Compute FFT of X in place.
 *       Multiply X by H (result goes into X).
 *       Compute IFFT of X in place
 *       Add X to R.
 *       Now N samples of R can be output.
 *       Copy 2nd half of R to first half and zero the 2nd half.
 *         (this is actually done first, and the first time does
 *          nothing because R is initially filled with zeros)
 *
 * Length of output is length of x input + length of h
 */

#define _USE_MATH_DEFINES 1 /* for Visual C++ to get M_LN2 */
#include <math.h>
#include "stdio.h"
#ifndef mips
#include "stdlib.h"
#endif
#include "xlisp.h"
#include "sound.h"

#include "falloc.h"
#include "cext.h"
#include "fftlib.h"
#include "fftext.h"
#include "convolve.h"

void convolve_free();


typedef struct convolve_susp_struct {
    snd_susp_node susp;
    long terminate_cnt;
    boolean logically_stopped;
    sound_type x_snd;
    long x_snd_cnt;
    sample_block_values_type x_snd_ptr;

    sample_type *H; // the FFT of h_snd
    int h_len; // true length of H
    int N; // length of block, FFTs are of size 2*N
    int M; // log2 of 2*N, the FFT size
    sample_type *X;
    sample_type *R; // result buffer where output is summed
    sample_type *R_current;
} convolve_susp_node, *convolve_susp_type;


void h_reverse(sample_type *h, long len)
{
    sample_type temp;
    int i;
    
    for (i = 0; i < len; i++) {
        temp = h[i];
        h[i] = h[len - 1];
        h[len - 1] = temp;
        len--;
    }
}


void convolve_s_fetch(snd_susp_type a_susp, snd_list_type snd_list)
{
    convolve_susp_type susp = (convolve_susp_type) a_susp;
    int cnt = 0; /* how many samples computed */
    int togo;
    int n;
    sample_block_type out;
    register sample_block_values_type out_ptr;

    register sample_block_values_type out_ptr_reg;

    sample_type *R = susp->R;
    sample_type *R_current;
    int N = susp->N;
    falloc_sample_block(out, "convolve_s_fetch");
    out_ptr = out->samples;
    snd_list->block = out;

    while (cnt < max_sample_block_len) { /* outer loop */
        /* first compute how many samples to generate in inner loop: */
        /* don't overflow the output sample block: */
        togo = max_sample_block_len - cnt;
        /* if we need output samples, generate them here */
        if (susp->R_current >= R + N) {
            /* Copy N samples of x_snd into X and zero fill to size 2N */
            int i = 0;
            sample_type *X = susp->X;
            sample_type *H = susp->H;
            int to_copy;
            while (i < N) {
                if (susp->x_snd_cnt == 0) {
                    susp_get_samples(x_snd, x_snd_ptr, x_snd_cnt);
                    if (susp->x_snd->logical_stop_cnt == 
                        susp->x_snd->current - susp->x_snd_cnt) {
                        min_cnt(&susp->susp.log_stop_cnt, susp->x_snd, 
                                (snd_susp_type) susp, susp->x_snd_cnt);
                    }
                }
                if (susp->x_snd_ptr == zero_block->samples) {
                    min_cnt(&susp->terminate_cnt, susp->x_snd,
                            (snd_susp_type) susp, susp->x_snd_cnt);
                    /* extend the output to include impulse response */
                    susp->terminate_cnt += susp->h_len;
                }
                /* copy no more than the remaining space and no more than
                 * the amount remaining in the block
                 */
                to_copy = min(N - i, susp->x_snd_cnt);
                memcpy(X + i, susp->x_snd_ptr, 
                       to_copy * sizeof(*susp->x_snd_ptr));
                susp->x_snd_ptr += to_copy;
                susp->x_snd_cnt -= to_copy;
                i += to_copy;
            }
            /* zero fill to size 2N */
            memset(X + N, 0, N * sizeof(X[0]));
            /* Compute FFT of X in place */
            fftInit(susp->M);
            rffts(X, susp->M, 1);
            /* Multiply X by H (result goes into X) */
            rspectprod(X, H, X, N * 2);
            /* Compute IFFT of X in place */
            riffts(X, susp->M, 1);
            /* Shift R, zero fill, add X, all in one loop */
            for (i = 0; i < N; i++) {
                R[i] = R[i + N] + X[i];
                R[i + N] = X[i + N];
            }
            /* now N samples of R can be output */
            susp->R_current = R;
        }
        /* compute togo, the number of samples to "compute" */
        /* can't use more than what's left in R. R_current is
           the next sample of R, so what's left is N - (R - R_current) */
        R_current = susp->R_current;
        togo = min(togo, N - (R_current - R));

        /* don't run past terminate time */
        if (susp->terminate_cnt != UNKNOWN &&
            susp->terminate_cnt <= susp->susp.current + cnt + togo) {
            togo = susp->terminate_cnt - (susp->susp.current + cnt);
            if (togo == 0) break;
        }

        /* don't run past logical stop time */
        if (!susp->logically_stopped &&
            susp->susp.log_stop_cnt !=  UNKNOWN &&
            susp->susp.log_stop_cnt <= susp->susp.current + cnt + togo) {
            togo = susp->susp.log_stop_cnt - (susp->susp.current + cnt);
            if (togo == 0) break;
        }       

        n = togo;
        out_ptr_reg = out_ptr;
        if (n) do { /* the inner sample computation loop */
            *out_ptr_reg++ = (sample_type) *R_current++;
	} while (--n); /* inner loop */

        /* using R_current is a bad idea on RS/6000: */
        susp->R_current += togo;
        out_ptr += togo;
        cnt += togo;
    } /* outer loop */

    /* test for termination */
    if (togo == 0 && cnt == 0) {
        snd_list_terminate(snd_list);
    } else {
        snd_list->block_len = cnt;
        susp->susp.current += cnt;
    }
    /* test for logical stop */
    if (susp->logically_stopped) {
        snd_list->logically_stopped = true;
    } else if (susp->susp.log_stop_cnt == susp->susp.current) {
        susp->logically_stopped = true;
    }
} /* convolve_s_fetch */


void convolve_toss_fetch(snd_susp_type a_susp, snd_list_type snd_list)
{
    convolve_susp_type susp = (convolve_susp_type) susp;
    time_type final_time = susp->susp.t0;
    long n;

    /* fetch samples from x_snd up to final_time for this block of zeros */
    while ((round((final_time - susp->x_snd->t0) * susp->x_snd->sr)) >=
	   susp->x_snd->current)
	susp_get_samples(x_snd, x_snd_ptr, x_snd_cnt);
    /* convert to normal processing when we hit final_count */
    /* we want each signal positioned at final_time */
    n = round((final_time - susp->x_snd->t0) * susp->x_snd->sr -
              (susp->x_snd->current - susp->x_snd_cnt));
    susp->x_snd_ptr += n;
    susp_took(x_snd_cnt, n);
    susp->susp.fetch = susp->susp.keep_fetch;
    (*(susp->susp.fetch))(a_susp, snd_list);
}


void convolve_mark(snd_susp_type a_susp)
{
    convolve_susp_type susp = (convolve_susp_type) a_susp;
    sound_xlmark(susp->x_snd);
}


void convolve_free(snd_susp_type a_susp)
{
    convolve_susp_type susp = (convolve_susp_type) a_susp;
    free(susp->R);
    free(susp->X);
    free(susp->H);
    sound_unref(susp->x_snd);
    ffree_generic(susp, sizeof(convolve_susp_node), "convolve_free");
}


void convolve_print_tree(snd_susp_type a_susp, int n)
{
    convolve_susp_type susp = (convolve_susp_type) a_susp;
    indent(n);
    stdputstr("x_snd:");
    sound_print_tree_1(susp->x_snd, n);
}

sound_type snd_make_convolve(sound_type x_snd, sound_type h_snd)
{
    register convolve_susp_type susp;
    rate_type sr = x_snd->sr;
    time_type t0 = x_snd->t0;
    sample_type scale_factor = 1.0F;
    time_type t0_min = t0;
    table_type table;
    double log_len;
    falloc_generic(susp, convolve_susp_node, "snd_make_convolve");
    table = sound_to_table(h_snd);
    susp->h_len = table->length;
    log_len = log(table->length) / M_LN2; /* compute log-base-2(length) */
    susp->M = (int) log_len;
    if (susp->M != log_len) susp->M++; /* round up */
    susp->N = 1 << susp->M; /* size of data blocks */
    susp->M++; /* M = log2(2 * N) */
    susp->H = (sample_type *) calloc(2 * susp->N, sizeof(susp->H[0]));
    if (!susp->H) {
        xlabort("memory allocation failure in convolve");
    }
    memcpy(susp->H, table->samples, sizeof(susp->H[0]) * susp->N);
    table_unref(table); /* don't need table now */
    /* remaining N samples are already zero-filled */
    if (fftInit(susp->M)) {
        free(susp->H);
        xlabort("fft initialization error in convolve");
    }
    rffts(susp->H, susp->M, 1);
    susp->X = (sample_type *) calloc(2 * susp->N, sizeof(susp->X[0]));
    susp->R = (sample_type *) calloc(2 * susp->N, sizeof(susp->R[0]));
    if (!susp->X || !susp->R) {
        free(susp->H);
        if (susp->X) free(susp->X);
        if (susp->R) free(susp->R);
        xlabort("memory allocation failed in convolve");
    }
    susp->R_current = susp->R + susp->N;
    susp->susp.fetch = &convolve_s_fetch;
    susp->terminate_cnt = UNKNOWN;
    /* handle unequal start times, if any */
    if (t0 < x_snd->t0) sound_prepend_zeros(x_snd, t0);
    /* minimum start time over all inputs: */
    t0_min = min(x_snd->t0, t0);
    /* how many samples to toss before t0: */
    susp->susp.toss_cnt = (long) ((t0 - t0_min) * sr + 0.5);
    if (susp->susp.toss_cnt > 0) {
	susp->susp.keep_fetch = susp->susp.fetch;
	susp->susp.fetch = convolve_toss_fetch;
    }

    /* initialize susp state */
    susp->susp.free = convolve_free;
    susp->susp.sr = sr;
    susp->susp.t0 = t0;
    susp->susp.mark = convolve_mark;
    susp->susp.print_tree = convolve_print_tree;
    susp->susp.name = "convolve";
    susp->logically_stopped = false;
    susp->susp.log_stop_cnt = logical_stop_cnt_cvt(x_snd);
    susp->susp.current = 0;
    susp->x_snd = x_snd;
    susp->x_snd_cnt = 0;
    return sound_create((snd_susp_type)susp, t0, sr, scale_factor);
}


sound_type snd_convolve(sound_type x_snd, sound_type h_snd)
{
    sound_type x_snd_copy = sound_copy(x_snd);
    return snd_make_convolve(x_snd_copy, h_snd);
}