changing the sampling rate should now work
[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    int    sinc_table_length;
59    int    in_stride;
60    int    out_stride;
61    SpeexSincType type;
62 } SpeexResamplerState;
63
64 static float sinc(float x, int N)
65 {
66    /*fprintf (stderr, "%f ", x);*/
67    if (fabs(x)<1e-6)
68       return 1;
69    else if (fabs(x) > .5f*N)
70       return 0;
71    /*FIXME: Can it really be any slower than this? */
72    return sin(M_PI*x)/(M_PI*x) * (.5+.5*cos(2*x*M_PI/N));
73 }
74
75 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den);
76
77 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
78 {
79    int i;
80    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
81    st->in_rate = 0;
82    st->out_rate = 0;
83    st->num_rate = 0;
84    st->den_rate = 0;
85
86    st->last_sample = 0;
87    st->filt_len = FILTER_SIZE;
88    st->mem = (float*)speex_alloc(nb_channels*(st->filt_len-1) * sizeof(float));
89    for (i=0;i<nb_channels*(st->filt_len-1);i++)
90       st->mem[i] = 0;
91    st->sinc_table_length = 0;
92    
93    speex_resample_set_rate(st, in_rate, out_rate, in_rate_den, out_rate_den);
94
95    st->in_stride = 1;
96    st->out_stride = 1;
97    return st;
98 }
99
100 void speex_resampler_destroy(SpeexResamplerState *st)
101 {
102    speex_free(st->mem);
103    speex_free(st->sinc_table);
104    speex_free(st);
105 }
106
107 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
108 {
109    int j=0;
110    int N = st->filt_len;
111    int out_sample = 0;
112    float *mem;
113    mem = st->mem + channel_index * (N-1);
114    while (1)
115    {
116       int j;
117       float sum=0;
118       
119       if (st->type == SPEEX_RESAMPLER_DIRECT)
120       {
121          /* We already have all the filter coefficients pre-computed in the table */
122          const float *ptr;
123          /* Do the memory part */
124          for (j=0;st->last_sample-N+1+j < 0;j++)
125          {
126             sum += mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
127          }
128          
129          /* Do the new part */
130          ptr = in+st->last_sample-N+1+j;
131          for (;j<N;j++)
132          {
133             sum += *ptr*st->sinc_table[st->samp_frac_num*st->filt_len+j];
134             ptr += st->in_stride;
135          }
136       } else {
137          /* We need to interpolate the sinc filter */
138          float accum[4] = {0.f,0.f, 0.f, 0.f};
139          float interp[4];
140          const float *ptr;
141          float alpha = ((float)st->samp_frac_num)/st->den_rate;
142          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
143          float frac = alpha*OVERSAMPLE - offset;
144          /* This code is written like this to make it easy to optimise with SIMD.
145             For most DSPs, it would be best to split the loops in two because most DSPs 
146             have only two accumulators */
147          for (j=0;st->last_sample-N+1+j < 0;j++)
148          {
149             float curr_mem = mem[st->last_sample+j];
150             accum[0] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
151             accum[1] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
152             accum[2] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
153             accum[3] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
154          }
155          ptr = in+st->last_sample-N+1+j;
156          /* Do the new part */
157          for (;j<N;j++)
158          {
159             float curr_in = *ptr;
160             ptr += st->in_stride;
161             accum[0] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
162             accum[1] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
163             accum[2] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
164             accum[3] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
165          }
166          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
167             but I know it's MMSE-optimal on a sinc */
168          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
169          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
170          interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;
171          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
172          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
173          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
174       }
175       *out = sum;
176       out += st->out_stride;
177       out_sample++;
178       st->last_sample += st->num_rate/st->den_rate;
179       st->samp_frac_num += st->num_rate%st->den_rate;
180       if (st->samp_frac_num >= st->den_rate)
181       {
182          st->samp_frac_num -= st->den_rate;
183          st->last_sample++;
184       }
185       if (st->last_sample >= *in_len || out_sample >= *out_len)
186          break;
187    }
188    if (st->last_sample < *in_len)
189       *in_len = st->last_sample;
190    *out_len = out_sample;
191    st->last_sample -= *in_len;
192    
193    /* FIXME: The details of this are untested */
194    for (j=0;j<N-1-*in_len;j++)
195       mem[j] = mem[j-*in_len];
196    for (;j<N-1;j++)
197       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
198    
199 }
200
201 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
202 {
203    float cutoff;
204    int i, fact;
205    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == in_rate && st->den_rate == out_rate)
206       return;
207    
208    st->in_rate = in_rate;
209    st->out_rate = out_rate;
210    st->num_rate = in_rate;
211    st->den_rate = out_rate;
212    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
213    for (fact=2;fact<=sqrt(MAX32(in_rate, out_rate));fact++)
214    {
215       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
216       {
217          st->num_rate /= fact;
218          st->den_rate /= fact;
219       }
220    }
221    
222    /* FIXME: Is there a danger of overflow? */
223    if (in_rate*out_rate_den > out_rate*in_rate_den)
224    {
225       /* down-sampling */
226       cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
227    } else {
228       /* up-sampling */
229       cutoff = .97;
230    }
231    
232    /* Choose the resampling type that requires the least amount of memory */
233    if (st->den_rate <= OVERSAMPLE)
234    {
235       if (!st->sinc_table)
236          st->sinc_table = (float *)speex_alloc(st->filt_len*st->den_rate*sizeof(float));
237       else if (st->sinc_table_length < st->filt_len*st->den_rate)
238       {
239          st->sinc_table = (float *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(float));
240          st->sinc_table_length = st->filt_len*st->den_rate;
241       }
242       for (i=0;i<st->den_rate;i++)
243       {
244          int j;
245          for (j=0;j<st->filt_len;j++)
246          {
247             st->sinc_table[i*st->filt_len+j] = sinc(cutoff*((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len);
248          }
249       }
250       st->type = SPEEX_RESAMPLER_DIRECT;
251       fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);
252    } else {
253       if (!st->sinc_table)
254          st->sinc_table = (float *)speex_alloc((st->filt_len*OVERSAMPLE+8)*sizeof(float));
255       else if (st->sinc_table_length < st->filt_len*OVERSAMPLE+8)
256       {
257          st->sinc_table = (float *)speex_realloc(st->sinc_table,(st->filt_len*OVERSAMPLE+8)*sizeof(float));
258          st->sinc_table_length = st->filt_len*OVERSAMPLE+8;
259       }
260       for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
261          st->sinc_table[i+4] = sinc(cutoff*(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
262       st->type = SPEEX_RESAMPLER_INTERPOLATE;
263       fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);
264    }
265
266 }
267
268 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
269 {
270    st->in_stride = stride;
271 }
272
273 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
274 {
275    st->out_stride = stride;
276 }
277
278 void speex_resample_skip_zeros(SpeexResamplerState *st)
279 {
280 }
281
282 void speex_resample_reset_mem(SpeexResamplerState *st)
283 {
284 }
285
286 #define NN 256
287
288 int main(int argc, char **argv)
289 {
290    int i;
291    short *in;
292    short *out;
293    float *fin, *fout;
294    SpeexResamplerState *st = speex_resampler_init(1, 8000, 12000, 1, 1);
295    speex_resample_set_rate(st, 8000, 13501, 1, 1);
296    
297    in = speex_alloc(NN*sizeof(short));
298    out = speex_alloc(2*NN*sizeof(short));
299    fin = speex_alloc(NN*sizeof(float));
300    fout = speex_alloc(2*NN*sizeof(float));
301    while (1)
302    {
303       int in_len;
304       int out_len;
305       fread(in, sizeof(short), NN, stdin);
306       if (feof(stdin))
307          break;
308       for (i=0;i<NN;i++)
309          fin[i]=in[i];
310       in_len = NN;
311       out_len = 2*NN;
312       speex_resampler_process_float(st, 0, fin, &in_len, fout, &out_len);
313       for (i=0;i<out_len;i++)
314          out[i]=floor(.5+fout[i]);
315       fwrite(out, sizeof(short), out_len, stdout);
316    }
317    speex_resampler_destroy(st);
318    speex_free(in);
319    speex_free(out);
320    speex_free(fin);
321    speex_free(fout);
322    return 0;
323 }
324