gstreamer/gst-libs/gst/resample/resample.c

/* Resampling library
 * Copyright (C) <2001> David A. Schleef <ds@schleef.org>
 *
 * 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 <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <resample.h>

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));
	}
}