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