asterisk/codecs/ilbc/helpfun.c

/******************************************************************

    iLBC Speech Coder ANSI-C Source Code

    helpfun.c 

    Copyright (C) The Internet Society (2004). 
    All Rights Reserved.

******************************************************************/

#include <math.h>

#include "iLBC_define.h"
#include "helpfun.h"
#include "constants.h"

/*----------------------------------------------------------------*
 *  calculation of auto correlation 
 *---------------------------------------------------------------*/

void autocorr( 
    float *r,       /* (o) autocorrelation vector */
    const float *x, /* (i) data vector */
    int N,          /* (i) length of data vector */
    int order       /* largest lag for calculated 
                       autocorrelations */
){
    int     lag, n;
    float   sum;
    
    for (lag = 0; lag <= order; lag++) {
        sum = 0;
        for (n = 0; n < N - lag; n++) {
            sum += x[n] * x[n+lag];
        }
        r[lag] = sum;
    }
}



/*----------------------------------------------------------------*
 *  window multiplication  
 *---------------------------------------------------------------*/

void window( 
    float *z,       /* (o) the windowed data */
    const float *x, /* (i) the original data vector */
    const float *y, /* (i) the window */
    int N           /* (i) length of all vectors */
){
    int     i;
    
    for (i = 0; i < N; i++) {
        z[i] = x[i] * y[i];
    }
}

/*----------------------------------------------------------------*
 *  levinson-durbin solution for lpc coefficients
 *---------------------------------------------------------------*/

void levdurb( 
    float *a,       /* (o) lpc coefficient vector starting 
                           with 1.0 */
    float *k,       /* (o) reflection coefficients */
    float *r,       /* (i) autocorrelation vector */
    int order       /* (i) order of lpc filter */
){
    float  sum, alpha;
    int     m, m_h, i;

    a[0] = 1.0;
    
    if (r[0] < EPS) { /* if r[0] <= 0, set LPC coeff. to zero */
        for (i = 0; i < order; i++) {
            k[i] = 0;
            a[i+1] = 0;
        }   
    } else {
        a[1] = k[0] = -r[1]/r[0];
        alpha = r[0] + r[1] * k[0];
        for (m = 1; m < order; m++){
            sum = r[m + 1];
            for (i = 0; i < m; i++){
                sum += a[i+1] * r[m - i];
            }
            k[m] = -sum / alpha;
            alpha += k[m] * sum;
            m_h = (m + 1) >> 1;
            for (i = 0; i < m_h; i++){
                sum = a[i+1] + k[m] * a[m - i];
                a[m - i] += k[m] * a[i+1];
                a[i+1] = sum;


            }
            a[m+1] = k[m];
        }
    }
}

/*----------------------------------------------------------------*
 *  interpolation between vectors 
 *---------------------------------------------------------------*/

void interpolate( 
    float *out,      /* (o) the interpolated vector */
    float *in1,     /* (i) the first vector for the 
                           interpolation */
    float *in2,     /* (i) the second vector for the 
                           interpolation */
    float coef,      /* (i) interpolation weights */
    int length      /* (i) length of all vectors */
){
    int i;
    float invcoef;

    invcoef = (float)1.0 - coef;
    for (i = 0; i < length; i++) {
        out[i] = coef * in1[i] + invcoef * in2[i];
    }
}

/*----------------------------------------------------------------*
 *  lpc bandwidth expansion                   
 *---------------------------------------------------------------*/

void bwexpand( 
    float *out,      /* (o) the bandwidth expanded lpc 
                           coefficients */
    float *in,      /* (i) the lpc coefficients before bandwidth 
                           expansion */
    float coef,     /* (i) the bandwidth expansion factor */
    int length      /* (i) the length of lpc coefficient vectors */
){
    int i;
    float  chirp;
    
    chirp = coef;
    
    out[0] = in[0];
    for (i = 1; i < length; i++) {
        out[i] = chirp * in[i];
        chirp *= coef;
    }
}

/*----------------------------------------------------------------*
 *  vector quantization 


 *---------------------------------------------------------------*/

