coarse probability model in static modes too
[opus.git] / libcelt / mdct.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2008 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 /* This is a simple MDCT implementation that uses a N/4 complex FFT
34    to do most of the work. It should be relatively straightforward to
35    plug in pretty much and FFT here.
36    
37    This replaces the Vorbis FFT (and uses the exact same API), which 
38    was a bit too messy and that was ending up duplicating code 
39    (might as well use the same FFT everywhere).
40    
41    The algorithm is similar to (and inspired from) Fabrice Bellard's
42    MDCT implementation in FFMPEG, but has differences in signs, ordering
43    and scaling in many places. 
44 */
45
46 #ifdef HAVE_CONFIG_H
47 #include "config.h"
48 #endif
49
50 #include "mdct.h"
51 #include "kiss_fft.h"
52 #include "_kiss_fft_guts.h"
53 #include <math.h>
54 #include "os_support.h"
55 #include "mathops.h"
56 #include "stack_alloc.h"
57
58 #ifndef M_PI
59 #define M_PI 3.141592653
60 #endif
61
62 void clt_mdct_init(mdct_lookup *l,int N, int maxshift)
63 {
64    int i;
65    int N2, N4;
66    kiss_twiddle_scalar *trig;
67    l->n = N;
68    N2 = N>>1;
69    N4 = N>>2;
70    l->maxshift = maxshift;
71    for (i=0;i<=maxshift;i++)
72    {
73       if (i==0)
74          l->kfft[i] = kiss_fft_alloc(N>>2>>i, 0, 0);
75       else
76          l->kfft[i] = kiss_fft_alloc_twiddles(N>>2>>i, 0, 0, l->kfft[0]);
77 #ifndef ENABLE_TI_DSPLIB55
78       if (l->kfft[i]==NULL)
79          return;
80 #endif
81    }
82    l->trig = trig = (kiss_twiddle_scalar*)celt_alloc((N4+1)*sizeof(kiss_twiddle_scalar));
83    if (l->trig==NULL)
84      return;
85    /* We have enough points that sine isn't necessary */
86 #if defined(FIXED_POINT)
87    for (i=0;i<=N4;i++)
88       trig[i] = TRIG_UPSCALE*celt_cos_norm(DIV32(ADD32(SHL32(EXTEND32(i),17),N2),N));
89 #else
90    for (i=0;i<=N4;i++)
91       trig[i] = cos(2*M_PI*i/N);
92 #endif
93 }
94
95 void clt_mdct_clear(mdct_lookup *l)
96 {
97    int i;
98    for (i=0;i<=l->maxshift;i++)
99       kiss_fft_free(l->kfft[i]);
100    celt_free((kiss_twiddle_scalar*)l->trig);
101 }
102
103 void clt_mdct_forward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out, const celt_word16 *window, int overlap, int shift)
104 {
105    int i;
106    int N, N2, N4;
107    kiss_twiddle_scalar sine;
108    VARDECL(kiss_fft_scalar, f);
109    SAVE_STACK;
110    N = l->n;
111    N >>= shift;
112    N2 = N>>1;
113    N4 = N>>2;
114    ALLOC(f, N2, kiss_fft_scalar);
115    /* sin(x) ~= x here */
116 #ifdef FIXED_POINT
117    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
118 #else
119    sine = 2*M_PI*(.125f)/N;
120 #endif
121
122    /* Consider the input to be composed of four blocks: [a, b, c, d] */
123    /* Window, shuffle, fold */
124    {
125       /* Temp pointers to make it really clear to the compiler what we're doing */
126       const kiss_fft_scalar * restrict xp1 = in+(overlap>>1);
127       const kiss_fft_scalar * restrict xp2 = in+N2-1+(overlap>>1);
128       kiss_fft_scalar * restrict yp = out;
129       const celt_word16 * restrict wp1 = window+(overlap>>1);
130       const celt_word16 * restrict wp2 = window+(overlap>>1)-1;
131       for(i=0;i<(overlap>>2);i++)
132       {
133          /* Real part arranged as -d-cR, Imag part arranged as -b+aR*/
134          *yp++ = MULT16_32_Q15(*wp2, xp1[N2]) + MULT16_32_Q15(*wp1,*xp2);
135          *yp++ = MULT16_32_Q15(*wp1, *xp1)    - MULT16_32_Q15(*wp2, xp2[-N2]);
136          xp1+=2;
137          xp2-=2;
138          wp1+=2;
139          wp2-=2;
140       }
141       wp1 = window;
142       wp2 = window+overlap-1;
143       for(;i<N4-(overlap>>2);i++)
144       {
145          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
146          *yp++ = *xp2;
147          *yp++ = *xp1;
148          xp1+=2;
149          xp2-=2;
150       }
151       for(;i<N4;i++)
152       {
153          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
154          *yp++ =  -MULT16_32_Q15(*wp1, xp1[-N2]) + MULT16_32_Q15(*wp2, *xp2);
155          *yp++ = MULT16_32_Q15(*wp2, *xp1)     + MULT16_32_Q15(*wp1, xp2[N2]);
156          xp1+=2;
157          xp2-=2;
158          wp1+=2;
159          wp2-=2;
160       }
161    }
162    /* Pre-rotation */
163    {
164       kiss_fft_scalar * restrict yp = out;
165       const kiss_fft_scalar *t = &l->trig[0];
166       for(i=0;i<N4;i++)
167       {
168          kiss_fft_scalar re, im, yr, yi;
169          re = yp[0];
170          im = yp[1];
171          yr = -S_MUL(re,t[i<<shift])  -  S_MUL(im,t[(N4-i)<<shift]);
172          yi = -S_MUL(im,t[i<<shift])  +  S_MUL(re,t[(N4-i)<<shift]);
173          /* works because the cos is nearly one */
174          *yp++ = yr + S_MUL(yi,sine);
175          *yp++ = yi - S_MUL(yr,sine);
176       }
177    }
178
179    /* N/4 complex FFT, down-scales by 4/N */
180    kiss_fft(l->kfft[shift], (kiss_fft_cpx *)out, (kiss_fft_cpx *)f);
181
182    /* Post-rotate */
183    {
184       /* Temp pointers to make it really clear to the compiler what we're doing */
185       const kiss_fft_scalar * restrict fp = f;
186       kiss_fft_scalar * restrict yp1 = out;
187       kiss_fft_scalar * restrict yp2 = out+N2-1;
188       const kiss_fft_scalar *t = &l->trig[0];
189       /* Temp pointers to make it really clear to the compiler what we're doing */
190       for(i=0;i<N4;i++)
191       {
192          kiss_fft_scalar yr, yi;
193          yr = S_MUL(fp[1],t[(N4-i)<<shift]) + S_MUL(fp[0],t[i<<shift]);
194          yi = S_MUL(fp[0],t[(N4-i)<<shift]) - S_MUL(fp[1],t[i<<shift]);
195          /* works because the cos is nearly one */
196          *yp1 = yr - S_MUL(yi,sine);
197          *yp2 = yi + S_MUL(yr,sine);;
198          fp += 2;
199          yp1 += 2;
200          yp2 -= 2;
201       }
202    }
203    RESTORE_STACK;
204 }
205
206
207 void clt_mdct_backward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out, const celt_word16 * restrict window, int overlap, int shift)
208 {
209    int i;
210    int N, N2, N4;
211    kiss_twiddle_scalar sine;
212    VARDECL(kiss_fft_scalar, f);
213    VARDECL(kiss_fft_scalar, f2);
214    SAVE_STACK;
215    N = l->n;
216    N >>= shift;
217    N2 = N>>1;
218    N4 = N>>2;
219    ALLOC(f, N2, kiss_fft_scalar);
220    ALLOC(f2, N2, kiss_fft_scalar);
221    /* sin(x) ~= x here */
222 #ifdef FIXED_POINT
223    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
224 #else
225    sine = 2*M_PI*(.125f)/N;
226 #endif
227    
228    /* Pre-rotate */
229    {
230       /* Temp pointers to make it really clear to the compiler what we're doing */
231       const kiss_fft_scalar * restrict xp1 = in;
232       const kiss_fft_scalar * restrict xp2 = in+N2-1;
233       kiss_fft_scalar * restrict yp = f2;
234       const kiss_fft_scalar *t = &l->trig[0];
235       for(i=0;i<N4;i++) 
236       {
237          kiss_fft_scalar yr, yi;
238          yr = -S_MUL(*xp2, t[i<<shift]) + S_MUL(*xp1,t[(N4-i)<<shift]);
239          yi =  -S_MUL(*xp2, t[(N4-i)<<shift]) - S_MUL(*xp1,t[i<<shift]);
240          /* works because the cos is nearly one */
241          *yp++ = yr - S_MUL(yi,sine);
242          *yp++ = yi + S_MUL(yr,sine);
243          xp1+=2;
244          xp2-=2;
245       }
246    }
247
248    /* Inverse N/4 complex FFT. This one should *not* downscale even in fixed-point */
249    kiss_ifft(l->kfft[shift], (kiss_fft_cpx *)f2, (kiss_fft_cpx *)f);
250    
251    /* Post-rotate */
252    {
253       kiss_fft_scalar * restrict fp = f;
254       const kiss_fft_scalar *t = &l->trig[0];
255
256       for(i=0;i<N4;i++)
257       {
258          kiss_fft_scalar re, im, yr, yi;
259          re = fp[0];
260          im = fp[1];
261          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
262          yr = S_MUL(re,t[i<<shift]) - S_MUL(im,t[(N4-i)<<shift]);
263          yi = S_MUL(im,t[i<<shift]) + S_MUL(re,t[(N4-i)<<shift]);
264          /* works because the cos is nearly one */
265          *fp++ = yr - S_MUL(yi,sine);
266          *fp++ = yi + S_MUL(yr,sine);
267       }
268    }
269    /* De-shuffle the components for the middle of the window only */
270    {
271       const kiss_fft_scalar * restrict fp1 = f;
272       const kiss_fft_scalar * restrict fp2 = f+N2-1;
273       kiss_fft_scalar * restrict yp = f2;
274       for(i = 0; i < N4; i++)
275       {
276          *yp++ =-*fp1;
277          *yp++ = *fp2;
278          fp1 += 2;
279          fp2 -= 2;
280       }
281    }
282
283    /* Mirror on both sides for TDAC */
284    {
285       kiss_fft_scalar * restrict fp1 = f2+N4-1;
286       kiss_fft_scalar * restrict xp1 = out+N2-1;
287       kiss_fft_scalar * restrict yp1 = out+N4-overlap/2;
288       const celt_word16 * restrict wp1 = window;
289       const celt_word16 * restrict wp2 = window+overlap-1;
290       for(i = 0; i< N4-overlap/2; i++)
291       {
292          *xp1 = *fp1;
293          xp1--;
294          fp1--;
295       }
296       for(; i < N4; i++)
297       {
298          kiss_fft_scalar x1;
299          x1 = *fp1--;
300          *yp1++ +=-MULT16_32_Q15(*wp1, x1);
301          *xp1-- += MULT16_32_Q15(*wp2, x1);
302          wp1++;
303          wp2--;
304       }
305    }
306    {
307       kiss_fft_scalar * restrict fp2 = f2+N4;
308       kiss_fft_scalar * restrict xp2 = out+N2;
309       kiss_fft_scalar * restrict yp2 = out+N-1-(N4-overlap/2);
310       const celt_word16 * restrict wp1 = window;
311       const celt_word16 * restrict wp2 = window+overlap-1;
312       for(i = 0; i< N4-overlap/2; i++)
313       {
314          *xp2 = *fp2;
315          xp2++;
316          fp2++;
317       }
318       for(; i < N4; i++)
319       {
320          kiss_fft_scalar x2;
321          x2 = *fp2++;
322          *yp2--  = MULT16_32_Q15(*wp1, x2);
323          *xp2++  = MULT16_32_Q15(*wp2, x2);
324          wp1++;
325          wp2--;
326       }
327    }
328    RESTORE_STACK;
329 }
330
331