Added a few "restrict" keywords and changed some divisions to shifts
[opus.git] / libcelt / psy.c
1 /* (C) 2007 Jean-Marc Valin, CSIRO
2 */
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 #include "psy.h"
37 #include <math.h>
38 #include "os_support.h"
39 #include "arch.h"
40
41 /* The Vorbis freq<->Bark mapping */
42 #define toBARK(n)   (13.1f*atan(.00074f*(n))+2.24f*atan((n)*(n)*1.85e-8f)+1e-4f*(n))
43 #define fromBARK(z) (102.f*(z)-2.f*pow(z,2.f)+.4f*pow(z,3.f)+pow(1.46f,z)-1.f)
44
45 #ifndef STATIC_MODES
46 /* Psychoacoustic spreading function. The idea here is compute a first order 
47    recursive filter. The filter coefficient is frequency dependent and 
48    chosen such that we have a -10dB/Bark slope on the right side and a -25dB/Bark
49    slope on the left side. */
50 void psydecay_init(struct PsyDecay *decay, int len, celt_int32_t Fs)
51 {
52    int i;
53    celt_word16_t *decayR = (celt_word16_t*)celt_alloc(sizeof(celt_word16_t)*len);
54    /*decay->decayL = celt_alloc(sizeof(celt_word16_t)*len);*/
55    for (i=0;i<len;i++)
56    {
57       float f;
58       float deriv;
59       /* Real frequency (in Hz) */
60       f = Fs*i*(1/(2.f*len));
61       /* This is the derivative of the Vorbis freq->Bark function (see above) */
62       deriv = (8.288e-8 * f)/(3.4225e-16 *f*f*f*f + 1) +  .009694/(5.476e-7 *f*f + 1) + 1e-4;
63       /* Back to FFT bin units */
64       deriv *= Fs*(1/(2.f*len));
65       /* decay corresponding to -10dB/Bark */
66       decayR[i] = Q15ONE*pow(.1f, deriv);
67       /* decay corresponding to -25dB/Bark */
68       /*decay->decayL[i] = Q15ONE*pow(0.0031623f, deriv);*/
69       /*printf ("%f %f\n", decayL[i], decayR[i]);*/
70    }
71    decay->decayR = decayR;
72 }
73
74 void psydecay_clear(struct PsyDecay *decay)
75 {
76    celt_free((celt_word16_t *)decay->decayR);
77    /*celt_free(decay->decayL);*/
78 }
79 #endif
80
81 static void spreading_func(const struct PsyDecay *d, celt_word32_t *psd, celt_mask_t *mask, int len)
82 {
83    int i;
84    celt_word32_t mem;
85    /*for (i=0;i<len;i++) printf ("%f ", psd[i]);*/
86    /* Compute right slope (-10 dB/Bark) */
87    mem=psd[0];
88    for (i=0;i<len;i++)
89    {
90       mask[i] = MULT16_32_Q15(Q15ONE-d->decayR[i],psd[i]) + MULT16_32_Q15(d->decayR[i],mem);
91       if (mask[i]<1)
92          mask[i]=1;
93       mem = mask[i];
94    }
95    /* Compute left slope (-25 dB/Bark) */
96    mem=mask[len-1];
97    for (i=len-1;i>=0;i--)
98    {
99       /* Left side has around twice the slope as the right side, so we just
100          square the coef instead of storing two sets of decay coefs */
101       celt_word16_t decayL = MULT16_16_Q15(d->decayR[i], d->decayR[i]);
102       mask[i] = MULT16_32_Q15(Q15ONE-decayL,mask[i]) + MULT16_32_Q15(decayL,mem);
103       if (mask[i]<1)
104          mask[i]=1;
105       mem = mask[i];
106    }
107    /*for (i=0;i<len;i++) printf ("%f ", mask[i]); printf ("\n");*/
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_t 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_t *X, celt_mask_t *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, mask, N);
145 }
146
147 #if 0 /* Not needed for now, but will be useful in the future */
148 void compute_mdct_masking(const struct PsyDecay *decay, celt_word32_t *X, celt_mask_t *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       mask[i] = X[i]*X[i];
156    for (i=1;i<len-1;i++)
157       psd[i] = .5*mask[i] + .25*(mask[i-1]+mask[i+1]);
158    /*psd[0] = .5*mask[0]+.25*(mask[1]+mask[2]);*/
159    psd[0] = .5*mask[0]+.5*mask[1];
160    psd[len-1] = .5*(mask[len-1]+mask[len-2]);
161    /* TODO: Do tone masking */
162    /* Noise masking */
163    spreading_func(decay, psd, mask, len);
164    RESTORE_STACK;  
165 }
166 #endif