acelp_cp.c

语音编码G.729 语音编码G.729

        
        /* stage 2..5: fix pulse i2,i3,i4,i5,i6,i7,i8 and i9 */
        for (j=2; j<10; j+=2) {
            
        /*--------------------------------------------------*
        * Store all impulse response of all fixed pulses   *
        * in vector vec[] for the "cor_h_vec()" function.  *
            *--------------------------------------------------*/
            if (sign[ix] < 0) p0 = h_inv - ix;
            else p0 = h - ix;
            
            if (sign[iy] < 0) p1 = h_inv - iy;
            else p1 = h - iy;
            
            for (i=0; i<L_SUBFR; i++) {
                vec[i] += *p0 + *p1;
                p0++; p1++;
            }
            
            /*--------------------------------------------------*
            * Calculate correlation of all possible positions  *
            * of the next 2 pulses with previous fixed pulses. *
            * Each pulse can have 8 possible positions         *
            *--------------------------------------------------*/
            cor_h_vec(h, vec, ipos[k+j], sign, rrixix, cor_x);
            cor_h_vec(h, vec, ipos[k+j+1], sign, rrixix, cor_y);
            
            /*--------------------------------------------------*
            * Fix 2 pulses, try 8x8 = 64 positions.            *
            *--------------------------------------------------*/
            search_ixiy(ipos[k+j], ipos[k+j+1], &ps, &alp, &ix, &iy,
                dn, cor_x, cor_y, rrixiy);
            ip[j] = ix;
            ip[j+1] = iy;
        }
        
        /* memorise new codevector if it's better than the last one. */
        ps *= ps;
        s = (alpk*ps)-(psk*alp);
        
        if (s > (FLOAT)0.) {
            psk = ps;
            alpk = alp;
            for (i=0; i<10; i++) codvec[i] = ip[i];
        }
        
    } /* end of for (pos=0; pos<3; pos++) */
    
      /*-------------------------------------------------------------------*
      * index of 10 pulses = 35 bits on 5 words                           *
      * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                           *
      * indx[0] = 7 bits --> 3(pos#6) + 1(sign#1) + 3(pos#1)              *
      * indx[1] = 7 bits --> 3(pos#7) + 1(sign#2) + 3(pos#2)              *
      * indx[2] = 7 bits --> 3(pos#8) + 1(sign#3) + 3(pos#3)              *
      * indx[3] = 7 bits --> 3(pos#9) + 1(sign#4) + 3(pos#4)              *
      * indx[4] = 7 bits --> 3(pos#10)+ 1(sign#5) + 3(pos#5)              *
    *-------------------------------------------------------------------*/
    build_code(codvec, sign, 10, h, code, y, indx);
    
    for (i=0; i<NB_TRACK; i++) indx[i] = indx[i] & (int)127;
    
    return;
    
}

/*-------------------------------------------------------------------*
* Function  cor_h_vec()                                             *
* ~~~~~~~~~~~~~~~~~~~~~                                             *
* Compute correlations of h[] with vec[] for the specified track.   *
*-------------------------------------------------------------------*
*-------------------------------------------------------------------*/
static void cor_h_vec(
                      FLOAT h[],           /* (i) scaled impulse response */
                      FLOAT vec[],         /* (i) vector to correlate with h[] */
                      int track,         /* (i) track to use */
                      FLOAT sign[],        /* (i) sign vector */
                      FLOAT rrixix[][NB_POS],  /* (i) correlation of h[x] with h[x] */
                      FLOAT cor[]          /* (o) result of correlation (NB_POS elements) */
                      )
{
    int i, j, pos;
    FLOAT *p0, *p1, *p2;
    FLOAT s;
    
    p0 = rrixix[track];
    pos = track;
    for (i=0; i<NB_POS; i++, pos+=STEP) {
        s = (FLOAT)0.;
        p1 = h;
        p2 = &vec[pos];
        for (j=pos; j<L_SUBFR; j++) {
            s += *p1 *  *p2;
            p1++; p2++;
        }
        cor[i] = (s* sign[pos]) + *p0++;
    }
    
    return;
}

