Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 1 | /* Copyright (C) 2005 Jean-Marc Valin, CSIRO, Christopher Montgomery |
| 2 | File: vorbis_psy.c |
| 3 | |
| 4 | Redistribution and use in source and binary forms, with or without |
| 5 | modification, are permitted provided that the following conditions |
| 6 | are met: |
| 7 | |
| 8 | - Redistributions of source code must retain the above copyright |
| 9 | notice, this list of conditions and the following disclaimer. |
| 10 | |
| 11 | - Redistributions in binary form must reproduce the above copyright |
| 12 | notice, this list of conditions and the following disclaimer in the |
| 13 | documentation and/or other materials provided with the distribution. |
| 14 | |
| 15 | - Neither the name of the Xiph.org Foundation nor the names of its |
| 16 | contributors may be used to endorse or promote products derived from |
| 17 | this software without specific prior written permission. |
| 18 | |
| 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 20 | ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
| 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR |
| 23 | CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
| 24 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
| 25 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
| 26 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF |
| 27 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING |
| 28 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
| 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 30 | */ |
| 31 | |
| 32 | #ifdef HAVE_CONFIG_H |
| 33 | #include "config.h" |
| 34 | #endif |
| 35 | |
| 36 | #ifdef VORBIS_PSYCHO |
| 37 | |
| 38 | #include "arch.h" |
| 39 | #include "smallft.h" |
| 40 | #include "lpc.h" |
| 41 | #include "vorbis_psy.h" |
| 42 | |
| 43 | #include <stdlib.h> |
| 44 | #include <math.h> |
| 45 | #include <string.h> |
| 46 | |
| 47 | /* psychoacoustic setup ********************************************/ |
| 48 | |
| 49 | static VorbisPsyInfo example_tuning = { |
| 50 | |
| 51 | .5,.5, |
| 52 | 3,3,25, |
| 53 | |
| 54 | /*63 125 250 500 1k 2k 4k 8k 16k*/ |
| 55 | // vorbis mode 4 style |
| 56 | //{-32,-32,-32,-32,-28,-24,-22,-20,-20, -20, -20, -8, -6, -6, -6, -6, -6}, |
| 57 | { -4, -6, -6, -6, -6, -6, -6, -6, -8, -8,-10,-10, -8, -6, -4, -4, -2}, |
| 58 | |
| 59 | { |
| 60 | 0, 1, 2, 3, 4, 5, 5, 5, /* 7dB */ |
| 61 | 6, 6, 6, 5, 4, 4, 4, 4, /* 15dB */ |
| 62 | 4, 4, 5, 5, 5, 6, 6, 6, /* 23dB */ |
| 63 | 7, 7, 7, 8, 8, 8, 9, 10, /* 31dB */ |
| 64 | 11,12,13,14,15,16,17, 18, /* 39dB */ |
| 65 | } |
| 66 | |
| 67 | }; |
| 68 | |
| 69 | |
| 70 | |
| 71 | /* there was no great place to put this.... */ |
| 72 | #include <stdio.h> |
| 73 | static void _analysis_output(char *base,int i,float *v,int n,int bark,int dB){ |
| 74 | int j; |
| 75 | FILE *of; |
| 76 | char buffer[80]; |
| 77 | |
| 78 | sprintf(buffer,"%s_%d.m",base,i); |
| 79 | of=fopen(buffer,"w"); |
| 80 | |
| 81 | if(!of)perror("failed to open data dump file"); |
| 82 | |
| 83 | for(j=0;j<n;j++){ |
| 84 | if(bark){ |
| 85 | float b=toBARK((4000.f*j/n)+.25); |
| 86 | fprintf(of,"%f ",b); |
| 87 | }else |
| 88 | fprintf(of,"%f ",(double)j); |
| 89 | |
| 90 | if(dB){ |
| 91 | float val; |
| 92 | if(v[j]==0.) |
| 93 | val=-140.; |
| 94 | else |
| 95 | val=todB(v[j]); |
| 96 | fprintf(of,"%f\n",val); |
| 97 | }else{ |
| 98 | fprintf(of,"%f\n",v[j]); |
| 99 | } |
| 100 | } |
| 101 | fclose(of); |
| 102 | } |
| 103 | |
| 104 | static void bark_noise_hybridmp(int n,const long *b, |
| 105 | const float *f, |
| 106 | float *noise, |
| 107 | const float offset, |
| 108 | const int fixed){ |
| 109 | |
| 110 | float *N=alloca(n*sizeof(*N)); |
| 111 | float *X=alloca(n*sizeof(*N)); |
| 112 | float *XX=alloca(n*sizeof(*N)); |
| 113 | float *Y=alloca(n*sizeof(*N)); |
| 114 | float *XY=alloca(n*sizeof(*N)); |
| 115 | |
| 116 | float tN, tX, tXX, tY, tXY; |
| 117 | int i; |
| 118 | |
| 119 | int lo, hi; |
| 120 | float R, A, B, D; |
| 121 | float w, x, y; |
| 122 | |
| 123 | tN = tX = tXX = tY = tXY = 0.f; |
| 124 | |
| 125 | y = f[0] + offset; |
| 126 | if (y < 1.f) y = 1.f; |
| 127 | |
| 128 | w = y * y * .5; |
| 129 | |
| 130 | tN += w; |
| 131 | tX += w; |
| 132 | tY += w * y; |
| 133 | |
| 134 | N[0] = tN; |
| 135 | X[0] = tX; |
| 136 | XX[0] = tXX; |
| 137 | Y[0] = tY; |
| 138 | XY[0] = tXY; |
| 139 | |
| 140 | for (i = 1, x = 1.f; i < n; i++, x += 1.f) { |
| 141 | |
| 142 | y = f[i] + offset; |
| 143 | if (y < 1.f) y = 1.f; |
| 144 | |
| 145 | w = y * y; |
| 146 | |
| 147 | tN += w; |
| 148 | tX += w * x; |
| 149 | tXX += w * x * x; |
| 150 | tY += w * y; |
| 151 | tXY += w * x * y; |
| 152 | |
| 153 | N[i] = tN; |
| 154 | X[i] = tX; |
| 155 | XX[i] = tXX; |
| 156 | Y[i] = tY; |
| 157 | XY[i] = tXY; |
| 158 | } |
| 159 | |
| 160 | for (i = 0, x = 0.f;; i++, x += 1.f) { |
| 161 | |
| 162 | lo = b[i] >> 16; |
| 163 | if( lo>=0 ) break; |
| 164 | hi = b[i] & 0xffff; |
| 165 | |
| 166 | tN = N[hi] + N[-lo]; |
| 167 | tX = X[hi] - X[-lo]; |
| 168 | tXX = XX[hi] + XX[-lo]; |
| 169 | tY = Y[hi] + Y[-lo]; |
| 170 | tXY = XY[hi] - XY[-lo]; |
| 171 | |
| 172 | A = tY * tXX - tX * tXY; |
| 173 | B = tN * tXY - tX * tY; |
| 174 | D = tN * tXX - tX * tX; |
| 175 | R = (A + x * B) / D; |
| 176 | if (R < 0.f) |
| 177 | R = 0.f; |
| 178 | |
| 179 | noise[i] = R - offset; |
| 180 | } |
| 181 | |
| 182 | for ( ;; i++, x += 1.f) { |
| 183 | |
| 184 | lo = b[i] >> 16; |
| 185 | hi = b[i] & 0xffff; |
| 186 | if(hi>=n)break; |
| 187 | |
| 188 | tN = N[hi] - N[lo]; |
| 189 | tX = X[hi] - X[lo]; |
| 190 | tXX = XX[hi] - XX[lo]; |
| 191 | tY = Y[hi] - Y[lo]; |
| 192 | tXY = XY[hi] - XY[lo]; |
| 193 | |
| 194 | A = tY * tXX - tX * tXY; |
| 195 | B = tN * tXY - tX * tY; |
| 196 | D = tN * tXX - tX * tX; |
| 197 | R = (A + x * B) / D; |
| 198 | if (R < 0.f) R = 0.f; |
| 199 | |
| 200 | noise[i] = R - offset; |
| 201 | } |
| 202 | for ( ; i < n; i++, x += 1.f) { |
| 203 | |
| 204 | R = (A + x * B) / D; |
| 205 | if (R < 0.f) R = 0.f; |
| 206 | |
| 207 | noise[i] = R - offset; |
| 208 | } |
| 209 | |
| 210 | if (fixed <= 0) return; |
| 211 | |
| 212 | for (i = 0, x = 0.f;; i++, x += 1.f) { |
| 213 | hi = i + fixed / 2; |
| 214 | lo = hi - fixed; |
| 215 | if(lo>=0)break; |
| 216 | |
| 217 | tN = N[hi] + N[-lo]; |
| 218 | tX = X[hi] - X[-lo]; |
| 219 | tXX = XX[hi] + XX[-lo]; |
| 220 | tY = Y[hi] + Y[-lo]; |
| 221 | tXY = XY[hi] - XY[-lo]; |
| 222 | |
| 223 | |
| 224 | A = tY * tXX - tX * tXY; |
| 225 | B = tN * tXY - tX * tY; |
| 226 | D = tN * tXX - tX * tX; |
| 227 | R = (A + x * B) / D; |
| 228 | |
| 229 | if (R - offset < noise[i]) noise[i] = R - offset; |
| 230 | } |
| 231 | for ( ;; i++, x += 1.f) { |
| 232 | |
| 233 | hi = i + fixed / 2; |
| 234 | lo = hi - fixed; |
| 235 | if(hi>=n)break; |
| 236 | |
| 237 | tN = N[hi] - N[lo]; |
| 238 | tX = X[hi] - X[lo]; |
| 239 | tXX = XX[hi] - XX[lo]; |
| 240 | tY = Y[hi] - Y[lo]; |
| 241 | tXY = XY[hi] - XY[lo]; |
| 242 | |
| 243 | A = tY * tXX - tX * tXY; |
| 244 | B = tN * tXY - tX * tY; |
| 245 | D = tN * tXX - tX * tX; |
| 246 | R = (A + x * B) / D; |
| 247 | |
| 248 | if (R - offset < noise[i]) noise[i] = R - offset; |
| 249 | } |
| 250 | for ( ; i < n; i++, x += 1.f) { |
| 251 | R = (A + x * B) / D; |
| 252 | if (R - offset < noise[i]) noise[i] = R - offset; |
| 253 | } |
| 254 | } |
| 255 | |
| 256 | static void _vp_noisemask(VorbisPsy *p, |
| 257 | float *logfreq, |
| 258 | float *logmask){ |
| 259 | |
| 260 | int i,n=p->n/2; |
| 261 | float *work=alloca(n*sizeof(*work)); |
| 262 | |
| 263 | bark_noise_hybridmp(n,p->bark,logfreq,logmask, |
| 264 | 140.,-1); |
| 265 | |
| 266 | for(i=0;i<n;i++)work[i]=logfreq[i]-logmask[i]; |
| 267 | |
| 268 | bark_noise_hybridmp(n,p->bark,work,logmask,0., |
| 269 | p->vi->noisewindowfixed); |
| 270 | |
| 271 | for(i=0;i<n;i++)work[i]=logfreq[i]-work[i]; |
| 272 | |
| 273 | { |
| 274 | static int seq=0; |
| 275 | |
| 276 | float work2[n]; |
| 277 | for(i=0;i<n;i++){ |
| 278 | work2[i]=logmask[i]+work[i]; |
| 279 | } |
| 280 | |
| 281 | //_analysis_output("logfreq",seq,logfreq,n,0,0); |
| 282 | //_analysis_output("median",seq,work,n,0,0); |
| 283 | //_analysis_output("envelope",seq,work2,n,0,0); |
| 284 | seq++; |
| 285 | } |
| 286 | |
| 287 | for(i=0;i<n;i++){ |
| 288 | int dB=logmask[i]+.5; |
| 289 | if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1; |
| 290 | if(dB<0)dB=0; |
| 291 | logmask[i]= work[i]+p->vi->noisecompand[dB]+p->noiseoffset[i]; |
| 292 | } |
| 293 | |
| 294 | } |
| 295 | |
| 296 | VorbisPsy *vorbis_psy_init(int rate, int n) |
| 297 | { |
| 298 | long i,j,lo=-99,hi=1; |
| 299 | VorbisPsy *p = speex_alloc(sizeof(VorbisPsy)); |
| 300 | memset(p,0,sizeof(*p)); |
| 301 | |
| 302 | p->n = n; |
| 303 | spx_drft_init(&p->lookup, n); |
| 304 | p->bark = speex_alloc(n*sizeof(*p->bark)); |
| 305 | p->rate=rate; |
| 306 | p->vi = &example_tuning; |
| 307 | |
| 308 | /* BH4 window */ |
| 309 | p->window = speex_alloc(sizeof(*p->window)*n); |
| 310 | float a0 = .35875f; |
| 311 | float a1 = .48829f; |
| 312 | float a2 = .14128f; |
| 313 | float a3 = .01168f; |
| 314 | for(i=0;i<n;i++) |
| 315 | p->window[i] = //a0 - a1*cos(2.*M_PI/n*(i+.5)) + a2*cos(4.*M_PI/n*(i+.5)) - a3*cos(6.*M_PI/n*(i+.5)); |
| 316 | sin((i+.5)/n * M_PI)*sin((i+.5)/n * M_PI); |
| 317 | /* bark scale lookups */ |
| 318 | for(i=0;i<n;i++){ |
| 319 | float bark=toBARK(rate/(2*n)*i); |
| 320 | |
| 321 | for(;lo+p->vi->noisewindowlomin<i && |
| 322 | toBARK(rate/(2*n)*lo)<(bark-p->vi->noisewindowlo);lo++); |
| 323 | |
| 324 | for(;hi<=n && (hi<i+p->vi->noisewindowhimin || |
| 325 | toBARK(rate/(2*n)*hi)<(bark+p->vi->noisewindowhi));hi++); |
| 326 | |
| 327 | p->bark[i]=((lo-1)<<16)+(hi-1); |
| 328 | |
| 329 | } |
| 330 | |
| 331 | /* set up rolling noise median */ |
| 332 | p->noiseoffset=speex_alloc(n*sizeof(*p->noiseoffset)); |
| 333 | |
| 334 | for(i=0;i<n;i++){ |
| 335 | float halfoc=toOC((i+.5)*rate/(2.*n))*2.; |
| 336 | int inthalfoc; |
| 337 | float del; |
| 338 | |
| 339 | if(halfoc<0)halfoc=0; |
| 340 | if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1; |
| 341 | inthalfoc=(int)halfoc; |
| 342 | del=halfoc-inthalfoc; |
| 343 | |
| 344 | p->noiseoffset[i]= |
| 345 | p->vi->noiseoff[inthalfoc]*(1.-del) + |
| 346 | p->vi->noiseoff[inthalfoc+1]*del; |
| 347 | |
| 348 | } |
| 349 | #if 0 |
| 350 | _analysis_output_always("noiseoff0",ls,p->noiseoffset,n,1,0,0); |
| 351 | #endif |
| 352 | |
| 353 | return p; |
| 354 | } |
| 355 | |
| 356 | void vorbis_psy_destroy(VorbisPsy *p) |
| 357 | { |
| 358 | if(p){ |
| 359 | spx_drft_clear(&p->lookup); |
| 360 | if(p->bark) |
| 361 | speex_free(p->bark); |
| 362 | if(p->noiseoffset) |
| 363 | speex_free(p->noiseoffset); |
| 364 | if(p->window) |
| 365 | speex_free(p->window); |
| 366 | memset(p,0,sizeof(*p)); |
| 367 | speex_free(p); |
| 368 | } |
| 369 | } |
| 370 | |
| 371 | void compute_curve(VorbisPsy *psy, float *audio, float *curve) |
| 372 | { |
| 373 | int i; |
| 374 | float work[psy->n]; |
| 375 | |
| 376 | float scale=4.f/psy->n; |
| 377 | float scale_dB; |
| 378 | |
| 379 | scale_dB=todB(scale); |
| 380 | |
| 381 | /* window the PCM data; use a BH4 window, not vorbis */ |
| 382 | for(i=0;i<psy->n;i++) |
| 383 | work[i]=audio[i] * psy->window[i]; |
| 384 | |
| 385 | { |
| 386 | static int seq=0; |
| 387 | |
| 388 | //_analysis_output("win",seq,work,psy->n,0,0); |
| 389 | |
| 390 | seq++; |
| 391 | } |
| 392 | |
| 393 | /* FFT yields more accurate tonal estimation (not phase sensitive) */ |
| 394 | spx_drft_forward(&psy->lookup,work); |
| 395 | |
| 396 | /* magnitudes */ |
| 397 | work[0]=scale_dB+todB(work[0]); |
| 398 | for(i=1;i<psy->n-1;i+=2){ |
| 399 | float temp = work[i]*work[i] + work[i+1]*work[i+1]; |
| 400 | work[(i+1)>>1] = scale_dB+.5f * todB(temp); |
| 401 | } |
| 402 | |
| 403 | /* derive a noise curve */ |
| 404 | _vp_noisemask(psy,work,curve); |
| 405 | #define SIDEL 12 |
| 406 | for (i=0;i<SIDEL;i++) |
| 407 | { |
| 408 | curve[i]=curve[SIDEL]; |
| 409 | } |
| 410 | #define SIDEH 12 |
| 411 | for (i=0;i<SIDEH;i++) |
| 412 | { |
| 413 | curve[(psy->n>>1)-i-1]=curve[(psy->n>>1)-SIDEH]; |
| 414 | } |
| 415 | for(i=0;i<((psy->n)>>1);i++) |
| 416 | curve[i] = fromdB(1.2*curve[i]+.2*i); |
| 417 | //curve[i] = fromdB(0.8*curve[i]+.35*i); |
| 418 | //curve[i] = fromdB(0.9*curve[i])*pow(1.0*i+45,1.3); |
| 419 | } |
| 420 | |
| 421 | /* Transform a masking curve (power spectrum) into a pole-zero filter */ |
| 422 | void curve_to_lpc(VorbisPsy *psy, float *curve, float *awk1, float *awk2, int ord) |
| 423 | { |
| 424 | int i; |
| 425 | float ac[psy->n]; |
| 426 | float tmp; |
| 427 | int len = psy->n >> 1; |
| 428 | for (i=0;i<2*len;i++) |
| 429 | ac[i] = 0; |
| 430 | for (i=1;i<len;i++) |
| 431 | ac[2*i-1] = curve[i]; |
| 432 | ac[0] = curve[0]; |
| 433 | ac[2*len-1] = curve[len-1]; |
| 434 | |
| 435 | spx_drft_backward(&psy->lookup, ac); |
| 436 | _spx_lpc(awk1, ac, ord); |
| 437 | tmp = 1.; |
| 438 | for (i=0;i<ord;i++) |
| 439 | { |
| 440 | tmp *= .99; |
| 441 | awk1[i] *= tmp; |
| 442 | } |
| 443 | #if 0 |
| 444 | for (i=0;i<ord;i++) |
| 445 | awk2[i] = 0; |
| 446 | #else |
| 447 | /* Use the second (awk2) filter to correct the first one */ |
| 448 | for (i=0;i<2*len;i++) |
| 449 | ac[i] = 0; |
| 450 | for (i=0;i<ord;i++) |
| 451 | ac[i+1] = awk1[i]; |
| 452 | ac[0] = 1; |
| 453 | spx_drft_forward(&psy->lookup, ac); |
| 454 | /* Compute (power) response of awk1 (all zero) */ |
| 455 | ac[0] *= ac[0]; |
| 456 | for (i=1;i<len;i++) |
| 457 | ac[i] = ac[2*i-1]*ac[2*i-1] + ac[2*i]*ac[2*i]; |
| 458 | ac[len] = ac[2*len-1]*ac[2*len-1]; |
| 459 | /* Compute correction required */ |
| 460 | for (i=0;i<len;i++) |
| 461 | curve[i] = 1. / (1e-6f+curve[i]*ac[i]); |
| 462 | |
| 463 | for (i=0;i<2*len;i++) |
| 464 | ac[i] = 0; |
| 465 | for (i=1;i<len;i++) |
| 466 | ac[2*i-1] = curve[i]; |
| 467 | ac[0] = curve[0]; |
| 468 | ac[2*len-1] = curve[len-1]; |
| 469 | |
| 470 | spx_drft_backward(&psy->lookup, ac); |
| 471 | _spx_lpc(awk2, ac, ord); |
| 472 | tmp = 1; |
| 473 | for (i=0;i<ord;i++) |
| 474 | { |
| 475 | tmp *= .99; |
| 476 | awk2[i] *= tmp; |
| 477 | } |
| 478 | #endif |
| 479 | } |
| 480 | |
| 481 | #if 0 |
| 482 | #include <stdio.h> |
| 483 | #include <math.h> |
| 484 | |
| 485 | #define ORDER 10 |
| 486 | #define CURVE_SIZE 24 |
| 487 | |
| 488 | int main() |
| 489 | { |
| 490 | int i; |
| 491 | float curve[CURVE_SIZE]; |
| 492 | float awk1[ORDER], awk2[ORDER]; |
| 493 | for (i=0;i<CURVE_SIZE;i++) |
| 494 | scanf("%f ", &curve[i]); |
| 495 | for (i=0;i<CURVE_SIZE;i++) |
| 496 | curve[i] = pow(10.f, .1*curve[i]); |
| 497 | curve_to_lpc(curve, CURVE_SIZE, awk1, awk2, ORDER); |
| 498 | for (i=0;i<ORDER;i++) |
| 499 | printf("%f ", awk1[i]); |
| 500 | printf ("\n"); |
| 501 | for (i=0;i<ORDER;i++) |
| 502 | printf("%f ", awk2[i]); |
| 503 | printf ("\n"); |
| 504 | return 0; |
| 505 | } |
| 506 | #endif |
| 507 | |
| 508 | #endif |