5121c164c1e6228a69dfbe2e40e98c7d554b943e
[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 /*int speex_resample_float(SpeexResamplerState *st, int index, const float *in, int *in_len, float *out, int *out_len)*/
136 int speex_resample_float(SpeexResamplerState *st, const float *in, int len, float *out)
137 {
138    int j=0;
139    int N = st->filt_len;
140    int out_sample = 0;
141    while (1)
142    {
143       int j;
144       float sum=0;
145       /* Do the memory part */
146       if (st->type == SPEEX_RESAMPLER_DIRECT)
147       {
148          for (j=0;st->last_sample-N+1+j < 0;j++)
149          {
150             sum += st->mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
151          }
152          /* Do the new part */
153          for (;j<N;j++)
154          {
155             sum += in[st->last_sample-N+1+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
156          }
157       } else {
158          float accum[4] = {0.f,0.f, 0.f, 0.f};
159          float interp[4];
160          float alpha = ((float)st->samp_frac_num)/st->den_rate;
161          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
162          float frac = alpha*OVERSAMPLE - offset;
163          for (j=0;st->last_sample-N+1+j < 0;j++)
164          {
165             accum[0] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
166             accum[1] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
167             accum[2] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
168             accum[3] += st->mem[st->last_sample+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
169          }
170          /* Do the new part */
171          for (;j<N;j++)
172          {
173             accum[0] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
174             accum[1] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
175             accum[2] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
176             accum[3] += in[st->last_sample-N+1+j]*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
177          }
178          /* Compute cubic interpolation coefficients */
179          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
180          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
181          interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;
182          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
183          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
184          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
185       }
186       out[out_sample++] = sum;
187       
188       st->last_sample += st->num_rate/st->den_rate;
189       st->samp_frac_num += st->num_rate%st->den_rate;
190       if (st->samp_frac_num >= st->den_rate)
191       {
192          st->samp_frac_num -= st->den_rate;
193          st->last_sample++;
194       }
195       if (st->last_sample >= len)
196       {
197          st->last_sample -= len;
198          break;
199       }      
200    }
201    /* FIXME: Handle the case where the input signal has less samples than the memory */
202    for (j=0;j<st->filt_len-1;j++)
203       st->mem[j] = in[j+len-N+1];
204    return out_sample;
205 }
206
207 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den);
208
209 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride);
210
211 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride);
212
213 void speex_resample_skip_zeros(SpeexResamplerState *st);
214
215
216 #define NN 256
217
218 int main(int argc, char **argv)
219 {
220    int i;
221    short *in;
222    short *out;
223    float *fin, *fout;
224    SpeexResamplerState *st = speex_resampler_init(1, 8000, 13501, 1, 1);
225    in = speex_alloc(NN*sizeof(short));
226    out = speex_alloc(2*NN*sizeof(short));
227    fin = speex_alloc(NN*sizeof(float));
228    fout = speex_alloc(2*NN*sizeof(float));
229    while (1)
230    {
231       int out_num;
232       fread(in, sizeof(short), NN, stdin);
233       if (feof(stdin))
234          break;
235       for (i=0;i<NN;i++)
236          fin[i]=in[i];
237       out_num = speex_resample_float(st, fin, NN, fout);
238       for (i=0;i<2*NN;i++)
239          out[i]=floor(.5+fout[i]);
240       fwrite(out, sizeof(short), out_num, stdout);
241    }
242    speex_resampler_destroy(st);
243    speex_free(in);
244    speex_free(out);
245    speex_free(fin);
246    speex_free(fout);
247    return 0;
248 }
249