fixed-point version of the high-pass seems to work now.
[speexdsp.git] / libspeex / vorbis_psy.c
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 "misc.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