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