src/lib/doslib/ext/speex/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