libspeex/cb_search.c

   1 /* Original copyright */
   2 /*-----------------------------------------------------------------------*\
   3
   4     FILE........: GAINSHAPE.C
   5     TYPE........: C Module
   6     AUTHOR......: David Rowe
   7     COMPANY.....: Voicetronix
   8     DATE CREATED: 19/2/02
   9
  10     General gain-shape codebook search.
  11
  12 \*-----------------------------------------------------------------------*/
  13
  14
  15 /* Modified by Jean-Marc Valin 2002
  16
  17    This library is free software; you can redistribute it and/or
  18    modify it under the terms of the GNU Lesser General Public
  19    License as published by the Free Software Foundation; either
  20    version 2.1 of the License, or (at your option) any later version.
  21
  22    This library is distributed in the hope that it will be useful,
  23    but WITHOUT ANY WARRANTY; without even the implied warranty of
  24    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  25    Lesser General Public License for more details.
  26
  27    You should have received a copy of the GNU Lesser General Public
  28    License along with this library; if not, write to the Free Software
  29    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  30 */
  31
  32
  33
  34 #include <stdlib.h>
  35 #include <cb_search.h>
  36 #include "filters.h"
  37 #include <math.h>
  38 #include <stdio.h>
  39 #include "stack_alloc.h"
  40 #include "vq.h"
  41
  42 #define min(a,b) ((a) < (b) ? (a) : (b))
  43 #define max(a,b) ((a) > (b) ? (a) : (b))
  44
  45
  46 static float scal_gains4[16] = {
  47    0.27713,
  48    0.49282,
  49    0.69570,
  50    0.90786,
  51    1.14235,
  52    1.42798,
  53    1.80756,
  54    2.42801
  55 };
  56
  57
  58 /*---------------------------------------------------------------------------*\
  59
  60  void overlap_cb_search()
  61
  62  Searches a gain/shape codebook consisting of overlapping entries for the
  63  closest vector to the target.  Gives identical results to search() above
  64  buts uses fast end correction algorithm for the synthesis of response
  65  vectors.
  66
  67 \*---------------------------------------------------------------------------*/
  68
  69 float overlap_cb_search(
  70 float target[],                 /* target vector */
  71 float ak[],                     /* LPCs for this subframe */
  72 float awk1[],                   /* Weighted LPCs for this subframe */
  73 float awk2[],                   /* Weighted LPCs for this subframe */
  74 float codebook[],               /* overlapping codebook */
  75 int   entries,                  /* number of overlapping entries to search */
  76 float *gain,                    /* gain of optimum entry */
  77 int   *index,                   /* index of optimum entry */
  78 int   p,                        /* number of LPC coeffs */
  79 int   nsf,                      /* number of samples in subframe */
  80 float *stack
  81 )
  82 {
  83   float *resp;                  /* zero state response to current entry */
  84   float *h;                     /* impulse response of synthesis filter */
  85   float *impulse;               /* excitation vector containing one impulse */
  86   float d,e,g,score;            /* codebook searching variables */
  87   float bscore;                 /* score of "best" vector so far */
  88   int i,k;                      /* loop variables */
  89
  90   /* Initialise */
  91
  92   /*resp = (float*)malloc(sizeof(float)*nsf);
  93   h = (float*)malloc(sizeof(float)*nsf);
  94   impulse = (float*)malloc(sizeof(float)*nsf);
  95   */
  96   resp=PUSH(stack, nsf);
  97   h=PUSH(stack, nsf);
  98   impulse=PUSH(stack, nsf);
  99
 100   for(i=0; i<nsf; i++)
 101     impulse[i] = 0.0;
 102
 103   *gain = 0.0;
 104   *index = 0;
 105   bscore = 0.0;
 106   impulse[0] = 1.0;
 107
 108   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
 109   residue_zero(impulse, awk1, h, nsf, p);
 110   syn_filt_zero(h, ak, h, nsf, p);
 111   syn_filt_zero(h, awk2, h, nsf,p);
 112
 113   /* Calculate codebook zero-response */
 114   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
 115   syn_filt_zero(resp,ak,resp,nsf,p);
 116   syn_filt_zero(resp,awk2,resp,nsf,p);
 117
 118   /* Search codebook backwards using end correction for synthesis */
 119
 120   for(k=entries-1; k>=0; k--) {
 121
 122     d = 0.0; e = 0.0;
 123     for(i=0; i<nsf; i++) {
 124       d += target[i]*resp[i];
 125       e += resp[i]*resp[i];
 126     }
 127     g = d/(e+.0001);
 128     score = g*d;
 129     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
 130     if (score >= bscore) {
 131       bscore = score;
 132       *gain = g;
 133       *index = k;
 134     }
 135
 136     /* Synthesise next entry */
 137
 138     if (k) {
 139       for(i=nsf-1; i>=1; i--)
 140         resp[i] = resp[i-1] + codebook[k-1]*h[i];
 141       resp[0] = codebook[k-1]*h[0];
 142     }
 143   }
 144
 145   /*free(resp);
 146   free(h);
 147   free(impulse);*/
 148   POP(stack);
 149   POP(stack);
 150   POP(stack);
 151   return bscore;
 152 }
 153
 154
 155
 156 void split_cb_search(
 157 float target[],                 /* target vector */
 158 float ak[],                     /* LPCs for this subframe */
 159 float awk1[],                   /* Weighted LPCs for this subframe */
 160 float awk2[],                   /* Weighted LPCs for this subframe */
 161 void *par,                      /* Codebook/search parameters*/
 162 int   p,                        /* number of LPC coeffs */
 163 int   nsf,                      /* number of samples in subframe */
 164 float *exc,
 165 SpeexBits *bits,
 166 float *stack
 167 )
 168 {
 169    int i,j, id;
 170    float *resp, *E, q;
 171    float *t, *r, *e;
 172    float *gains;
 173    int *ind;
 174    float *shape_cb;
 175    int shape_cb_size, subvect_size, nb_subvect;
 176    float exc_energy=0;
 177    split_cb_params *params;
 178
 179    params = (split_cb_params *) par;
 180    subvect_size = params->subvect_size;
 181    nb_subvect = params->nb_subvect;
 182    shape_cb_size = 1<<params->shape_bits;
 183    shape_cb = params->shape_cb;
 184    resp = PUSH(stack, shape_cb_size*subvect_size);
 185    E = PUSH(stack, shape_cb_size);
 186    t = PUSH(stack, nsf);
 187    r = PUSH(stack, nsf);
 188    e = PUSH(stack, nsf);
 189    gains = PUSH(stack, nb_subvect);
 190    ind = (int*)PUSH(stack, nb_subvect);
 191
 192    /* Compute energy of the "real excitation" */
 193    syn_filt_zero(target, awk1, e, nsf, p);
 194    residue_zero(e, ak, e, nsf, p);
 195    residue_zero(e, awk2, e, nsf, p);
 196    for (i=0;i<nsf;i++)
 197       exc_energy += e[i]*e[i];
 198    exc_energy=sqrt(exc_energy/nb_subvect);
 199
 200    /* Quantize global ("average") gain */
 201    q=log(exc_energy+.1);
 202    q=floor(.5+2*(q-2));
 203    if (q<0)
 204       q=0;
 205    if (q>15)
 206       q=15;
 207    id = (int)q;
 208    speex_bits_pack(bits, id, 4);
 209    exc_energy=exp(.5*q+2);
 210
 211
 212    for (i=0;i<nsf;i++)
 213       t[i]=target[i];
 214
 215    e[0]=1;
 216    for (i=1;i<nsf;i++)
 217       e[i]=0;
 218    residue_zero(e, awk1, r, nsf, p);
 219    syn_filt_zero(r, ak, r, nsf, p);
 220    syn_filt_zero(r, awk2, r, nsf,p);
 221
 222    /* Pre-compute codewords response and energy */
 223    for (i=0;i<shape_cb_size;i++)
 224    {
 225       float *res = resp+i*subvect_size;
 226
 227       /* Compute codeword response */
 228       int k;
 229       for(j=0;j<subvect_size;j++)
 230          res[j]=0;
 231       for(j=0;j<subvect_size;j++)
 232       {
 233          for (k=j;k<subvect_size;k++)
 234             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
 235       }
 236       /* Compute energy of codeword response */
 237       E[i]=0;
 238       for(j=0;j<subvect_size;j++)
 239          E[i]+=res[j]*res[j];
 240       E[i]=1/(.001+E[i]);
 241    }
 242
 243    for (i=0;i<nb_subvect;i++)
 244    {
 245       int best_index=0, k, m;
 246       float g, corr, best_gain=0, score, best_score=-1;
 247       /* Find best codeword for current sub-vector */
 248       for (j=0;j<shape_cb_size;j++)
 249       {
 250          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
 251          score=corr*corr*E[j];
 252          g = corr*E[j];
 253          if (score>best_score)
 254          {
 255             best_index=j;
 256             best_score=score;
 257             best_gain=g;
 258          }
 259       }
 260       speex_bits_pack(bits,best_index,params->shape_bits);
 261
 262       /* Quantize gain */
 263       {
 264          int s=0, best_id;
 265          best_gain /= .01+exc_energy;
 266          if (best_gain<0)
 267          {
 268             best_gain=-best_gain;
 269             s=1;
 270          }
 271
 272          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
 273          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
 274
 275          best_gain=scal_gains4[best_id];
 276          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
 277          if (s)
 278             best_gain=-best_gain;
 279          best_gain *= exc_energy;
 280          speex_bits_pack(bits,s,1);
 281          speex_bits_pack(bits,best_id,3);
 282       }
 283       ind[i]=best_index;
 284       gains[i]=best_gain;
 285       /* Update target for next subvector */
 286       for (j=0;j<subvect_size;j++)
 287       {
 288          g=best_gain*shape_cb[best_index*subvect_size+j];
 289          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 290             t[k] -= g*r[m];
 291       }
 292    }
 293
 294    /* Put everything back together */
 295    for (i=0;i<nb_subvect;i++)
 296       for (j=0;j<subvect_size;j++)
 297          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
 298
 299    /* Update excitation */
 300    for (j=0;j<nsf;j++)
 301       exc[j]+=e[j];
 302
 303    /* Update target */
 304    residue_zero(e, awk1, r, nsf, p);
 305    syn_filt_zero(r, ak, r, nsf, p);
 306    syn_filt_zero(r, awk2, r, nsf,p);
 307    for (j=0;j<nsf;j++)
 308       target[j]-=r[j];
 309
 310
 311
 312
 313    POP(stack);
 314    POP(stack);
 315    POP(stack);
 316    POP(stack);
 317    POP(stack);
 318    POP(stack);
 319 }
 320
 321 void split_cb_search_nogain(
 322 float target[],                 /* target vector */
 323 float ak[],                     /* LPCs for this subframe */
 324 float awk1[],                   /* Weighted LPCs for this subframe */
 325 float awk2[],                   /* Weighted LPCs for this subframe */
 326 void *par,                      /* Codebook/search parameters*/
 327 int   p,                        /* number of LPC coeffs */
 328 int   nsf,                      /* number of samples in subframe */
 329 float *exc,
 330 SpeexBits *bits,
 331 float *stack
 332 )
 333 {
 334    int i,j;
 335    float *resp;
 336    float *t, *r, *e;
 337    int *ind;
 338    float *shape_cb;
 339    int shape_cb_size, subvect_size, nb_subvect;
 340    split_cb_params *params;
 341
 342    params = (split_cb_params *) par;
 343    subvect_size = params->subvect_size;
 344    nb_subvect = params->nb_subvect;
 345    shape_cb_size = 1<<params->shape_bits;
 346    shape_cb = params->shape_cb;
 347    resp = PUSH(stack, shape_cb_size*subvect_size);
 348    t = PUSH(stack, nsf);
 349    r = PUSH(stack, nsf);
 350    e = PUSH(stack, nsf);
 351    ind = (int*)PUSH(stack, nb_subvect);
 352
 353    for (i=0;i<nsf;i++)
 354       t[i]=target[i];
 355
 356    e[0]=1;
 357    for (i=1;i<nsf;i++)
 358       e[i]=0;
 359    residue_zero(e, awk1, r, nsf, p);
 360    syn_filt_zero(r, ak, r, nsf, p);
 361    syn_filt_zero(r, awk2, r, nsf,p);
 362
 363    /* Pre-compute codewords response and energy */
 364    for (i=0;i<shape_cb_size;i++)
 365    {
 366       float *res = resp+i*subvect_size;
 367
 368       /* Compute codeword response */
 369       int k;
 370       for(j=0;j<subvect_size;j++)
 371          res[j]=0;
 372       for(j=0;j<subvect_size;j++)
 373       {
 374          for (k=j;k<subvect_size;k++)
 375             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
 376       }
 377    }
 378
 379    for (i=0;i<nb_subvect;i++)
 380    {
 381       int best_index=0, k, m;
 382       float g, dist, best_dist=-1;
 383       float *a, *b;
 384
 385       /* Find best codeword for current sub-vector */
 386       for (j=0;j<shape_cb_size;j++)
 387       {
 388          dist=0;
 389          a=resp+j*subvect_size;
 390          b=t+subvect_size*i;
 391          for (k=0;k<subvect_size;k++)
 392             dist += (a[k]-b[k])*(a[k]-b[k]);
 393          if (dist<best_dist || j==0)
 394          {
 395             best_dist=dist;
 396             best_index=j;
 397          }
 398       }
 399       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
 400       speex_bits_pack(bits,best_index,params->shape_bits);
 401
 402       ind[i]=best_index;
 403       /* Update target for next subvector */
 404       for (j=0;j<subvect_size;j++)
 405       {
 406          g=shape_cb[best_index*subvect_size+j];
 407          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 408             t[k] -= g*r[m];
 409       }
 410
 411    }
 412
 413    /* Put everything back together */
 414    for (i=0;i<nb_subvect;i++)
 415       for (j=0;j<subvect_size;j++)
 416          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
 417
 418    /* Update excitation */
 419    for (j=0;j<nsf;j++)
 420       exc[j]+=e[j];
 421
 422    /* Update target */
 423    residue_zero(e, awk1, r, nsf, p);
 424    syn_filt_zero(r, ak, r, nsf, p);
 425    syn_filt_zero(r, awk2, r, nsf,p);
 426    for (j=0;j<nsf;j++)
 427       target[j]-=r[j];
 428
 429
 430
 431
 432    POP(stack);
 433    POP(stack);
 434    POP(stack);
 435    POP(stack);
 436 }
 437
 438
 439 void split_cb_search_nogain2(
 440 float target[],                 /* target vector */
 441 float ak[],                     /* LPCs for this subframe */
 442 float awk1[],                   /* Weighted LPCs for this subframe */
 443 float awk2[],                   /* Weighted LPCs for this subframe */
 444 void *par,                      /* Codebook/search parameters*/
 445 int   p,                        /* number of LPC coeffs */
 446 int   nsf,                      /* number of samples in subframe */
 447 float *exc,
 448 SpeexBits *bits,
 449 float *stack
 450 )
 451 {
 452    int i,j;
 453    float *resp;
 454    float *t, *r, *e, *E;
 455    int *ind;
 456    float *shape_cb;
 457    int shape_cb_size, subvect_size, nb_subvect;
 458    split_cb_params *params;
 459
 460    params = (split_cb_params *) par;
 461    subvect_size = params->subvect_size;
 462    nb_subvect = params->nb_subvect;
 463    shape_cb_size = 1<<params->shape_bits;
 464    shape_cb = params->shape_cb;
 465    resp = PUSH(stack, shape_cb_size*subvect_size);
 466    t = PUSH(stack, nsf);
 467    r = PUSH(stack, nsf);
 468    e = PUSH(stack, nsf);
 469    E = PUSH(stack, shape_cb_size);
 470    ind = (int*)PUSH(stack, nb_subvect);
 471
 472    for (i=0;i<nsf;i++)
 473       t[i]=target[i];
 474
 475    e[0]=1;
 476    for (i=1;i<nsf;i++)
 477       e[i]=0;
 478    residue_zero(e, awk1, r, nsf, p);
 479    syn_filt_zero(r, ak, r, nsf, p);
 480    syn_filt_zero(r, awk2, r, nsf,p);
 481
 482    /* Pre-compute codewords response and energy */
 483    for (i=0;i<shape_cb_size;i++)
 484    {
 485       float *res = resp+i*subvect_size;
 486
 487       /* Compute codeword response */
 488       int k;
 489       for(j=0;j<subvect_size;j++)
 490          res[j]=0;
 491       for(j=0;j<subvect_size;j++)
 492       {
 493          for (k=j;k<subvect_size;k++)
 494             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
 495       }
 496       E[i]=0;
 497       for(j=0;j<subvect_size;j++)
 498          E[i]+=res[j]*res[j];
 499    }
 500
 501    for (i=0;i<nb_subvect;i++)
 502    {
 503       int best_index[2]={0,0}, k, m;
 504       float g, dist, best_dist[2]={-1,-1};
 505       float *a, *x;
 506       float energy=0;
 507       x=t+subvect_size*i;
 508
 509       for (k=0;k<subvect_size;k++)
 510          energy+=x[k]*x[k];
 511       /* Find best codeword for current sub-vector */
 512       for (j=0;j<shape_cb_size;j++)
 513       {
 514          dist=0;
 515          a=resp+j*subvect_size;
 516          dist=energy+E[j];
 517          for (k=0;k<subvect_size;k++)
 518             dist -= 2*a[k]*x[k];
 519          if (dist<best_dist[0] || best_dist[0]<0)
 520          {
 521             best_dist[1]=best_dist[0];
 522             best_index[1]=best_index[0];
 523             best_dist[0]=dist;
 524             best_index[0]=j;
 525          } else if (dist<best_dist[1] || best_dist[1]<0)
 526          {
 527             best_dist[1]=dist;
 528             best_index[1]=j;
 529          }
 530       }
 531       if (i<nb_subvect-1)
 532       {
 533          int nbest;
 534          float *tt, err[2];
 535          float best_score[2];
 536          tt=PUSH(stack,nsf);
 537          for (nbest=0;nbest<2;nbest++)
 538          {
 539             for (j=0;j<nsf;j++)
 540                tt[j]=t[j];
 541             for (j=0;j<subvect_size;j++)
 542             {
 543                g=shape_cb[best_index[nbest]*subvect_size+j];
 544                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 545                   tt[k] -= g*r[m];
 546             }
 547
 548             {
 549                int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);
 550                for (j=0;j<subvect_size;j++)
 551                {
 552                   g=shape_cb[i2*subvect_size+j];
 553                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
 554                      tt[k] -= g*r[m];
 555                }
 556             }
 557
 558             err[nbest]=0;
 559             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
 560                err[nbest]-=tt[j]*tt[j];
 561
 562             best_score[nbest]=err[nbest];
 563          }
 564
 565          if (best_score[1]>best_score[0])
 566          {
 567             best_index[0]=best_index[1];
 568             best_score[0]=best_score[1];
 569          }
 570          POP(stack);
 571
 572       }
 573
 574       ind[i]=best_index[0];
 575
 576       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
 577       speex_bits_pack(bits,ind[i],params->shape_bits);
 578
 579       /* Update target for next subvector */
 580       for (j=0;j<subvect_size;j++)
 581       {
 582          g=shape_cb[ind[i]*subvect_size+j];
 583          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 584             t[k] -= g*r[m];
 585       }
 586    }
 587
 588    /* Put everything back together */
 589    for (i=0;i<nb_subvect;i++)
 590       for (j=0;j<subvect_size;j++)
 591          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
 592
 593    /* Update excitation */
 594    for (j=0;j<nsf;j++)
 595       exc[j]+=e[j];
 596
 597    /* Update target */
 598    residue_zero(e, awk1, r, nsf, p);
 599    syn_filt_zero(r, ak, r, nsf, p);
 600    syn_filt_zero(r, awk2, r, nsf,p);
 601    for (j=0;j<nsf;j++)
 602       target[j]-=r[j];
 603
 604
 605
 606    POP(stack);
 607    POP(stack);
 608    POP(stack);
 609    POP(stack);
 610    POP(stack);
 611 }
 612
 613 void split_cb_search_shape_sign(
 614 float target[],                 /* target vector */
 615 float ak[],                     /* LPCs for this subframe */
 616 float awk1[],                   /* Weighted LPCs for this subframe */
 617 float awk2[],                   /* Weighted LPCs for this subframe */
 618 void *par,                      /* Codebook/search parameters*/
 619 int   p,                        /* number of LPC coeffs */
 620 int   nsf,                      /* number of samples in subframe */
 621 float *exc,
 622 SpeexBits *bits,
 623 float *stack
 624 )
 625 {
 626    int i,j;
 627    float *resp;
 628    float *t, *r, *e, *E;
 629    int *ind, *signs;
 630    float *shape_cb;
 631    int shape_cb_size, subvect_size, nb_subvect;
 632    split_cb_params *params;
 633
 634    params = (split_cb_params *) par;
 635    subvect_size = params->subvect_size;
 636    nb_subvect = params->nb_subvect;
 637    shape_cb_size = 1<<params->shape_bits;
 638    shape_cb = params->shape_cb;
 639    resp = PUSH(stack, shape_cb_size*subvect_size);
 640    t = PUSH(stack, nsf);
 641    r = PUSH(stack, nsf);
 642    e = PUSH(stack, nsf);
 643    E = PUSH(stack, shape_cb_size);
 644    ind = (int*)PUSH(stack, nb_subvect);
 645    signs = (int*)PUSH(stack, nb_subvect);
 646
 647    for (i=0;i<nsf;i++)
 648       t[i]=target[i];
 649
 650    e[0]=1;
 651    for (i=1;i<nsf;i++)
 652       e[i]=0;
 653    residue_zero(e, awk1, r, nsf, p);
 654    syn_filt_zero(r, ak, r, nsf, p);
 655    syn_filt_zero(r, awk2, r, nsf,p);
 656
 657    /* Pre-compute codewords response and energy */
 658    for (i=0;i<shape_cb_size;i++)
 659    {
 660       float *res = resp+i*subvect_size;
 661
 662       /* Compute codeword response */
 663       int k;
 664       for(j=0;j<subvect_size;j++)
 665          res[j]=0;
 666       for(j=0;j<subvect_size;j++)
 667       {
 668          for (k=j;k<subvect_size;k++)
 669             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
 670       }
 671       E[i]=0;
 672       for(j=0;j<subvect_size;j++)
 673          E[i]+=res[j]*res[j];
 674    }
 675
 676    for (i=0;i<nb_subvect;i++)
 677    {
 678       int best_index[2]={0,0}, k, m;
 679       float g, dist, best_dist[2]={-1,-1}, best_sign[2]={0,0};
 680       float *a, *x;
 681       float energy=0;
 682       x=t+subvect_size*i;
 683
 684       for (k=0;k<subvect_size;k++)
 685          energy+=x[k]*x[k];
 686       /* Find best codeword for current sub-vector */
 687       for (j=0;j<shape_cb_size;j++)
 688       {
 689          int sign;
 690          dist=0;
 691          a=resp+j*subvect_size;
 692          dist=0;
 693          for (k=0;k<subvect_size;k++)
 694             dist -= 2*a[k]*x[k];
 695          if (dist > 0)
 696          {
 697             sign=1;
 698             dist =- dist;
 699          } else
 700             sign=0;
 701          dist += energy+E[j];
 702          if (dist<best_dist[0] || best_dist[0]<0)
 703          {
 704             best_dist[1]=best_dist[0];
 705             best_index[1]=best_index[0];
 706             best_sign[1]=best_sign[0];
 707             best_dist[0]=dist;
 708             best_index[0]=j;
 709             best_sign[0]=sign;
 710          } else if (dist<best_dist[1] || best_dist[1]<0)
 711          {
 712             best_dist[1]=dist;
 713             best_index[1]=j;
 714             best_sign[1]=sign;
 715          }
 716       }
 717       if (i<nb_subvect-1)
 718       {
 719          int nbest;
 720          float *tt, err[2];
 721          float best_score[2];
 722          tt=PUSH(stack,nsf);
 723          for (nbest=0;nbest<2;nbest++)
 724          {
 725             float s=1;
 726             if (best_sign[nbest])
 727                s=-1;
 728             for (j=0;j<nsf;j++)
 729                tt[j]=t[j];
 730             for (j=0;j<subvect_size;j++)
 731             {
 732                g=s*shape_cb[best_index[nbest]*subvect_size+j];
 733                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 734                   tt[k] -= g*r[m];
 735             }
 736
 737             {
 738                int best_index2, best_sign2, sign2;
 739                float  best_dist2;
 740                x=t+subvect_size*(i+1);
 741                for (j=0;j<shape_cb_size;j++)
 742                {
 743                   a=resp+j*subvect_size;
 744                   dist = 0;
 745                   for (k=0;k<subvect_size;k++)
 746                      dist -= 2*a[k]*x[k];
 747                   if (dist > 0)
 748                   {
 749                      sign2=1;
 750                      dist =- dist;
 751                   } else
 752                      sign2=0;
 753                   dist += energy+E[j];
 754                   if (dist<best_dist2 || j==0)
 755                   {
 756                      best_dist2=dist;
 757                      best_index2=j;
 758                      best_sign2=sign2;
 759                   }
 760                }
 761                s=1;
 762                if (best_sign2)
 763                   s=-1;
 764                /*int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);*/
 765
 766                for (j=0;j<subvect_size;j++)
 767                {
 768                   g=s*shape_cb[best_index2*subvect_size+j];
 769                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
 770                      tt[k] -= g*r[m];
 771                }
 772             }
 773
 774             err[nbest]=0;
 775             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
 776                err[nbest]-=tt[j]*tt[j];
 777
 778             best_score[nbest]=err[nbest];
 779          }
 780
 781          if (best_score[1]>best_score[0])
 782          {
 783             best_sign[0]=best_sign[1];
 784             best_index[0]=best_index[1];
 785             best_score[0]=best_score[1];
 786          }
 787          POP(stack);
 788
 789       }
 790
 791       ind[i]=best_index[0];
 792       signs[i] = best_sign[0];
 793
 794       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
 795       speex_bits_pack(bits,signs[i],1);
 796       speex_bits_pack(bits,ind[i],params->shape_bits);
 797
 798       /* Update target for next subvector */
 799       for (j=0;j<subvect_size;j++)
 800       {
 801          g=shape_cb[ind[i]*subvect_size+j];
 802          if (signs[i])
 803             g=-g;
 804          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 805             t[k] -= g*r[m];
 806       }
 807    }
 808
 809    /* Put everything back together */
 810    for (i=0;i<nb_subvect;i++)
 811    {
 812       float s=1;
 813       if (signs[i])
 814          s=-1;
 815       for (j=0;j<subvect_size;j++)
 816          e[subvect_size*i+j]=s*shape_cb[ind[i]*subvect_size+j];
 817    }
 818    /* Update excitation */
 819    for (j=0;j<nsf;j++)
 820       exc[j]+=e[j];
 821
 822    /* Update target */
 823    residue_zero(e, awk1, r, nsf, p);
 824    syn_filt_zero(r, ak, r, nsf, p);
 825    syn_filt_zero(r, awk2, r, nsf,p);
 826    for (j=0;j<nsf;j++)
 827       target[j]-=r[j];
 828
 829
 830
 831    POP(stack);
 832    POP(stack);
 833    POP(stack);
 834    POP(stack);
 835    POP(stack);
 836    POP(stack);
 837 }
 838
 839
 840 void split_cb_search2(
 841 float target[],                 /* target vector */
 842 float ak[],                     /* LPCs for this subframe */
 843 float awk1[],                   /* Weighted LPCs for this subframe */
 844 float awk2[],                   /* Weighted LPCs for this subframe */
 845 void *par,                      /* Codebook/search parameters*/
 846 int   p,                        /* number of LPC coeffs */
 847 int   nsf,                      /* number of samples in subframe */
 848 float *exc,
 849 SpeexBits *bits,
 850 float *stack
 851 )
 852 {
 853    int i,j, id;
 854    float *resp, *E, q;
 855    float *t, *r, *e;
 856    float *gains;
 857    int *ind, *gain_ind;
 858    float *shape_cb;
 859    int shape_cb_size, subvect_size, nb_subvect;
 860    float exc_energy=0;
 861    split_cb_params *params;
 862
 863    params = (split_cb_params *) par;
 864    subvect_size = params->subvect_size;
 865    nb_subvect = params->nb_subvect;
 866    shape_cb_size = 1<<params->shape_bits;
 867    shape_cb = params->shape_cb;
 868    resp = PUSH(stack, shape_cb_size*subvect_size);
 869    E = PUSH(stack, shape_cb_size);
 870    t = PUSH(stack, nsf);
 871    r = PUSH(stack, nsf);
 872    e = PUSH(stack, nsf);
 873    gains = PUSH(stack, nb_subvect);
 874    ind = (int*)PUSH(stack, nb_subvect);
 875    gain_ind = (int*)PUSH(stack, nb_subvect);
 876
 877    /* Compute energy of the "real excitation" */
 878    syn_filt_zero(target, awk1, e, nsf, p);
 879    residue_zero(e, ak, e, nsf, p);
 880    residue_zero(e, awk2, e, nsf, p);
 881    for (i=0;i<nsf;i++)
 882       exc_energy += e[i]*e[i];
 883    exc_energy=sqrt(exc_energy/nb_subvect);
 884
 885    /* Quantize global ("average") gain */
 886    q=log(exc_energy+.1);
 887    q=floor(.5+2*(q-2));
 888    if (q<0)
 889       q=0;
 890    if (q>15)
 891       q=15;
 892    id = (int)q;
 893    speex_bits_pack(bits, id, 4);
 894    exc_energy=exp(.5*q+2);
 895
 896
 897    for (i=0;i<nsf;i++)
 898       t[i]=target[i];
 899
 900    e[0]=1;
 901    for (i=1;i<nsf;i++)
 902       e[i]=0;
 903    residue_zero(e, awk1, r, nsf, p);
 904    syn_filt_zero(r, ak, r, nsf, p);
 905    syn_filt_zero(r, awk2, r, nsf,p);
 906
 907    /* Pre-compute codewords response and energy */
 908    for (i=0;i<shape_cb_size;i++)
 909    {
 910       float *res = resp+i*subvect_size;
 911
 912       /* Compute codeword response */
 913       int k;
 914       for(j=0;j<subvect_size;j++)
 915          res[j]=0;
 916       for(j=0;j<subvect_size;j++)
 917       {
 918          for (k=j;k<subvect_size;k++)
 919             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
 920       }
 921       /* Compute energy of codeword response */
 922       E[i]=0;
 923       for(j=0;j<subvect_size;j++)
 924          E[i]+=res[j]*res[j];
 925       E[i]=1/(.001+E[i]);
 926    }
 927
 928    for (i=0;i<nb_subvect;i++)
 929    {
 930       int best_index[2]={0,0}, k, m, best_gain_ind[2]={0,0};
 931       float g, corr, best_gain[2]={0,0}, score, best_score[2]={-1,-1};
 932       /* Find best codeword for current sub-vector */
 933       for (j=0;j<shape_cb_size;j++)
 934       {
 935          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
 936          score=corr*corr*E[j];
 937          g = corr*E[j];
 938          if (score>best_score[0])
 939          {
 940             best_index[1]=best_index[0];
 941             best_score[1]=best_score[0];
 942             best_gain[1]=best_gain[0];
 943
 944             best_index[0]=j;
 945             best_score[0]=score;
 946             best_gain[0]=g;
 947          } else if (score>best_score[1]) {
 948             best_index[1]=j;
 949             best_score[1]=score;
 950             best_gain[1]=g;
 951          }
 952       }
 953
 954       /* Quantize gain */
 955       for (k=0;k<2;k++) {
 956          int s=0, best_id;
 957          best_gain[k] /= .01+exc_energy;
 958          if (best_gain[k]<0)
 959          {
 960             best_gain[k]=-best_gain[k];
 961             s=1;
 962          }
 963
 964          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
 965          best_id = vq_index(&best_gain[k], scal_gains4, 1, 8);
 966
 967          best_gain_ind[k]=best_id;
 968          best_gain[k]=scal_gains4[best_id];
 969          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
 970          if (s)
 971             best_gain[k]=-best_gain[k];
 972          best_gain[k] *= exc_energy;
 973       }
 974
 975
 976
 977       if (i<nb_subvect-1) {
 978          int best_index2=0;
 979          float best_score2=-1, best_gain2=0;
 980          int nbest;
 981          float err[2]={0,0};
 982          float *tt=PUSH(stack,nsf);
 983          for (nbest=0;nbest<2;nbest++)
 984          {
 985             for (j=0;j<nsf;j++)
 986                tt[j]=t[j];
 987             for (j=0;j<subvect_size;j++)
 988             {
 989                g=best_gain[nbest]*shape_cb[best_index[nbest]*subvect_size+j];
 990                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
 991                   tt[k] -= g*r[m];
 992             }
 993
 994
 995             for (j=0;j<shape_cb_size;j++)
 996             {
 997                corr=xcorr(resp+j*subvect_size,tt+subvect_size*(i+1),subvect_size);
 998                score=corr*corr*E[j];
 999                g = corr*E[j];
1000                if (score>best_score2)
1001                {
1002                   best_index2=j;
1003                   best_score2=score;
1004                   best_gain2=g;
1005                }
1006             }
1007
1008             {
1009                int s=0, best_id;
1010                best_gain2 /= .01+exc_energy;
1011                if (best_gain2<0)
1012                {
1013                   best_gain2=-best_gain2;
1014                   s=1;
1015                }
1016                best_id = vq_index(&best_gain2, scal_gains4, 1, 8);
1017                best_gain2=scal_gains4[best_id];
1018                if (s)
1019                   best_gain2=-best_gain2;
1020                best_gain2 *= exc_energy;
1021             }
1022
1023             for (j=0;j<subvect_size;j++)
1024             {
1025                g=best_gain2*shape_cb[best_index2*subvect_size+j];
1026                for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
1027                   tt[k] -= g*r[m];
1028             }
1029             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
1030                err[nbest]-=tt[j]*tt[j];
1031
1032             best_score[nbest]=err[nbest];
1033          }
1034
1035          if (best_score[1]>best_score[0])
1036          {
1037             best_index[0]=best_index[1];
1038             best_score[0]=best_score[1];
1039             best_gain[0]=best_gain[1];
1040             best_gain_ind[0]=best_gain_ind[1];
1041          }
1042          POP(stack);
1043       }
1044
1045
1046
1047
1048       ind[i]=best_index[0];
1049       gain_ind[i]=best_gain_ind[0];
1050       gains[i]=best_gain[0];
1051       /* Update target for next subvector */
1052       for (j=0;j<subvect_size;j++)
1053       {
1054          g=best_gain[0]*shape_cb[best_index[0]*subvect_size+j];
1055          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
1056             t[k] -= g*r[m];
1057       }
1058    }
1059    for (i=0;i<nb_subvect;i++)
1060    {
1061       speex_bits_pack(bits, ind[i], params->shape_bits);
1062       if (gains[i]<0)
1063          speex_bits_pack(bits, 1, 1);
1064       else
1065          speex_bits_pack(bits, 0, 1);
1066       speex_bits_pack(bits, gain_ind[i], 3);
1067       /*printf ("encode split: %d %d %f\n", i, ind[i], gains[i]);*/
1068
1069    }
1070    /* Put everything back together */
1071    for (i=0;i<nb_subvect;i++)
1072       for (j=0;j<subvect_size;j++)
1073          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
1074
1075    /* Update excitation */
1076    for (j=0;j<nsf;j++)
1077       exc[j]+=e[j];
1078
1079    /* Update target */
1080    residue_zero(e, awk1, r, nsf, p);
1081    syn_filt_zero(r, ak, r, nsf, p);
1082    syn_filt_zero(r, awk2, r, nsf,p);
1083    for (j=0;j<nsf;j++)
1084       target[j]-=r[j];
1085
1086
1087
1088
1089    POP(stack);
1090    POP(stack);
1091    POP(stack);
1092    POP(stack);
1093    POP(stack);
1094    POP(stack);
1095    POP(stack);
1096 }
1097
1098
1099
1100
1101 void split_cb_unquant(
1102 float *exc,
1103 void *par,                      /* non-overlapping codebook */
1104 int   nsf,                      /* number of samples in subframe */
1105 SpeexBits *bits,
1106 float *stack
1107 )
1108 {
1109    int i,j;
1110    int *ind;
1111    float *gains;
1112    float *sign;
1113    float *shape_cb, exc_energy;
1114    int shape_cb_size, subvect_size, nb_subvect;
1115    split_cb_params *params;
1116
1117    params = (split_cb_params *) par;
1118    subvect_size = params->subvect_size;
1119    nb_subvect = params->nb_subvect;
1120    shape_cb_size = 1<<params->shape_bits;
1121    shape_cb = params->shape_cb;
1122
1123    ind = (int*)PUSH(stack, nb_subvect);
1124    gains = PUSH(stack, nb_subvect);
1125    sign = PUSH(stack, nb_subvect);
1126
1127    /* Decode global (average) gain */
1128    {
1129       int id;
1130       id = speex_bits_unpack_unsigned(bits, 4);
1131       exc_energy=exp(.5*id+2);
1132    }
1133
1134    /* Decode codewords and gains */
1135    for (i=0;i<nb_subvect;i++)
1136    {
1137       int gain_id;
1138       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1139       if (speex_bits_unpack_unsigned(bits, 1))
1140          sign[i]=-1;
1141       else
1142          sign[i]=1;
1143
1144       gain_id = speex_bits_unpack_unsigned(bits, 3);
1145       gains[i]=scal_gains4[gain_id];
1146       gains[i] *= sign[i];
1147       gains[i] *= exc_energy;
1148
1149       /*printf ("decode split: %d %d %f\n", i, ind[i], gains[i]);*/
1150    }
1151
1152    /* Compute decoded excitation */
1153    for (i=0;i<nb_subvect;i++)
1154       for (j=0;j<subvect_size;j++)
1155          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
1156
1157    POP(stack);
1158    POP(stack);
1159    POP(stack);
1160 }
1161
1162
1163
1164 void split_cb_nogain_unquant(
1165 float *exc,
1166 void *par,                      /* non-overlapping codebook */
1167 int   nsf,                      /* number of samples in subframe */
1168 SpeexBits *bits,
1169 float *stack
1170 )
1171 {
1172    int i,j;
1173    int *ind;
1174    float *shape_cb;
1175    int shape_cb_size, subvect_size, nb_subvect;
1176    split_cb_params *params;
1177
1178    params = (split_cb_params *) par;
1179    subvect_size = params->subvect_size;
1180    nb_subvect = params->nb_subvect;
1181    shape_cb_size = 1<<params->shape_bits;
1182    shape_cb = params->shape_cb;
1183
1184    ind = (int*)PUSH(stack, nb_subvect);
1185
1186    /* Decode codewords and gains */
1187    for (i=0;i<nb_subvect;i++)
1188       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1189
1190    /* Compute decoded excitation */
1191    for (i=0;i<nb_subvect;i++)
1192       for (j=0;j<subvect_size;j++)
1193          exc[subvect_size*i+j]+=shape_cb[ind[i]*subvect_size+j];
1194
1195    POP(stack);
1196 }
1197
1198 void split_cb_shape_sign_unquant(
1199 float *exc,
1200 void *par,                      /* non-overlapping codebook */
1201 int   nsf,                      /* number of samples in subframe */
1202 SpeexBits *bits,
1203 float *stack
1204 )
1205 {
1206    int i,j;
1207    int *ind, *signs;
1208    float *shape_cb;
1209    int shape_cb_size, subvect_size, nb_subvect;
1210    split_cb_params *params;
1211
1212    params = (split_cb_params *) par;
1213    subvect_size = params->subvect_size;
1214    nb_subvect = params->nb_subvect;
1215    shape_cb_size = 1<<params->shape_bits;
1216    shape_cb = params->shape_cb;
1217
1218    ind = (int*)PUSH(stack, nb_subvect);
1219    signs = (int*)PUSH(stack, nb_subvect);
1220
1221    /* Decode codewords and gains */
1222    for (i=0;i<nb_subvect;i++)
1223    {
1224       signs[i] = speex_bits_unpack_unsigned(bits, 1);
1225       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1226    }
1227    /* Compute decoded excitation */
1228    for (i=0;i<nb_subvect;i++)
1229    {
1230       float s=1;
1231       if (signs[i])
1232          s=-1;
1233       for (j=0;j<subvect_size;j++)
1234          exc[subvect_size*i+j]+=s*shape_cb[ind[i]*subvect_size+j];
1235    }
1236    POP(stack);
1237    POP(stack);
1238 }