/*-------------------------------------------------------------------*
* Function  search_ixiy()                                           *
* ~~~~~~~~~~~~~~~~~~~~~~~                                           *
* Find the best positions of 2 pulses in a subframe.                *
*-------------------------------------------------------------------*/
static void search_ixiy(
                        int track_x,       /* (i) track of pulse 1 */
                        int track_y,       /* (i) track of pulse 2 */
                        FLOAT *ps,           /* (i/o) correlation of all fixed pulses */
                        FLOAT *alp,          /* (i/o) energy of all fixed pulses */
                        int *ix,           /* (o) position of pulse 1 */
                        int *iy,           /* (o) position of pulse 2 */
                        FLOAT dn[],          /* (i) corr. between target and h[] */
                        FLOAT cor_x[],       /* (i) corr. of pulse 1 with fixed pulses */
                        FLOAT cor_y[],       /* (i) corr. of pulse 2 with fixed pulses */
                        FLOAT rrixiy[][MSIZE]  /* (i) corr. of pulse 1 with pulse 2 */
                        )
{
    int i, j, pos;
    FLOAT ps1, ps2, sq, sqk;
    FLOAT alp1, alp2, alpk;
    FLOAT *p0, *p1, *p2;
    FLOAT s;
    
    p0 = cor_x;
    p1 = cor_y;
    p2 = rrixiy[track_x];
    sqk = (FLOAT)-1.;
    alpk = (FLOAT)1.;
    for (i=track_x; i<L_SUBFR; i+=STEP) {
        ps1 = *ps + dn[i];
        alp1 = *alp + *p0++;
        pos = -1;
        for (j=track_y; j<L_SUBFR; j+=STEP) {
            ps2 = ps1 + dn[j];
            alp2 = alp1 +  *p1++ + *p2++;
            sq = (ps2 * ps2);
            s = (alpk*sq)-(sqk*alp2);
            if (s > (FLOAT)0.) {
                sqk = sq;
                alpk = alp2;
                pos = j;
            }
        }
        p1 -= NB_POS;
        if (pos >= 0) {
            *ix = i;
            *iy = pos;
        }
    }
    *ps += dn[*ix] + dn[*iy];
    *alp = alpk;
    
    return;
}

/*-------------------------------------------------------------------*
* Function  set_sign()                                              *
* ~~~~~~~~~~~~~~~~~~~~                                              *
* Set the sign of each pulse position.                              *
*-------------------------------------------------------------------*/
static void set_sign(
    FLOAT cn[],       /* (i) : residual after long term prediction    */
    FLOAT dn[],       /* (i) : correlation between target and h[]     */
    FLOAT sign[],     /* (o) : sign vector (sign of each position)    */
    FLOAT inv_sign[], /* (o) : inverse of sign[]                      */
    int pos_max[],    /* (o) : pos of max of correlation              */
    FLOAT corr[]      /* (o) : correlation of each track              */
)
{
    int i, k, pos;
    FLOAT k_cn, k_dn, val;
    FLOAT s, max;
    
    /* calculate energy for normalization of cn[] and dn[] */
    s = (FLOAT)0.;
    for (i=0; i<L_SUBFR; i++) s += cn[i] * cn[i];
    if (s < (F)0.01) s = (F)0.01;
    k_cn = (F)1./(FLOAT)sqrt((double)s);

    s = (FLOAT)0.;
    for (i=0; i<L_SUBFR; i++) s += dn[i] * dn[i];
    if (s < (F)0.01) s = (F)0.01;
    k_dn = (F)1./(FLOAT)sqrt((double)s);
    
    /* set sign according to en[] = k_cn*cn[] + k_dn*dn[]    */

    /* find position of maximum of correlation in each track */
    pos = 0; /* to avoid visual warning */
    for (k=0; k<NB_TRACK; k++) {
        max = (FLOAT)-1.;
        for (i=k; i<L_SUBFR; i+=STEP) {
            val = dn[i];
            s = (k_cn* cn[i])+ (k_dn* val);
            if (s >= (FLOAT)0.) {
                sign[i] = (FLOAT)1.;
                inv_sign[i] = (FLOAT)-1.;
            }
            else {
                sign[i] = (FLOAT)-1.;
                inv_sign[i] = (FLOAT)1.;
                val = -val;
            }
            dn[i] = val;      /* modify dn[] according to the fixed sign */
            s = (FLOAT)fabs(s);
            if (s > max) {
                max = s;
                pos = i;
            }
        }
        pos_max[k] = pos;
        corr[k] = max;
    }
    
    return;
}