void vq( 
    float *Xq,      /* (o) the quantized vector */
    int *index,     /* (o) the quantization index */
    const float *CB,/* (i) the vector quantization codebook */
    float *X,       /* (i) the vector to quantize */
    int n_cb,       /* (i) the number of vectors in the codebook */
    int dim         /* (i) the dimension of all vectors */
){
    int     i, j;
    int     pos, minindex;
    float   dist, tmp, mindist;

    pos = 0;
    mindist = FLOAT_MAX;
    minindex = 0;
    for (j = 0; j < n_cb; j++) {
        dist = X[0] - CB[pos];
        dist *= dist;
        for (i = 1; i < dim; i++) {
            tmp = X[i] - CB[pos + i];
            dist += tmp*tmp;
        }
        
        if (dist < mindist) {
            mindist = dist;
            minindex = j;
        }
        pos += dim;
    }
    for (i = 0; i < dim; i++) {
        Xq[i] = CB[minindex*dim + i];
    }
    *index = minindex;
}

/*----------------------------------------------------------------*
 *  split vector quantization 
 *---------------------------------------------------------------*/

void SplitVQ( 
    float *qX,      /* (o) the quantized vector */
    int *index,     /* (o) a vector of indexes for all vector
                           codebooks in the split */
    float *X,       /* (i) the vector to quantize */
    const float *CB,/* (i) the quantizer codebook */
    int nsplit,     /* the number of vector splits */
    const int *dim, /* the dimension of X and qX */
    const int *cbsize /* the number of vectors in the codebook */
){
    int    cb_pos, X_pos, i;
    
    cb_pos = 0;


    X_pos= 0;
    for (i = 0; i < nsplit; i++) {
        vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos, 
            cbsize[i], dim[i]);
        X_pos += dim[i];
        cb_pos += dim[i] * cbsize[i];
    }
}

/*----------------------------------------------------------------*
 *  scalar quantization 
 *---------------------------------------------------------------*/

void sort_sq( 
    float *xq,      /* (o) the quantized value */
    int *index,     /* (o) the quantization index */
    float x,    /* (i) the value to quantize */
    const float *cb,/* (i) the quantization codebook */
    int cb_size      /* (i) the size of the quantization codebook */
){
    int i;
    
    if (x <= cb[0]) {
        *index = 0;
        *xq = cb[0];
    } else {
        i = 0;
        while ((x > cb[i]) && i < cb_size - 1) {
            i++;
        }
        
        if (x > ((cb[i] + cb[i - 1])/2)) {
            *index = i;
            *xq = cb[i];
        } else {
            *index = i - 1;
            *xq = cb[i - 1];
        }
    }
}

/*----------------------------------------------------------------*
 *  check for stability of lsf coefficients
 *---------------------------------------------------------------*/

int LSF_check(    /* (o) 1 for stable lsf vectors and 0 for
                           nonstable ones */
    float *lsf,     /* (i) a table of lsf vectors */
    int dim,    /* (i) the dimension of each lsf vector */
    int NoAn    /* (i) the number of lsf vectors in the 
                           table */
){
    int k,n,m, Nit=2, change=0,pos;
    float tmp;


    static float eps=(float)0.039; /* 50 Hz */
    static float eps2=(float)0.0195;
    static float maxlsf=(float)3.14; /* 4000 Hz */
    static float minlsf=(float)0.01; /* 0 Hz */
    
    /* LSF separation check*/

    for (n=0; n<Nit; n++) { /* Run through a couple of times */
        for (m=0; m<NoAn; m++) { /* Number of analyses per frame */
            for (k=0; k<(dim-1); k++) {
                pos=m*dim+k;
                
                if ((lsf[pos+1]-lsf[pos])<eps) {
                    
                    if (lsf[pos+1]<lsf[pos]) {
                        tmp=lsf[pos+1];
                        lsf[pos+1]= lsf[pos]+eps2;
                        lsf[pos]= lsf[pos+1]-eps2;
                    } else {
                        lsf[pos]-=eps2;
                        lsf[pos+1]+=eps2;
                    }
                    change=1;
                }
                
                if (lsf[pos]<minlsf) { 
                    lsf[pos]=minlsf;
                    change=1;
                }
                
                if (lsf[pos]>maxlsf) { 
                    lsf[pos]=maxlsf;
                    change=1;
                }               
            }
        }
    }
    
    return change;  
}