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