From 0d4f573fc103544c7972485b010072eda52a0e4b Mon Sep 17 00:00:00 2001 From: Thomas Vander Stichele Date: Sun, 23 Dec 2001 10:44:28 +0000 Subject: adding resample lib Original commit message from CVS: adding resample lib --- gst-libs/gst/resample/resample.c | 530 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 530 insertions(+) create mode 100644 gst-libs/gst/resample/resample.c (limited to 'gst-libs/gst/resample/resample.c') diff --git a/gst-libs/gst/resample/resample.c b/gst-libs/gst/resample/resample.c new file mode 100644 index 00000000..cedb874e --- /dev/null +++ b/gst-libs/gst/resample/resample.c @@ -0,0 +1,530 @@ +/* Resampling library + * Copyright (C) <2001> David A. Schleef + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Library General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Library General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + + +#include +#include +#include +#include + +#include + +inline double sinc(double x) +{ + if(x==0)return 1; + return sin(x) / x; +} + +inline double window_func(double x) +{ + x = 1 - x*x; + return x*x; +} + +signed short double_to_s16(double x) +{ + if(x<-32768){ + printf("clipped\n"); + return -32768; + } + if(x>32767){ + printf("clipped\n"); + return -32767; + } + return rint(x); +} + +signed short double_to_s16_ppcasm(double x) +{ + if(x<-32768){ + return -32768; + } + if(x>32767){ + return -32767; + } + return rint(x); +} + +static void resample_sinc_ft(resample_t * r); + +void resample_init(resample_t * r) +{ + r->i_start = 0; + if(r->filter_length&1){ + r->o_start = 0; + }else{ + r->o_start = r->o_inc * 0.5; + } + + memset(r->acc, 0, sizeof(r->acc)); + + resample_reinit(r); +} + +void resample_reinit(resample_t * r) +{ + /* i_inc is the number of samples that the output increments for + * each input sample. o_inc is the opposite. */ + r->i_inc = (double) r->o_rate / r->i_rate; + r->o_inc = (double) r->i_rate / r->o_rate; + + r->halftaps = (r->filter_length - 1.0) * 0.5; + + switch (r->method) { + default: + case RESAMPLE_NEAREST: + r->scale = resample_nearest; + break; + case RESAMPLE_BILINEAR: + r->scale = resample_bilinear; + break; + case RESAMPLE_SINC_SLOW: + r->scale = resample_sinc; + break; + case RESAMPLE_SINC: + r->scale = resample_sinc_ft; + break; + } +} + +/* + * Prepare to be confused. + * + * We keep a "timebase" that is based on output samples. The zero + * of the timebase cooresponds to the next output sample that will + * be written. + * + * i_start is the "time" that corresponds to the first input sample + * in an incoming buffer. Since the output depends on input samples + * ahead in time, i_start will tend to be around halftaps. + * + * i_start_buf is the time of the first sample in the temporary + * buffer. + */ +void resample_scale(resample_t * r, void *i_buf, unsigned int i_size) +{ + int o_size; + + r->i_buf = i_buf; + + r->i_samples = i_size / 2 / r->channels; + + r->i_start_buf = r->i_start - r->filter_length * r->i_inc; + + /* i_start is the offset (in a given output sample) that is the + * beginning of the current input buffer */ + r->i_end = r->i_start + r->i_inc * r->i_samples; + + r->o_samples = floor(r->i_end - r->halftaps * r->i_inc); + + o_size = r->o_samples * r->channels * 2; + r->o_buf = r->get_buffer(r->priv, o_size); + + if(r->verbose){ + printf("resample_scale: i_buf=%p i_size=%d\n", + i_buf,i_size); + printf("resample_scale: i_samples=%d o_samples=%d i_inc=%g o_buf=%p\n", + r->i_samples, r->o_samples, r->i_inc, r->o_buf); + printf("resample_scale: i_start=%g i_end=%g o_start=%g\n", + r->i_start, r->i_end, r->o_start); + } + + if ((r->filter_length + r->i_samples)*2*2 > r->buffer_len) { + int size = (r->filter_length + r->i_samples) * sizeof(double) * 2; + + if(r->verbose){ + printf("resample temp buffer size=%d\n",size); + } + if(r->buffer)free(r->buffer); + r->buffer_len = size; + r->buffer = malloc(size); + memset(r->buffer, 0, size); + } + + if(r->channels==2){ + conv_double_short( + r->buffer + r->filter_length * sizeof(double) * 2, + r->i_buf, r->i_samples * 2); + }else{ + conv_double_short_dstr( + r->buffer + r->filter_length * sizeof(double) * 2, + r->i_buf, r->i_samples, sizeof(double) * 2); + } + + r->scale(r); + + memcpy(r->buffer, + r->buffer + r->i_samples * sizeof(double) * 2, + r->filter_length * sizeof(double) * 2); + + /* updating times */ + r->i_start += r->i_samples * r->i_inc; + r->o_start += r->o_samples * r->o_inc - r->i_samples; + + /* adjusting timebase zero */ + r->i_start -= r->o_samples; +} + +void resample_nearest(resample_t * r) +{ + signed short *i_ptr, *o_ptr; + int i_count = 0; + double a; + int i; + + i_ptr = (signed short *) r->i_buf; + o_ptr = (signed short *) r->o_buf; + + a = r->o_start; + i_count = 0; +#define SCALE_LOOP(COPY,INC) \ + for (i = 0; i < r->o_samples; i++) { \ + COPY; \ + a += r->o_inc; \ + while (a >= 1) { \ + a -= 1; \ + i_ptr+=INC; \ + i_count++; \ + } \ + o_ptr+=INC; \ + } + + switch (r->channels) { + case 1: + SCALE_LOOP(o_ptr[0] = i_ptr[0], 1); + break; + case 2: + SCALE_LOOP(o_ptr[0] = i_ptr[0]; + o_ptr[1] = i_ptr[1], 2); + break; + default: + { + int n, n_chan = r->channels; + + SCALE_LOOP(for (n = 0; n < n_chan; n++) o_ptr[n] = + i_ptr[n], n_chan); + } + } + if (i_count != r->i_samples) { + printf("handled %d in samples (expected %d)\n", i_count, + r->i_samples); + } +} + +void resample_bilinear(resample_t * r) +{ + signed short *i_ptr, *o_ptr; + int o_count = 0; + double b; + int i; + double acc0, acc1; + + i_ptr = (signed short *) r->i_buf; + o_ptr = (signed short *) r->o_buf; + + acc0 = r->acc[0]; + acc1 = r->acc[1]; + b = r->i_start; + for (i = 0; i < r->i_samples; i++) { + b += r->i_inc; + //printf("in %d\n",i_ptr[0]); + if(b>=2){ + printf("not expecting b>=2\n"); + } + if (b >= 1) { + acc0 += (1.0 - (b-r->i_inc)) * i_ptr[0]; + acc1 += (1.0 - (b-r->i_inc)) * i_ptr[1]; + + o_ptr[0] = rint(acc0); + //printf("out %d\n",o_ptr[0]); + o_ptr[1] = rint(acc1); + o_ptr += 2; + o_count++; + + b -= 1.0; + + acc0 = b * i_ptr[0]; + acc1 = b * i_ptr[1]; + } else { + acc0 += i_ptr[0] * r->i_inc; + acc1 += i_ptr[1] * r->i_inc; + } + i_ptr += 2; + } + r->acc[0] = acc0; + r->acc[1] = acc1; + + if (o_count != r->o_samples) { + printf("handled %d out samples (expected %d)\n", o_count, + r->o_samples); + } +} + +void resample_sinc_slow(resample_t * r) +{ + signed short *i_ptr, *o_ptr; + int i, j; + double c0, c1; + double a; + int start; + double center; + double weight; + + if (!r->buffer) { + int size = r->filter_length * 2 * r->channels; + + printf("resample temp buffer\n"); + r->buffer = malloc(size); + memset(r->buffer, 0, size); + } + + i_ptr = (signed short *) r->i_buf; + o_ptr = (signed short *) r->o_buf; + + a = r->i_start; +#define GETBUF(index,chan) (((index)<0) \ + ? ((short *)(r->buffer))[((index)+r->filter_length)*2+(chan)] \ + : i_ptr[(index)*2+(chan)]) + { + double sinx, cosx, sind, cosd; + double x, d; + double t; + + for (i = 0; i < r->o_samples; i++) { + start = floor(a) - r->filter_length; + center = a - r->halftaps; + x = M_PI * (start - center) * r->o_inc; + sinx = sin(M_PI * (start - center) * r->o_inc); + cosx = cos(M_PI * (start - center) * r->o_inc); + d = M_PI * r->o_inc; + sind = sin(M_PI * r->o_inc); + cosd = cos(M_PI * r->o_inc); + c0 = 0; + c1 = 0; + for (j = 0; j < r->filter_length; j++) { + weight = (x==0)?1:(sinx/x); +//printf("j %d sin %g cos %g\n",j,sinx,cosx); +//printf("j %d sin %g x %g sinc %g\n",j,sinx,x,weight); + c0 += weight * GETBUF((start + j), 0); + c1 += weight * GETBUF((start + j), 1); + t = cosx * cosd - sinx * sind; + sinx = cosx * sind + sinx * cosd; + cosx = t; + x += d; + } + o_ptr[0] = rint(c0); + o_ptr[1] = rint(c1); + o_ptr += 2; + a += r->o_inc; + } + } + + memcpy(r->buffer, + i_ptr + (r->i_samples - r->filter_length) * r->channels, + r->filter_length * 2 * r->channels); +} + +void resample_sinc(resample_t * r) +{ + double *ptr; + signed short *o_ptr; + int i, j; + double c0, c1; + double a; + int start; + double center; + double weight; + double x0, x, d; + double scale; + + ptr = (double *) r->buffer; + o_ptr = (signed short *) r->o_buf; + + /* scale provides a cutoff frequency for the low + * pass filter aspects of sinc(). scale=M_PI + * will cut off at the input frequency, which is + * good for up-sampling, but will cause aliasing + * for downsampling. Downsampling needs to be + * cut off at o_rate, thus scale=M_PI*r->i_inc. */ + /* actually, it needs to be M_PI*r->i_inc*r->i_inc. + * Need to research why. */ + scale = M_PI*r->i_inc; + for (i = 0; i < r->o_samples; i++) { + a = r->o_start + i * r->o_inc; + start = floor(a - r->halftaps); +//printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); + center = a; + //x = M_PI * (start - center) * r->o_inc; + //d = M_PI * r->o_inc; + //x = (start - center) * r->o_inc; + x0 = (start - center) * r->o_inc; + d = r->o_inc; + c0 = 0; + c1 = 0; + for (j = 0; j < r->filter_length; j++) { + x = x0 + d * j; + weight = sinc(x*scale*r->i_inc)*scale/M_PI; + weight *= window_func(x/r->halftaps*r->i_inc); + c0 += weight * ptr[(start + j + r->filter_length)*2 + 0]; + c1 += weight * ptr[(start + j + r->filter_length)*2 + 1]; + } + o_ptr[0] = double_to_s16(c0); + o_ptr[1] = double_to_s16(c1); + o_ptr += 2; + } +} + + + + +/* + * Resampling audio is best done using a sinc() filter. + * + * + * out[t] = Sum( in[t'] * sinc((t-t')/delta_t), all t') + * + * The immediate problem with this algorithm is that it involves a + * sum over an infinite number of input samples, both in the past + * and future. Note that even though sinc(x) is bounded by 1/x, + * and thus decays to 0 for large x, since sum(x,{x=0,1..,n}) diverges + * as log(n), we need to be careful about convergence. This is + * typically done by using a windowing function, which also makes + * the sum over a finite number of input samples. + * + * The next problem is computational: sinc(), and especially + * sinc() multiplied by a non-trivial windowing function is expensive + * to calculate, and also difficult to find SIMD optimizations. Since + * the time increment on input and output is different, it is not + * possible to use a FIR filter, because the taps would have to be + * recalculated for every t. + * + * To get around the expense of calculating sinc() for every point, + * we pre-calculate sinc() at a number of points, and then interpolate + * for the values we want in calculations. The interpolation method + * chosen is bi-cubic, which requires both the evalated function and + * its derivative at every pre-sampled point. Also, if the sampled + * points are spaced commensurate with the input delta_t, we notice + * that the interpolating weights are the same for every input point. + * This decreases the number of operations to 4 multiplies and 4 adds + * for each tap, regardless of the complexity of the filtering function. + * + * At this point, it is possible to rearrange the problem as the sum + * of 4 properly weghted FIR filters. Typical SIMD computation units + * are highly optimized for FIR filters, making long filter lengths + * reasonable. + */ + +static functable_t *ft; + +double out_tmp[10000]; + +static void resample_sinc_ft(resample_t * r) +{ + double *ptr; + signed short *o_ptr; + int i; + //int j; + double c0, c1; + //double a; + double start_f, start_x; + int start; + double center; + //double weight; + double x, d; + double scale; + int n = 4; + + scale = r->i_inc; // cutoff at 22050 + //scale = 1.0; // cutoff at 24000 + //scale = r->i_inc * 0.5; // cutoff at 11025 + + if(!ft){ + ft = malloc(sizeof(*ft)); + memset(ft,0,sizeof(*ft)); + + ft->len = (r->filter_length + 2) * n; + ft->offset = 1.0 / n; + ft->start = - ft->len * 0.5 * ft->offset; + + ft->func_x = functable_sinc; + ft->func_dx = functable_dsinc; + ft->scale = M_PI * scale; + + ft->func2_x = functable_window_std; + ft->func2_dx = functable_window_dstd; + ft->scale2 = 1.0 / r->halftaps; + + functable_init(ft); + + //printf("len=%d offset=%g start=%g\n",ft->len,ft->offset,ft->start); + } + + ptr = r->buffer; + o_ptr = (signed short *) r->o_buf; + + center = r->o_start; + start_x = center - r->halftaps; + start_f = floor(start_x); + start_x -= start_f; + start = start_f; + for (i = 0; i < r->o_samples; i++) { + //start_f = floor(center - r->halftaps); +//printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); + x = start_f - center; + d = 1; + c0 = 0; + c1 = 0; +//#define slow +#ifdef slow + for (j = 0; j < r->filter_length; j++) { + weight = functable_eval(ft,x)*scale; + //weight = sinc(M_PI * scale * x)*scale*r->i_inc; + //weight *= window_func(x / r->halftaps); + c0 += weight * ptr[(start + j + r->filter_length)*2 + 0]; + c1 += weight * ptr[(start + j + r->filter_length)*2 + 1]; + x += d; + } +#else + functable_fir2(ft, + &c0,&c1, + x, n, + ptr+(start + r->filter_length)*2, + r->filter_length); + c0 *= scale; + c1 *= scale; +#endif + + out_tmp[2 * i + 0] = c0; + out_tmp[2 * i + 1] = c1; + center += r->o_inc; + start_x += r->o_inc; + while(start_x>=1.0){ + start_f++; + start_x -= 1.0; + start++; + } + } + + if(r->channels==2){ + conv_short_double(r->o_buf,out_tmp,2 * r->o_samples); + }else{ + conv_short_double_sstr(r->o_buf,out_tmp,r->o_samples,2 * sizeof(double)); + } +} + -- cgit v1.2.1