/*-------------------------------------------------------------------*
* Function  cor_h_e()                                                 *
* ~~~~~~~~~~~~~~~~~                                                 *
* Compute correlations of h[] needed for the codebook search.       *
*-------------------------------------------------------------------*/
static void cor_h_e(
    FLOAT sign[],           /* (i) : sign vector                 */
    FLOAT inv_sign[],       /* (i) : inverse of sign[]           */
    FLOAT h[],              /* (o) : scaled h[]                 */
    FLOAT h_inv[],          /* (o) : inverse of scaled h[]      */
    FLOAT rrixix[][NB_POS], /* (o) energy of h[].                   */
    FLOAT rrixiy[][MSIZE]   /* (o) correlation between 2 pulses.    */
)
{
    int i, j, k, pos;
    FLOAT *ptr_h1, *ptr_h2, *ptr_hf, *psign;
    FLOAT *p0, *p1, *p2, *p3, *p4;
    FLOAT cor;
    
    for(i=0; i<L_SUBFR; i++) {
        h_inv[i] = -h[i];
    }
    
    /*------------------------------------------------------------*
    * Compute rrixix[][] needed for the codebook search.         *
    * This algorithm compute impulse response energy of all      *
    * positions (8) in each track (5).         Total = 5x8 = 40. *
    *------------------------------------------------------------*/
    /* storage order --> i4i4, i3i3, i2i2, i1i1, i0i0 */
    /* Init pointers to last position of rrixix[] */
    p0 = &rrixix[0][NB_POS-1];
    p1 = &rrixix[1][NB_POS-1];
    p2 = &rrixix[2][NB_POS-1];
    p3 = &rrixix[3][NB_POS-1];
    p4 = &rrixix[4][NB_POS-1];
    ptr_h1 = h;
    cor    = (FLOAT)0.;
    for(i=0; i<NB_POS; i++) {
        cor += (*ptr_h1) * (*ptr_h1); ptr_h1++;
        *p4-- = cor;
        cor += (*ptr_h1) * (*ptr_h1); ptr_h1++;
        *p3-- = cor;
        cor += (*ptr_h1) * (*ptr_h1); ptr_h1++;
        *p2-- = cor;
        cor += (*ptr_h1) * (*ptr_h1); ptr_h1++;
        *p1-- = cor;
        cor += (*ptr_h1) * (*ptr_h1); ptr_h1++;
        *p0-- = cor;
    }
    
    /* Divide all elements of rrixix[][] by 2. */
    p0 = &rrixix[0][0];
    for(i=0; i<L_SUBFR; i++)
    {
        *p0 *= (FLOAT)0.5;
        p0++;
    }
    
    /*------------------------------------------------------------*
    * Compute rrixiy[][] needed for the codebook search.         *
    * This algorithm compute correlation between 2 pulses        *
    * (2 impulses responses) in 5 possible adjacents tracks.     *
    * (track 0-1, 1-2, 2-3, 3-4 and 4-0).   Total = 5x8x8 = 320. *
    *------------------------------------------------------------*/
    /* storage order --> i3i4, i2i3, i1i2, i0i1, i4i0 */
    pos = MSIZE-1;
    ptr_hf = h + 1;
    for(k=0; k<NB_POS; k++) {
        p4 = &rrixiy[3][pos];
        p3 = &rrixiy[2][pos];
        p2 = &rrixiy[1][pos];
        p1 = &rrixiy[0][pos];
        p0 = &rrixiy[4][pos-NB_POS];
        cor = (FLOAT)0.;
        ptr_h1 = h;
        ptr_h2 = ptr_hf;
        for(i=k+1; i<NB_POS; i++ ) {
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p4 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p3 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p2 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p1 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p0 = cor;
            p4 -= (NB_POS+1);
            p3 -= (NB_POS+1);
            p2 -= (NB_POS+1);
            p1 -= (NB_POS+1);
            p0 -= (NB_POS+1);
        }
        
        cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
        *p4 = cor;
        cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
        *p3 = cor;
        cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
        *p2 = cor;
        cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
        *p1 = cor;
        pos -= NB_POS;
        ptr_hf += STEP;
    }
    
    /* storage order --> i4i0, i3i4, i2i3, i1i2, i0i1 */
    pos = MSIZE-1;
    ptr_hf = h + 4;
    for(k=0; k<NB_POS; k++) {
        p4 = &rrixiy[4][pos];
        p3 = &rrixiy[3][pos-1];
        p2 = &rrixiy[2][pos-1];
        p1 = &rrixiy[1][pos-1];
        p0 = &rrixiy[0][pos-1];
        
        cor = (FLOAT)0.;
        ptr_h1 = h;
        ptr_h2 = ptr_hf;
        for(i=k+1; i<NB_POS; i++ ) {
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p4 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p3 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p2 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p1 = cor;
            cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
            *p0 = cor;
            
            p4 -= (NB_POS+1);
            p3 -= (NB_POS+1);
            p2 -= (NB_POS+1);
            p1 -= (NB_POS+1);
            p0 -= (NB_POS+1);
        }
        cor += (*ptr_h1) * (*ptr_h2); ptr_h1++; ptr_h2++;
        *p4 = cor;
        pos--;
        ptr_hf += STEP;
    }
    
    /*------------------------------------------------------------*
    * Modification of rrixiy[][] to take signs into account.     *
    *------------------------------------------------------------*/
    p0 = &rrixiy[0][0];
    for (k=0; k<NB_TRACK; k++) {
        for(i=k; i<L_SUBFR; i+=STEP) {
            psign = sign;
            if (psign[i] < 0) psign = inv_sign;
            for(j=ipos[k+1]; j<L_SUBFR; j+= STEP) {
                *p0 *= psign[j]; p0++;
            }
        }
    }
    
    return;
}

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