hvirtual/toolame-02l/psycho_2.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <math.h>
   4 #include <string.h>
   5 #include "common.h"
   6 #include "encoder.h"
   7 #include "mem.h"
   8 #include "fft.h"
   9 #include "psycho_2.h"
  10
  11 /* The static variables "r", "phi_sav", "new", "old" and "oldest" have    */
  12 /* to be remembered for the unpredictability measure.  For "r" and        */
  13 /* "phi_sav", the first index from the left is the channel select and     */
  14 /* the second index is the "age" of the data.                             */
  15
  16 static int new = 0, old = 1, oldest = 0;
  17 static int init = 0, flush, sync_flush, syncsize, sfreq_idx;
  18
  19 /* The following static variables are constants.                           */
  20
  21 static double nmt = 5.5;
  22
  23 static FLOAT crit_band[27] = { 0, 100, 200, 300, 400, 510, 630, 770,
  24   920, 1080, 1270, 1480, 1720, 2000, 2320, 2700,
  25   3150, 3700, 4400, 5300, 6400, 7700, 9500, 12000,
  26   15500, 25000, 30000
  27 };
  28
  29 static FLOAT bmax[27] = { 20.0, 20.0, 20.0, 20.0, 20.0, 17.0, 15.0,
  30   10.0, 7.0, 4.4, 4.5, 4.5, 4.5, 4.5,
  31   4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5,
  32   4.5, 4.5, 4.5, 3.5, 3.5, 3.5
  33 };
  34
  35 static FLOAT *grouped_c, *grouped_e, *nb, *cb, *ecb, *bc;
  36 static FLOAT *wsamp_r, *phi, *energy;
  37 static FLOAT *c, *fthr;
  38 static F32 *snrtmp;
  39
  40 static int *numlines;
  41 static int *partition;
  42 static FLOAT *cbval, *rnorm;
  43 static FLOAT *window;
  44 static FLOAT *absthr;
  45 static double *tmn;
  46 static FCB *s;
  47 static FHBLK *lthr;
  48 static F2HBLK *r, *phi_sav;
  49
  50 void psycho_2_init (double sfreq);
  51
  52 void psycho_2 (short int *buffer, short int savebuf[1056], int chn,
  53                 double *smr, double sfreq, options *glopts)
  54 /* to match prototype : FLOAT args are always double */
  55 {
  56   unsigned int i, j, k;
  57   FLOAT r_prime, phi_prime;
  58   FLOAT minthres, sum_energy;
  59   double tb, temp1, temp2, temp3;
  60
  61   if (init == 0) {
  62     psycho_2_init (sfreq);
  63     init++;
  64   }
  65
  66   for (i = 0; i < 2; i++) {
  67       /*****************************************************************************
  68        * Net offset is 480 samples (1056-576) for layer 2; this is because one must*
  69        * stagger input data by 256 samples to synchronize psychoacoustic model with*
  70        * filter bank outputs, then stagger so that center of 1024 FFT window lines *
  71        * up with center of 576 "new" audio samples.                                *
  72
  73            flush = 384*3.0/2.0;  = 576
  74            syncsize = 1056;
  75            sync_flush = syncsize - flush;   480
  76            BLKSIZE = 1024
  77        *****************************************************************************/
  78
  79     for (j = 0; j < 480; j++) {
  80       savebuf[j] = savebuf[j + flush];
  81       wsamp_r[j] = window[j] * ((FLOAT) savebuf[j]);
  82     }
  83     for (; j < 1024; j++) {
  84       savebuf[j] = *buffer++;
  85       wsamp_r[j] = window[j] * ((FLOAT) savebuf[j]);
  86     }
  87     for (; j < 1056; j++)
  88       savebuf[j] = *buffer++;
  89
  90       /**Compute FFT****************************************************************/
  91     psycho_2_fft (wsamp_r, energy, phi);
  92       /*****************************************************************************
  93        * calculate the unpredictability measure, given energy[f] and phi[f]        *
  94        *****************************************************************************/
  95     /*only update data "age" pointers after you are done with both channels      */
  96     /*for layer 1 computations, for the layer 2 double computations, the pointers */
  97     /*are reset automatically on the second pass                                 */
  98     {
  99       if (new == 0) {
 100         new = 1;
 101         oldest = 1;
 102       } else {
 103         new = 0;
 104         oldest = 0;
 105       }
 106       if (old == 0)
 107         old = 1;
 108       else
 109         old = 0;
 110     }
 111     for (j = 0; j < HBLKSIZE; j++) {
 112       r_prime = 2.0 * r[chn][old][j] - r[chn][oldest][j];
 113       phi_prime = 2.0 * phi_sav[chn][old][j] - phi_sav[chn][oldest][j];
 114       r[chn][new][j] = sqrt ((double) energy[j]);
 115       phi_sav[chn][new][j] = phi[j];
 116 #ifdef SINCOS
 117       {
 118         // 12% faster
 119         //#warning "Use __sincos"
 120         double sphi, cphi, sprime, cprime;
 121         __sincos ((double) phi[j], &sphi, &cphi);
 122         __sincos ((double) phi_prime, &sprime, &cprime);
 123         temp1 = r[chn][new][j] * cphi - r_prime * cprime;
 124         temp2 = r[chn][new][j] * sphi - r_prime * sprime;
 125       }
 126 #else
 127       temp1 =
 128         r[chn][new][j] * cos ((double) phi[j]) -
 129         r_prime * cos ((double) phi_prime);
 130       temp2 =
 131         r[chn][new][j] * sin ((double) phi[j]) -
 132         r_prime * sin ((double) phi_prime);
 133 #endif
 134
 135       temp3 = r[chn][new][j] + fabs ((double) r_prime);
 136       if (temp3 != 0)
 137         c[j] = sqrt (temp1 * temp1 + temp2 * temp2) / temp3;
 138       else
 139         c[j] = 0;
 140     }
 141       /*****************************************************************************
 142        * Calculate the grouped, energy-weighted, unpredictability measure,         *
 143        * grouped_c[], and the grouped energy. grouped_e[]                          *
 144        *****************************************************************************/
 145
 146     for (j = 1; j < CBANDS; j++) {
 147       grouped_e[j] = 0;
 148       grouped_c[j] = 0;
 149     }
 150     grouped_e[0] = energy[0];
 151     grouped_c[0] = energy[0] * c[0];
 152     for (j = 1; j < HBLKSIZE; j++) {
 153       grouped_e[partition[j]] += energy[j];
 154       grouped_c[partition[j]] += energy[j] * c[j];
 155     }
 156
 157       /*****************************************************************************
 158        * convolve the grouped energy-weighted unpredictability measure             *
 159        * and the grouped energy with the spreading function, s[j][k]               *
 160        *****************************************************************************/
 161     for (j = 0; j < CBANDS; j++) {
 162       ecb[j] = 0;
 163       cb[j] = 0;
 164       for (k = 0; k < CBANDS; k++) {
 165         if (s[j][k] != 0.0) {
 166           ecb[j] += s[j][k] * grouped_e[k];
 167           cb[j] += s[j][k] * grouped_c[k];
 168         }
 169       }
 170       if (ecb[j] != 0)
 171         cb[j] = cb[j] / ecb[j];
 172       else
 173         cb[j] = 0;
 174     }
 175
 176       /*****************************************************************************
 177        * Calculate the required SNR for each of the frequency partitions           *
 178        *         this whole section can be accomplished by a table lookup          *
 179        *****************************************************************************/
 180     for (j = 0; j < CBANDS; j++) {
 181       if (cb[j] < .05)
 182         cb[j] = 0.05;
 183       else if (cb[j] > .5)
 184         cb[j] = 0.5;
 185       tb = -0.434294482 * log ((double) cb[j]) - 0.301029996;
 186       cb[j] = tb;
 187       bc[j] = tmn[j] * tb + nmt * (1.0 - tb);
 188       k = cbval[j] + 0.5;
 189       bc[j] = (bc[j] > bmax[k]) ? bc[j] : bmax[k];
 190       bc[j] = exp ((double) -bc[j] * LN_TO_LOG10);
 191     }
 192
 193       /*****************************************************************************
 194        * Calculate the permissible noise energy level in each of the frequency     *
 195        * partitions. Include absolute threshold and pre-echo controls              *
 196        *         this whole section can be accomplished by a table lookup          *
 197        *****************************************************************************/
 198     for (j = 0; j < CBANDS; j++)
 199       if (rnorm[j] && numlines[j])
 200         nb[j] = ecb[j] * bc[j] / (rnorm[j] * numlines[j]);
 201       else
 202         nb[j] = 0;
 203     for (j = 0; j < HBLKSIZE; j++) {
 204       /*temp1 is the preliminary threshold */
 205       temp1 = nb[partition[j]];
 206       temp1 = (temp1 > absthr[j]) ? temp1 : absthr[j];
 207 #ifdef LAYERI
 208       /*do not use pre-echo control for layer 2 because it may do bad things to the */
 209       /*  MUSICAM bit allocation algorithm                                         */
 210       if (lay == 1) {
 211         fthr[j] = (temp1 < lthr[chn][j]) ? temp1 : lthr[chn][j];
 212         temp2 = temp1 * 0.00316;
 213         fthr[j] = (temp2 > fthr[j]) ? temp2 : fthr[j];
 214       } else
 215         fthr[j] = temp1;
 216       lthr[chn][j] = LXMIN * temp1;
 217 #else
 218       fthr[j] = temp1;
 219       lthr[chn][j] = LXMIN * temp1;
 220 #endif
 221     }
 222
 223       /*****************************************************************************
 224        * Translate the 512 threshold values to the 32 filter bands of the coder    *
 225        *****************************************************************************/
 226     for (j = 0; j < 193; j += 16) {
 227       minthres = 60802371420160.0;
 228       sum_energy = 0.0;
 229       for (k = 0; k < 17; k++) {
 230         if (minthres > fthr[j + k])
 231           minthres = fthr[j + k];
 232         sum_energy += energy[j + k];
 233       }
 234       snrtmp[i][j / 16] = sum_energy / (minthres * 17.0);
 235       snrtmp[i][j / 16] = 4.342944819 * log ((double) snrtmp[i][j / 16]);
 236     }
 237     for (j = 208; j < (HBLKSIZE - 1); j += 16) {
 238       minthres = 0.0;
 239       sum_energy = 0.0;
 240       for (k = 0; k < 17; k++) {
 241         minthres += fthr[j + k];
 242         sum_energy += energy[j + k];
 243       }
 244       snrtmp[i][j / 16] = sum_energy / minthres;
 245       snrtmp[i][j / 16] = 4.342944819 * log ((double) snrtmp[i][j / 16]);
 246     }
 247       /*****************************************************************************
 248        * End of Psychoacuostic calculation loop                                    *
 249        *****************************************************************************/
 250   }
 251   for (i = 0; i < 32; i++) {
 252     smr[i] = (snrtmp[0][i] > snrtmp[1][i]) ? snrtmp[0][i] : snrtmp[1][i];
 253   }
 254 }
 255
 256 /********************************
 257  * init psycho model 2
 258  ********************************/
 259 void psycho_2_init (double sfreq)
 260 {
 261   int i, j;
 262   FLOAT freq_mult;
 263   double temp1, temp2, temp3;
 264   FLOAT bval_lo;
 265
 266   grouped_c = (FLOAT *) mem_alloc (sizeof (FCB), "grouped_c");
 267   grouped_e = (FLOAT *) mem_alloc (sizeof (FCB), "grouped_e");
 268   nb = (FLOAT *) mem_alloc (sizeof (FCB), "nb");
 269   cb = (FLOAT *) mem_alloc (sizeof (FCB), "cb");
 270   ecb = (FLOAT *) mem_alloc (sizeof (FCB), "ecb");
 271   bc = (FLOAT *) mem_alloc (sizeof (FCB), "bc");
 272   wsamp_r = (FLOAT *) mem_alloc (sizeof (FBLK), "wsamp_r");
 273   phi = (FLOAT *) mem_alloc (sizeof (FBLK), "phi");
 274   energy = (FLOAT *) mem_alloc (sizeof (FBLK), "energy");
 275   c = (FLOAT *) mem_alloc (sizeof (FHBLK), "c");
 276   fthr = (FLOAT *) mem_alloc (sizeof (FHBLK), "fthr");
 277   snrtmp = (F32 *) mem_alloc (sizeof (F2_32), "snrtmp");
 278
 279   numlines = (int *) mem_alloc (sizeof (ICB), "numlines");
 280   partition = (int *) mem_alloc (sizeof (IHBLK), "partition");
 281   cbval = (FLOAT *) mem_alloc (sizeof (FCB), "cbval");
 282   rnorm = (FLOAT *) mem_alloc (sizeof (FCB), "rnorm");
 283   window = (FLOAT *) mem_alloc (sizeof (FBLK), "window");
 284   absthr = (FLOAT *) mem_alloc (sizeof (FHBLK), "absthr");
 285   tmn = (double *) mem_alloc (sizeof (DCB), "tmn");
 286   s = (FCB *) mem_alloc (sizeof (FCBCB), "s");
 287   lthr = (FHBLK *) mem_alloc (sizeof (F2HBLK), "lthr");
 288   r = (F2HBLK *) mem_alloc (sizeof (F22HBLK), "r");
 289   phi_sav = (F2HBLK *) mem_alloc (sizeof (F22HBLK), "phi_sav");
 290
 291   i = sfreq + 0.5;
 292   switch (i) {
 293   case 32000:
 294   case 16000:
 295     sfreq_idx = 0;
 296     break;
 297   case 44100:
 298   case 22050:
 299     sfreq_idx = 1;
 300     break;
 301   case 48000:
 302   case 24000:
 303     sfreq_idx = 2;
 304     break;
 305   default:
 306     fprintf (stderr, "error, invalid sampling frequency: %d Hz\n", i);
 307     exit (-1);
 308   }
 309   fprintf (stderr, "absthr[][] sampling frequency index: %d\n", sfreq_idx);
 310   psycho_2_read_absthr (absthr, sfreq_idx);
 311
 312   flush = 384 * 3.0 / 2.0;
 313   syncsize = 1056;
 314   sync_flush = syncsize - flush;
 315
 316   /* calculate HANN window coefficients */
 317   /*   for(i=0;i<BLKSIZE;i++)window[i]=0.5*(1-cos(2.0*PI*i/(BLKSIZE-1.0))); */
 318   for (i = 0; i < BLKSIZE; i++)
 319     window[i] = 0.5 * (1 - cos (2.0 * PI * (i - 0.5) / BLKSIZE));
 320   /* reset states used in unpredictability measure */
 321   for (i = 0; i < HBLKSIZE; i++) {
 322     r[0][0][i] = r[1][0][i] = r[0][1][i] = r[1][1][i] = 0;
 323     phi_sav[0][0][i] = phi_sav[1][0][i] = 0;
 324     phi_sav[0][1][i] = phi_sav[1][1][i] = 0;
 325     lthr[0][i] = 60802371420160.0;
 326     lthr[1][i] = 60802371420160.0;
 327   }
 328   /*****************************************************************************
 329    * Initialization: Compute the following constants for use later             *
 330    *    partition[HBLKSIZE] = the partition number associated with each        *
 331    *                          frequency line                                   *
 332    *    cbval[CBANDS]       = the center (average) bark value of each          *
 333    *                          partition                                        *
 334    *    numlines[CBANDS]    = the number of frequency lines in each partition  *
 335    *    tmn[CBANDS]         = tone masking noise                               *
 336    *****************************************************************************/
 337   /* compute fft frequency multiplicand */
 338   freq_mult = sfreq / BLKSIZE;
 339
 340   /* calculate fft frequency, then bval of each line (use fthr[] as tmp storage) */
 341   for (i = 0; i < HBLKSIZE; i++) {
 342     temp1 = i * freq_mult;
 343     j = 1;
 344     while (temp1 > crit_band[j])
 345       j++;
 346     fthr[i] =
 347       j - 1 + (temp1 - crit_band[j - 1]) / (crit_band[j] - crit_band[j - 1]);
 348   }
 349   partition[0] = 0;
 350   /* temp2 is the counter of the number of frequency lines in each partition */
 351   temp2 = 1;
 352   cbval[0] = fthr[0];
 353   bval_lo = fthr[0];
 354   for (i = 1; i < HBLKSIZE; i++) {
 355     if ((fthr[i] - bval_lo) > 0.33) {
 356       partition[i] = partition[i - 1] + 1;
 357       cbval[partition[i - 1]] = cbval[partition[i - 1]] / temp2;
 358       cbval[partition[i]] = fthr[i];
 359       bval_lo = fthr[i];
 360       numlines[partition[i - 1]] = temp2;
 361       temp2 = 1;
 362     } else {
 363       partition[i] = partition[i - 1];
 364       cbval[partition[i]] += fthr[i];
 365       temp2++;
 366     }
 367   }
 368   numlines[partition[i - 1]] = temp2;
 369   cbval[partition[i - 1]] = cbval[partition[i - 1]] / temp2;
 370
 371   /************************************************************************
 372    * Now compute the spreading function, s[j][i], the value of the spread-*
 373    * ing function, centered at band j, for band i, store for later use    *
 374    ************************************************************************/
 375   for (j = 0; j < CBANDS; j++) {
 376     for (i = 0; i < CBANDS; i++) {
 377       temp1 = (cbval[i] - cbval[j]) * 1.05;
 378       if (temp1 >= 0.5 && temp1 <= 2.5) {
 379         temp2 = temp1 - 0.5;
 380         temp2 = 8.0 * (temp2 * temp2 - 2.0 * temp2);
 381       } else
 382         temp2 = 0;
 383       temp1 += 0.474;
 384       temp3 =
 385         15.811389 + 7.5 * temp1 -
 386         17.5 * sqrt ((double) (1.0 + temp1 * temp1));
 387       if (temp3 <= -100)
 388         s[i][j] = 0;
 389       else {
 390         temp3 = (temp2 + temp3) * LN_TO_LOG10;
 391         s[i][j] = exp (temp3);
 392       }
 393     }
 394   }
 395
 396   /* Calculate Tone Masking Noise values */
 397   for (j = 0; j < CBANDS; j++) {
 398     temp1 = 15.5 + cbval[j];
 399     tmn[j] = (temp1 > 24.5) ? temp1 : 24.5;
 400     /* Calculate normalization factors for the net spreading functions */
 401     rnorm[j] = 0;
 402     for (i = 0; i < CBANDS; i++) {
 403       rnorm[j] += s[j][i];
 404     }
 405   }
 406
 407   if (glopts.verbosity > 10){
 408     /* Dump All the Values to STDOUT and exit */
 409     int wlow, whigh=0;
 410     fprintf(stdout,"psy model 2 init\n");
 411     fprintf(stdout,"index \tnlines \twlow \twhigh \tbval \tminval \ttmn\n");
 412     for (i=0;i<CBANDS;i++) {
 413       wlow = whigh+1;
 414       whigh = wlow + numlines[i] - 1;
 415       fprintf(stdout,"%i \t%i \t%i \t%i \t%5.2f \t%4.2f \t%4.2f\n",i+1, numlines[i],wlow, whigh, cbval[i],bmax[(int)(cbval[i]+0.5)],tmn[i]);
 416     }
 417     exit(0);
 418   }
 419
 420 }
 421
 422 void psycho_2_read_absthr (absthr, table)
 423      FLOAT *absthr;
 424      int table;
 425 {
 426   int j;
 427 #include "absthr.h"
 428
 429   if ((table < 0) || (table > 3)) {
 430     printf ("internal error: wrong table number");
 431     return;
 432   }
 433
 434   for (j = 0; j < HBLKSIZE; j++) {
 435     absthr[j] = absthr_table[table][j];
 436   }
 437   return;
 438 }