implemented input/output stride. Not yet tested though.
[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    if (st->sinc_table)
135       speex_free(st->sinc_table);
136    speex_free(st);
137 }
138
139 void speex_resample_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
140 {
141    int j=0;
142    int N = st->filt_len;
143    int out_sample = 0;
144    float *mem;
145    mem = st->mem + channel_index * (N-1);
146    while (1)
147    {
148       int j;
149       float sum=0;
150       
151       if (st->type == SPEEX_RESAMPLER_DIRECT)
152       {
153          /* We already have all the filter coefficients pre-computed in the table */
154          const float *ptr;
155          /* Do the memory part */
156          for (j=0;st->last_sample-N+1+j < 0;j++)
157          {
158             sum += mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
159          }
160          
161          /* Do the new part */
162          ptr = in+st->last_sample-N+1+j;
163          for (;j<N;j++)
164          {
165             sum += *ptr*st->sinc_table[st->samp_frac_num*st->filt_len+j];
166             ptr += st->in_stride;
167          }
168       } else {
169          /* We need to interpolate the sinc filter */
170          float accum[4] = {0.f,0.f, 0.f, 0.f};
171          float interp[4];
172          const float *ptr;
173          float alpha = ((float)st->samp_frac_num)/st->den_rate;
174          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
175          float frac = alpha*OVERSAMPLE - offset;
176          /* This code is written like this to make it easy to optimise with SIMD.
177             For most DSPs, it would be best to split the loops in two because most DSPs 
178             have only two accumulators */
179          for (j=0;st->last_sample-N+1+j < 0;j++)
180          {
181             float curr_mem = mem[st->last_sample+j];
182             accum[0] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
183             accum[1] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
184             accum[2] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
185             accum[3] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
186          }
187          ptr = in+st->last_sample-N+1+j;
188          /* Do the new part */
189          for (;j<N;j++)
190          {
191             float curr_in = *ptr;
192             ptr += st->in_stride;
193             accum[0] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
194             accum[1] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
195             accum[2] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
196             accum[3] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
197          }
198          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
199             but I know it's MMSE-optimal on a sinc */
200          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
201          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
202          interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;
203          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
204          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
205          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
206       }
207       *out = sum;
208       out += st->out_stride;
209       out_sample++;
210       st->last_sample += st->num_rate/st->den_rate;
211       st->samp_frac_num += st->num_rate%st->den_rate;
212       if (st->samp_frac_num >= st->den_rate)
213       {
214          st->samp_frac_num -= st->den_rate;
215          st->last_sample++;
216       }
217       if (st->last_sample >= *in_len || out_sample >= *out_len)
218          break;
219    }
220    if (st->last_sample < *in_len)
221       *in_len = st->last_sample;
222    *out_len = out_sample;
223    st->last_sample -= *in_len;
224    
225    /* FIXME: The details of this are untested */
226    for (j=0;j<N-1-*in_len;j++)
227       mem[j] = mem[j-*in_len];
228    for (;j<N-1;j++)
229       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
230    
231 }
232
233 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
234 {
235 }
236
237 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
238 {
239    st->in_stride = stride;
240 }
241
242 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
243 {
244    st->out_stride = stride;
245 }
246
247 void speex_resample_skip_zeros(SpeexResamplerState *st)
248 {
249 }
250
251
252 #define NN 256
253
254 int main(int argc, char **argv)
255 {
256    int i;
257    short *in;
258    short *out;
259    float *fin, *fout;
260    SpeexResamplerState *st = speex_resampler_init(1, 8000, 13501, 1, 1);
261    in = speex_alloc(NN*sizeof(short));
262    out = speex_alloc(2*NN*sizeof(short));
263    fin = speex_alloc(NN*sizeof(float));
264    fout = speex_alloc(2*NN*sizeof(float));
265    while (1)
266    {
267       int in_len;
268       int out_len;
269       fread(in, sizeof(short), NN, stdin);
270       if (feof(stdin))
271          break;
272       for (i=0;i<NN;i++)
273          fin[i]=in[i];
274       in_len = NN;
275       out_len = 2*NN;
276       speex_resample_float(st, 0, fin, &in_len, fout, &out_len);
277       for (i=0;i<out_len;i++)
278          out[i]=floor(.5+fout[i]);
279       fwrite(out, sizeof(short), out_len, stdout);
280    }
281    speex_resampler_destroy(st);
282    speex_free(in);
283    speex_free(out);
284    speex_free(fin);
285    speex_free(fout);
286    return 0;
287 }
288