More work on variable frame size (getting rid of FRAMESIZE() )
[opus.git] / libcelt / psy.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /*
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include "psy.h"
38 #include <math.h>
39 #include "os_support.h"
40 #include "arch.h"
41 #include "stack_alloc.h"
42 #include "mathops.h"
43
44 /* The Vorbis freq<->Bark mapping */
45 #define toBARK(n)   (13.1f*atan(.00074f*(n))+2.24f*atan((n)*(n)*1.85e-8f)+1e-4f*(n))
46 #define fromBARK(z) (102.f*(z)-2.f*pow(z,2.f)+.4f*pow(z,3.f)+pow(1.46f,z)-1.f)
47
48 #ifndef STATIC_MODES
49 /* Psychoacoustic spreading function. The idea here is compute a first order 
50    recursive filter. The filter coefficient is frequency dependent and 
51    chosen such that we have a -10dB/Bark slope on the right side and a -25dB/Bark
52    slope on the left side. */
53 void psydecay_init(struct PsyDecay *decay, int len, celt_int32 Fs)
54 {
55    int i;
56    celt_word16 *decayR = (celt_word16*)celt_alloc(sizeof(celt_word16)*len);
57    decay->decayR = decayR;
58    if (decayR==NULL)
59      return;
60    for (i=0;i<len;i++)
61    {
62       float f;
63       float deriv;
64       /* Real frequency (in Hz) */
65       f = Fs*i*(1/(2.f*len));
66       /* This is the derivative of the Vorbis freq->Bark function (see above) */
67       deriv = (8.288e-8 * f)/(3.4225e-16 *f*f*f*f + 1) +  .009694/(5.476e-7 *f*f + 1) + 1e-4;
68       /* Back to FFT bin units */
69       deriv *= Fs*(1/(2.f*len));
70       /* decay corresponding to -10dB/Bark */
71       decayR[i] = Q15ONE*pow(.1f, deriv);
72       /* decay corresponding to -25dB/Bark */
73       /*decay->decayL[i] = Q15ONE*pow(0.0031623f, deriv);*/
74       /*printf ("%f %f\n", decayL[i], decayR[i]);*/
75    }
76 }
77
78 void psydecay_clear(struct PsyDecay *decay)
79 {
80    celt_free((celt_word16 *)decay->decayR);
81    /*celt_free(decay->decayL);*/
82 }
83 #endif
84
85 static void spreading_func(const struct PsyDecay *d, celt_word32 * restrict psd, int len)
86 {
87    int i;
88    celt_word32 mem;
89    /* Compute right slope (-10 dB/Bark) */
90    mem=psd[0];
91    for (i=0;i<len;i++)
92    {
93       /* psd = (1-decay)*psd + decay*mem */
94       psd[i] = EPSILON + psd[i] + MULT16_32_Q15(d->decayR[i],mem-psd[i]);
95       mem = psd[i];
96    }
97    /* Compute left slope (-25 dB/Bark) */
98    mem=psd[len-1];
99    for (i=len-1;i>=0;i--)
100    {
101       /* Left side has around twice the slope as the right side, so we just
102          square the coef instead of storing two sets of decay coefs */
103       celt_word16 decayL = MULT16_16_Q15(d->decayR[i], d->decayR[i]);
104       /* psd = (1-decay)*psd + decay*mem */
105       psd[i] = EPSILON + psd[i] + MULT16_32_Q15(decayL,mem-psd[i]);
106       mem = psd[i];
107    }
108 #if 0 /* Prints signal and mask energy per critical band */
109    for (i=0;i<25;i++)
110    {
111       int start,end;
112       int j;
113       celt_word32 Esig=0, Emask=0;
114       start = (int)floor(fromBARK((float)i)*(2*len)/Fs);
115       if (start<0)
116          start = 0;
117       end = (int)ceil(fromBARK((float)(i+1))*(2*len)/Fs);
118       if (end<=start)
119          end = start+1;
120       if (end>len-1)
121          end = len-1;
122       for (j=start;j<end;j++)
123       {
124          Esig += psd[j];
125          Emask += mask[j];
126       }
127       printf ("%f %f ", Esig, Emask);
128    }
129    printf ("\n");
130 #endif
131 }
132
133 /* Compute a marking threshold from the spectrum X. */
134 void compute_masking(const struct PsyDecay *decay, celt_word16 *X, celt_mask * restrict mask, int len)
135 {
136    int i;
137    int N;
138    N=len>>1;
139    mask[0] = MULT16_16(X[0], X[0]);
140    for (i=1;i<N;i++)
141       mask[i] = ADD32(MULT16_16(X[i*2], X[i*2]), MULT16_16(X[i*2+1], X[i*2+1]));
142    /* TODO: Do tone masking */
143    /* Noise masking */
144    spreading_func(decay, mask, N);
145 }
146
147 #ifdef EXP_PSY /* Not needed for now, but will be useful in the future */
148 void compute_mdct_masking(const struct PsyDecay *decay, celt_word32 *X, celt_word16 *tonality, celt_word16 *long_window, celt_mask *mask, int len)
149 {
150    int i;
151    VARDECL(float, psd);
152    SAVE_STACK;
153    ALLOC(psd, len, float);
154    for (i=0;i<len;i++)
155       psd[i] = X[i]*X[i]*tonality[i];
156    for (i=1;i<len-1;i++)
157       mask[i] = .5*psd[i] + .25*(psd[i-1]+psd[i+1]);
158    /*psd[0] = .5*mask[0]+.25*(mask[1]+mask[2]);*/
159    mask[0] = .5*psd[0]+.5*psd[1];
160    mask[len-1] = .5*(psd[len-1]+psd[len-2]);
161    /* TODO: Do tone masking */
162    /* Noise masking */
163    spreading_func(decay, mask, len);
164    RESTORE_STACK;  
165 }
166
167 void compute_tonality(const CELTMode *m, celt_word16 * restrict X, celt_word16 * mem, int len, celt_word16 *tonality, int mdct_size)
168 {
169    int i;
170    celt_word16 norm_1;
171    celt_word16 *mem2;
172    int N = len>>2;
173
174    mem2 = mem+2*N;
175    X[0] = 0;
176    X[1] = 0;
177    tonality[0] = 1;
178    for (i=1;i<N;i++)
179    {
180       celt_word16 re, im, re2, im2;
181       re = X[2*i];
182       im = X[2*i+1];
183       /* Normalise spectrum */
184       norm_1 = celt_rsqrt(.01+MAC16_16(MULT16_16(re,re), im,im));
185       re = MULT16_16(re, norm_1);
186       im = MULT16_16(im, norm_1);
187       /* Phase derivative */
188       re2 = re*mem[2*i] + im*mem[2*i+1];
189       im2 = im*mem[2*i] - re*mem[2*i+1];
190       mem[2*i] = re;
191       mem[2*i+1] = im;
192       /* Phase second derivative */
193       re = re2*mem2[2*i] + im2*mem2[2*i+1];
194       im = im2*mem2[2*i] - re2*mem2[2*i+1];
195       mem2[2*i] = re2;
196       mem2[2*i+1] = im2;
197       /*printf ("%f ", re);*/
198       X[2*i] = re;
199       X[2*i+1] = im;
200    }
201    /*printf ("\n");*/
202    for (i=0;i<mdct_size;i++)
203    {
204       tonality[i] = 1.0-X[2*i]*X[2*i]*X[2*i];
205       if (tonality[i]>1)
206          tonality[i] = 1;
207       if (tonality[i]<.02)
208          tonality[i]=.02;
209    }
210 }
211 #endif