updated API as discussed with Lennart and Mikko
[speexdsp.git] / libspeex / resample.c
1 /* Copyright (C) 2007 Jean-Marc Valin
2       
3    File: resample.c
4    Resampling code
5       
6    The design goals of this code are:
7       - Very fast algorithm
8       - Low memory requirement
9       - Good *perceptual* quality (and not best SNR)
10
11    Redistribution and use in source and binary forms, with or without
12    modification, are permitted provided that the following conditions are
13    met:
14
15    1. Redistributions of source code must retain the above copyright notice,
16    this list of conditions and the following disclaimer.
17
18    2. Redistributions in binary form must reproduce the above copyright
19    notice, this list of conditions and the following disclaimer in the
20    documentation and/or other materials provided with the distribution.
21
22    3. The name of the author may not be used to endorse or promote products
23    derived from this software without specific prior written permission.
24
25    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
26    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
27    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
29    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
30    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
31    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
32    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
33    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35    POSSIBILITY OF SUCH DAMAGE.
36 */
37
38 #include "misc.h"
39 #include <math.h>
40 #include <stdio.h>
41             
42 /*#define float double*/
43 #define FILTER_SIZE 64
44 #define OVERSAMPLE 8
45
46 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
47
48 typedef struct {
49    int in_rate;
50    int out_rate;
51    int num_rate;
52    int den_rate;
53    int last_sample;
54    int samp_frac_num;
55    int filt_len;
56    float *mem;
57    float *sinc_table;
58    SpeexSincType type;
59 } SpeexResamplerState;
60
61 static float sinc(float x, int N)
62 {
63    /*fprintf (stderr, "%f ", x);*/
64    if (fabs(x)<1e-6)
65       return 1;
66    else if (fabs(x) > .5f*N)
67       return 0;
68    /*FIXME: Can it really be any slower than this? */
69    return sin(M_PI*x)/(M_PI*x) * (.5+.5*cos(2*x*M_PI/N));
70 }
71
72 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
73 {
74    int fact, i;
75    float cutoff;
76    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
77    st->in_rate = in_rate;
78    st->out_rate = out_rate;
79    st->num_rate = in_rate;
80    st->den_rate = out_rate;
81    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
82    for (fact=2;fact<=sqrt(MAX32(in_rate, out_rate));fact++)
83    {
84       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
85       {
86          st->num_rate /= fact;
87          st->den_rate /= fact;
88       }
89    }
90    st->last_sample = 0;
91    st->filt_len = FILTER_SIZE;
92    st->mem = (float*)speex_alloc((st->filt_len-1) * sizeof(float));
93    for (i=0;i<st->filt_len-1;i++)
94       st->mem[i] = 0;
95    /* FIXME: Is there a danger of overflow? */
96    if (in_rate*out_rate_den > out_rate*in_rate_den)
97    {
98       /* down-sampling */
99       cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
100    } else {
101       /* up-sampling */
102       cutoff = .97;
103    }
104    if (st->den_rate <= OVERSAMPLE)
105    {
106       st->sinc_table = (float *)speex_alloc(st->filt_len*st->den_rate*sizeof(float));
107       for (i=0;i<st->den_rate;i++)
108       {
109          int j;
110          for (j=0;j<st->filt_len;j++)
111          {
112             st->sinc_table[i*st->filt_len+j] = sinc(cutoff*((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len);
113          }
114       }
115       st->type = SPEEX_RESAMPLER_DIRECT;
116       fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);
117    } else {
118       st->sinc_table = (float *)speex_alloc(st->filt_len*st->den_rate*sizeof(float));
119       for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
120          st->sinc_table[i+4] = sinc(cutoff*(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
121       st->type = SPEEX_RESAMPLER_INTERPOLATE;
122       fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);
123    }
124    return st;
125 }
126
127 void speex_resampler_destroy(SpeexResamplerState *st)
128 {
129    speex_free(st->mem);
130    if (st->sinc_table)
131       speex_free(st->sinc_table);
132    speex_free(st);
133 }
134
135 void speex_resample_float(SpeexResamplerState *st, int index, const float *in, int *in_len, float *out, int *out_len)
136 {
137    int j=0;
138    int N = st->filt_len;
139    int out_sample = 0;
140    while (1)
141    {
142       int j;
143       float sum=0;
144       
145       if (st->type == SPEEX_RESAMPLER_DIRECT)
146       {
147          /* We already have all the filter coefficients pre-computed in the table */
148          /* Do the memory part */
149          for (j=0;st->last_sample-N+1+j < 0;j++)
150          {
151             sum += st->mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
152          }
153          /* Do the new part */
154          for (;j<N;j++)
155          {
156             sum += in[st->last_sample-N+1+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
157          }
158       } else {
159          /* We need to interpolate the sinc filter */
160          float accum[4] = {0.f,0.f, 0.f, 0.f};
161          float interp[4];
162          float alpha = ((float)st->samp_frac_num)/st->den_rate;
163          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
164          float frac = alpha*OVERSAMPLE - offset;
165          /* This code is written like this to make it easy to optimise with SIMD.
166             For most DSPs, it would be best to split the loops in two because most DSPs 
167             have only two accumulators */
168          for (j=0;st->last_sample-N+1+j < 0;j++)
169          {
170             accum[0] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
171             accum[1] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
172             accum[2] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
173             accum[3] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
174          }
175          /* Do the new part */
176          for (;j<N;j++)
177          {
178             accum[0] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
179             accum[1] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
180             accum[2] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
181             accum[3] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
182          }
183          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
184             but I know it's MMSE-optimal on a sinc */
185          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
186          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
187          interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;
188          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
189          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
190          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
191       }
192       out[out_sample++] = sum;
193       
194       st->last_sample += st->num_rate/st->den_rate;
195       st->samp_frac_num += st->num_rate%st->den_rate;
196       if (st->samp_frac_num >= st->den_rate)
197       {
198          st->samp_frac_num -= st->den_rate;
199          st->last_sample++;
200       }
201       if (st->last_sample >= *in_len || out_sample >= *out_len)
202          break;
203    }
204    if (st->last_sample < *in_len)
205       *in_len = st->last_sample;
206    *out_len = out_sample;
207    st->last_sample -= *in_len;
208    
209    /* FIXME: The details of this are untested */
210    for (j=0;j<N-1-*in_len;j++)
211       st->mem[j] = st->mem[j-*in_len];
212    for (;j<N-1;j++)
213       st->mem[j] = in[j+*in_len-N+1];
214    
215 }
216
217 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den);
218
219 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride);
220
221 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride);
222
223 void speex_resample_skip_zeros(SpeexResamplerState *st);
224
225
226 #define NN 256
227
228 int main(int argc, char **argv)
229 {
230    int i;
231    short *in;
232    short *out;
233    float *fin, *fout;
234    SpeexResamplerState *st = speex_resampler_init(1, 8000, 13501, 1, 1);
235    in = speex_alloc(NN*sizeof(short));
236    out = speex_alloc(2*NN*sizeof(short));
237    fin = speex_alloc(NN*sizeof(float));
238    fout = speex_alloc(2*NN*sizeof(float));
239    while (1)
240    {
241       int in_len;
242       int out_len;
243       fread(in, sizeof(short), NN, stdin);
244       if (feof(stdin))
245          break;
246       for (i=0;i<NN;i++)
247          fin[i]=in[i];
248       in_len = NN;
249       out_len = 2*NN;
250       speex_resample_float(st, 0, fin, &in_len, fout, &out_len);
251       for (i=0;i<out_len;i++)
252          out[i]=floor(.5+fout[i]);
253       fwrite(out, sizeof(short), out_len, stdout);
254    }
255    speex_resampler_destroy(st);
256    speex_free(in);
257    speex_free(out);
258    speex_free(fin);
259    speex_free(fout);
260    return 0;
261 }
262