plugins/audio/Speex/libspeex/filters.c

   1 /* Copyright (C) 2002 Jean-Marc Valin
   2    File: filters.c
   3    Various analysis/synthesis filters
   4
   5    Redistribution and use in source and binary forms, with or without
   6    modification, are permitted provided that the following conditions
   7    are met:
   8
   9    - Redistributions of source code must retain the above copyright
  10    notice, this list of conditions and the following disclaimer.
  11
  12    - Redistributions in binary form must reproduce the above copyright
  13    notice, this list of conditions and the following disclaimer in the
  14    documentation and/or other materials provided with the distribution.
  15
  16    - Neither the name of the Xiph.org Foundation nor the names of its
  17    contributors may be used to endorse or promote products derived from
  18    this software without specific prior written permission.
  19
  20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  31 */
  32
  33 #include "filters.h"
  34 #include "stack_alloc.h"
  35 #include <math.h>
  36
  37
  38 void bw_lpc(float gamma, float *lpc_in, float *lpc_out, int order)
  39 {
  40    int i;
  41    float tmp=1;
  42    for (i=0;i<order+1;i++)
  43    {
  44       lpc_out[i] = tmp * lpc_in[i];
  45       tmp *= gamma;
  46    }
  47 }
  48
  49 #ifdef _USE_SSE
  50 #include "filters_sse.h"
  51 #else
  52 void filter_mem2(float *x, float *num, float *den, float *y, int N, int ord, float *mem)
  53 {
  54    int i,j;
  55    float xi,yi;
  56    for (i=0;i<N;i++)
  57    {
  58       xi=x[i];
  59       y[i] = num[0]*xi + mem[0];
  60       yi=y[i];
  61       for (j=0;j<ord-1;j++)
  62       {
  63          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
  64       }
  65       mem[ord-1] = num[ord]*xi - den[ord]*yi;
  66    }
  67 }
  68
  69
  70 void iir_mem2(float *x, float *den, float *y, int N, int ord, float *mem)
  71 {
  72    int i,j;
  73    for (i=0;i<N;i++)
  74    {
  75       y[i] = x[i] + mem[0];
  76       for (j=0;j<ord-1;j++)
  77       {
  78          mem[j] = mem[j+1] - den[j+1]*y[i];
  79       }
  80       mem[ord-1] = - den[ord]*y[i];
  81    }
  82 }
  83 #endif
  84
  85 void fir_mem2(float *x, float *num, float *y, int N, int ord, float *mem)
  86 {
  87    int i,j;
  88    float xi;
  89    for (i=0;i<N;i++)
  90    {
  91       xi=x[i];
  92       y[i] = num[0]*xi + mem[0];
  93       for (j=0;j<ord-1;j++)
  94       {
  95          mem[j] = mem[j+1] + num[j+1]*xi;
  96       }
  97       mem[ord-1] = num[ord]*xi;
  98    }
  99 }
 100
 101 void syn_percep_zero(float *xx, float *ak, float *awk1, float *awk2, float *y, int N, int ord, char *stack)
 102 {
 103    int i;
 104    float *mem = PUSH(stack,ord, float);
 105    for (i=0;i<ord;i++)
 106       mem[i]=0;
 107    filter_mem2(xx, awk1, ak, y, N, ord, mem);
 108    for (i=0;i<ord;i++)
 109      mem[i]=0;
 110    iir_mem2(y, awk2, y, N, ord, mem);
 111 }
 112
 113 void residue_percep_zero(float *xx, float *ak, float *awk1, float *awk2, float *y, int N, int ord, char *stack)
 114 {
 115    int i;
 116    float *mem = PUSH(stack,ord, float);
 117    for (i=0;i<ord;i++)
 118       mem[i]=0;
 119    filter_mem2(xx, ak, awk1, y, N, ord, mem);
 120    for (i=0;i<ord;i++)
 121      mem[i]=0;
 122    fir_mem2(y, awk2, y, N, ord, mem);
 123 }
 124
 125
 126 void qmf_decomp(float *xx, float *aa, float *y1, float *y2, int N, int M, float *mem, char *stack)
 127 {
 128    int i,j,k,M2;
 129    float *a;
 130    float *x;
 131    float *x2;
 132
 133    a = PUSH(stack, M, float);
 134    x = PUSH(stack, N+M-1, float);
 135    x2=x+M-1;
 136    M2=M>>1;
 137    for (i=0;i<M;i++)
 138       a[M-i-1]=aa[i];
 139    for (i=0;i<M-1;i++)
 140       x[i]=mem[M-i-2];
 141    for (i=0;i<N;i++)
 142       x[i+M-1]=xx[i];
 143    for (i=0,k=0;i<N;i+=2,k++)
 144    {
 145       y1[k]=0;
 146       y2[k]=0;
 147       for (j=0;j<M2;j++)
 148       {
 149          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
 150          y2[k]-=a[j]*(x[i+j]-x2[i-j]);
 151          j++;
 152          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
 153          y2[k]+=a[j]*(x[i+j]-x2[i-j]);
 154       }
 155    }
 156    for (i=0;i<M-1;i++)
 157      mem[i]=xx[N-i-1];
 158 }
 159
 160 /* By segher */
 161 void fir_mem_up(float *x, float *a, float *y, int N, int M, float *mem, char *stack)
 162    /* assumptions:
 163       all odd x[i] are zero -- well, actually they are left out of the array now
 164       N and M are multiples of 4 */
 165 {
 166    int i, j;
 167    float *xx=PUSH(stack, M+N-1, float);
 168
 169    for (i = 0; i < N/2; i++)
 170       xx[2*i] = x[N/2-1-i];
 171    for (i = 0; i < M - 1; i += 2)
 172       xx[N+i] = mem[i+1];
 173
 174    for (i = 0; i < N; i += 4) {
 175       float y0, y1, y2, y3;
 176       float x0;
 177
 178       y0 = y1 = y2 = y3 = 0.f;
 179       x0 = xx[N-4-i];
 180
 181       for (j = 0; j < M; j += 4) {
 182          float x1;
 183          float a0, a1;
 184
 185          a0 = a[j];
 186          a1 = a[j+1];
 187          x1 = xx[N-2+j-i];
 188
 189          y0 += a0 * x1;
 190          y1 += a1 * x1;
 191          y2 += a0 * x0;
 192          y3 += a1 * x0;
 193
 194          a0 = a[j+2];
 195          a1 = a[j+3];
 196          x0 = xx[N+j-i];
 197
 198          y0 += a0 * x0;
 199          y1 += a1 * x0;
 200          y2 += a0 * x1;
 201          y3 += a1 * x1;
 202       }
 203       y[i] = y0;
 204       y[i+1] = y1;
 205       y[i+2] = y2;
 206       y[i+3] = y3;
 207    }
 208
 209    for (i = 0; i < M - 1; i += 2)
 210       mem[i+1] = xx[i];
 211 }
 212
 213
 214 void comp_filter_mem_init (CombFilterMem *mem)
 215 {
 216    mem->last_pitch=0;
 217    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
 218    mem->smooth_gain=1;
 219 }
 220
 221 void comb_filter(
 222 float *exc,          /*decoded excitation*/
 223 float *new_exc,      /*enhanced excitation*/
 224 float *ak,           /*LPC filter coefs*/
 225 int p,               /*LPC order*/
 226 int nsf,             /*sub-frame size*/
 227 int pitch,           /*pitch period*/
 228 float *pitch_gain,   /*pitch gain (3-tap)*/
 229 float  comb_gain,    /*gain of comb filter*/
 230 CombFilterMem *mem
 231 )
 232 {
 233    int i;
 234    float exc_energy=0, new_exc_energy=0;
 235    float gain;
 236    float step;
 237    float fact;
 238    /*Compute excitation energy prior to enhancement*/
 239    for (i=0;i<nsf;i++)
 240       exc_energy+=exc[i]*exc[i];
 241
 242    /*Some gain adjustment is pitch is too high or if unvoiced*/
 243    {
 244       float g=0;
 245       g = .5*fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2] +
 246       mem->last_pitch_gain[0] + mem->last_pitch_gain[1] + mem->last_pitch_gain[2]);
 247       if (g>1.3)
 248          comb_gain*=1.3/g;
 249       if (g<.5)
 250          comb_gain*=2*g;
 251    }
 252    step = 1.0/nsf;
 253    fact=0;
 254    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
 255    for (i=0;i<nsf;i++)
 256    {
 257       fact += step;
 258
 259       new_exc[i] = exc[i] + comb_gain * fact * (
 260                                          pitch_gain[0]*exc[i-pitch+1] +
 261                                          pitch_gain[1]*exc[i-pitch] +
 262                                          pitch_gain[2]*exc[i-pitch-1]
 263                                          )
 264       + comb_gain * (1-fact) * (
 265                                          mem->last_pitch_gain[0]*exc[i-mem->last_pitch+1] +
 266                                          mem->last_pitch_gain[1]*exc[i-mem->last_pitch] +
 267                                          mem->last_pitch_gain[2]*exc[i-mem->last_pitch-1]
 268                                          );
 269    }
 270
 271    mem->last_pitch_gain[0] = pitch_gain[0];
 272    mem->last_pitch_gain[1] = pitch_gain[1];
 273    mem->last_pitch_gain[2] = pitch_gain[2];
 274    mem->last_pitch = pitch;
 275
 276    /*Gain after enhancement*/
 277    for (i=0;i<nsf;i++)
 278       new_exc_energy+=new_exc[i]*new_exc[i];
 279
 280    /*Compute scaling factor and normalize energy*/
 281    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
 282    if (gain < .5)
 283       gain=.5;
 284    if (gain>1)
 285       gain=1;
 286
 287    for (i=0;i<nsf;i++)
 288    {
 289       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
 290       new_exc[i] *= mem->smooth_gain;
 291    }
 292 }