fixed-point: Got rid of the three last float bits in the
[speexdsp.git] / libspeex / filterbank.c
1 /* Copyright (C) 2006 Jean-Marc Valin */
2 /**
3    @file filterbank.c
4    @brief Converting between psd and filterbank
5  */
6 /*
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33
34 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include "filterbank.h"
39 #include "arch.h"
40 #include <math.h>
41 #include "math_approx.h"
42 #include "os_support.h"
43       
44 #ifdef FIXED_POINT
45
46 #define toBARK(n)   (MULT16_16(26829,spx_atan(SHR32(MULT16_16(97,n),2))) + MULT16_16(4588,spx_atan(MULT16_32_Q15(20,MULT16_16(n,n)))) + MULT16_16(3355,n))
47       
48 #else
49 #define toBARK(n)   (13.1f*atan(.00074f*(n))+2.24f*atan((n)*(n)*1.85e-8f)+1e-4f*(n))
50 #endif
51        
52 #define toMEL(n)    (2595.f*log10(1.f+(n)/700.f))
53
54 FilterBank *filterbank_new(int banks, spx_word32_t sampling, int len, int type)
55 {
56    FilterBank *bank;
57    spx_word32_t df;
58    spx_word32_t max_mel, mel_interval;
59    int i;
60    int id1;
61    int id2;
62    df = DIV32(SHL32(sampling,15),MULT16_16(2,len));
63    max_mel = toBARK(EXTRACT16(sampling/2));
64    mel_interval = PDIV32(max_mel,banks-1);
65    
66    bank = (FilterBank*)speex_alloc(sizeof(FilterBank));
67    bank->nb_banks = banks;
68    bank->len = len;
69    bank->bank_left = (int*)speex_alloc(len*sizeof(int));
70    bank->bank_right = (int*)speex_alloc(len*sizeof(int));
71    bank->filter_left = (spx_word16_t*)speex_alloc(len*sizeof(spx_word16_t));
72    bank->filter_right = (spx_word16_t*)speex_alloc(len*sizeof(spx_word16_t));
73    /* Think I can safely disable normalisation that for fixed-point (and probably float as well) */
74 #ifndef FIXED_POINT
75    bank->scaling = (float*)speex_alloc(banks*sizeof(float));
76 #endif
77    for (i=0;i<len;i++)
78    {
79       spx_word16_t curr_freq;
80       spx_word32_t mel;
81       spx_word16_t val;
82       curr_freq = EXTRACT16(MULT16_32_P15(i,df));
83       mel = toBARK(curr_freq);
84       if (mel > max_mel)
85          break;
86 #ifdef FIXED_POINT
87       id1 = DIV32(mel,mel_interval);
88 #else      
89       id1 = (int)(floor(mel/mel_interval));
90 #endif
91       if (id1>banks-2)
92       {
93          id1 = banks-2;
94          val = Q15_ONE;
95       } else {
96          val = DIV32_16(mel - id1*mel_interval,EXTRACT16(PSHR32(mel_interval,15)));
97       }
98       id2 = id1+1;
99       bank->bank_left[i] = id1;
100       bank->filter_left[i] = SUB16(Q15_ONE,val);
101       bank->bank_right[i] = id2;
102       bank->filter_right[i] = val;
103    }
104    
105    /* Think I can safely disable normalisation for fixed-point (and probably float as well) */
106 #ifndef FIXED_POINT
107    for (i=0;i<bank->nb_banks;i++)
108       bank->scaling[i] = 0;
109    for (i=0;i<bank->len;i++)
110    {
111       int id = bank->bank_left[i];
112       bank->scaling[id] += bank->filter_left[i];
113       id = bank->bank_right[i];
114       bank->scaling[id] += bank->filter_right[i];
115    }
116    for (i=0;i<bank->nb_banks;i++)
117       bank->scaling[i] = Q15_ONE/(bank->scaling[i]);
118 #endif
119    return bank;
120 }
121
122 void filterbank_destroy(FilterBank *bank)
123 {
124    speex_free(bank->bank_left);
125    speex_free(bank->bank_right);
126    speex_free(bank->filter_left);
127    speex_free(bank->filter_right);
128 #ifndef FIXED_POINT
129    speex_free(bank->scaling);
130 #endif
131    speex_free(bank);
132 }
133
134 void filterbank_compute_bank32(FilterBank *bank, spx_word32_t *ps, spx_word32_t *mel)
135 {
136    int i;
137    for (i=0;i<bank->nb_banks;i++)
138       mel[i] = 0;
139
140    for (i=0;i<bank->len;i++)
141    {
142       int id;
143       id = bank->bank_left[i];
144       mel[id] += MULT16_32_P15(bank->filter_left[i],ps[i]);
145       id = bank->bank_right[i];
146       mel[id] += MULT16_32_P15(bank->filter_right[i],ps[i]);
147    }
148    /* Think I can safely disable normalisation that for fixed-point (and probably float as well) */
149 #ifndef FIXED_POINT
150    /*for (i=0;i<bank->nb_banks;i++)
151       mel[i] = MULT16_32_P15(Q15(bank->scaling[i]),mel[i]);
152    */
153 #endif
154 }
155
156 void filterbank_compute_psd16(FilterBank *bank, spx_word16_t *mel, spx_word16_t *ps)
157 {
158    int i;
159    for (i=0;i<bank->len;i++)
160    {
161       spx_word32_t tmp;
162       int id1, id2;
163       id1 = bank->bank_left[i];
164       id2 = bank->bank_right[i];
165       tmp = MULT16_16(mel[id1],bank->filter_left[i]);
166       tmp += MULT16_16(mel[id2],bank->filter_right[i]);
167       ps[i] = EXTRACT16(PSHR32(tmp,15));
168    }
169 }
170
171
172 #ifndef FIXED_POINT
173 void filterbank_compute_bank(FilterBank *bank, float *ps, float *mel)
174 {
175    int i;
176    for (i=0;i<bank->nb_banks;i++)
177       mel[i] = 0;
178
179    for (i=0;i<bank->len;i++)
180    {
181       int id = bank->bank_left[i];
182       mel[id] += bank->filter_left[i]*ps[i];
183       id = bank->bank_right[i];
184       mel[id] += bank->filter_right[i]*ps[i];
185    }
186    for (i=0;i<bank->nb_banks;i++)
187       mel[i] *= bank->scaling[i];
188 }
189
190 void filterbank_compute_psd(FilterBank *bank, float *mel, float *ps)
191 {
192    int i;
193    for (i=0;i<bank->len;i++)
194    {
195       int id = bank->bank_left[i];
196       ps[i] = mel[id]*bank->filter_left[i];
197       id = bank->bank_right[i];
198       ps[i] += mel[id]*bank->filter_right[i];
199    }
200 }
201
202 void filterbank_psy_smooth(FilterBank *bank, float *ps, float *mask)
203 {
204    /* Low freq slope: 14 dB/Bark*/
205    /* High freq slope: 9 dB/Bark*/
206    /* Noise vs tone: 5 dB difference */
207    /* FIXME: Temporary kludge */
208    float bark[100];
209    int i;
210    /* Assumes 1/3 Bark resolution */
211    float decay_low = 0.34145f;
212    float decay_high = 0.50119f;
213    filterbank_compute_bank(bank, ps, bark);
214    for (i=1;i<bank->nb_banks;i++)
215    {
216       /*float decay_high = 13-1.6*log10(bark[i-1]);
217       decay_high = pow(10,(-decay_high/30.f));*/
218       bark[i] = bark[i] + decay_high*bark[i-1];
219    }
220    for (i=bank->nb_banks-2;i>=0;i--)
221    {
222       bark[i] = bark[i] + decay_low*bark[i+1];
223    }
224    filterbank_compute_psd(bank, bark, mask);
225 }
226
227 #endif