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