s/intra_decision()/loss_distortion()/
[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    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
20    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
21    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
22    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 /* This is a simple MDCT implementation that uses a N/4 complex FFT
30    to do most of the work. It should be relatively straightforward to
31    plug in pretty much and FFT here.
32    
33    This replaces the Vorbis FFT (and uses the exact same API), which 
34    was a bit too messy and that was ending up duplicating code 
35    (might as well use the same FFT everywhere).
36    
37    The algorithm is similar to (and inspired from) Fabrice Bellard's
38    MDCT implementation in FFMPEG, but has differences in signs, ordering
39    and scaling in many places. 
40 */
41
42 #ifndef SKIP_CONFIG_H
43 #ifdef HAVE_CONFIG_H
44 #include "config.h"
45 #endif
46 #endif
47
48 #include "mdct.h"
49 #include "kiss_fft.h"
50 #include "_kiss_fft_guts.h"
51 #include <math.h>
52 #include "os_support.h"
53 #include "mathops.h"
54 #include "stack_alloc.h"
55
56 #ifndef M_PI
57 #define M_PI 3.141592653
58 #endif
59
60 #ifdef CUSTOM_MODES
61
62 void clt_mdct_init(mdct_lookup *l,int N, int maxshift)
63 {
64    int i;
65    int N4, N2;
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] = (kiss_twiddle_scalar)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 #endif /* CUSTOM_MODES */
104
105 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)
106 {
107    int i;
108    int N, N2, N4;
109    kiss_twiddle_scalar sine;
110    VARDECL(kiss_fft_scalar, f);
111    SAVE_STACK;
112    N = l->n;
113    N >>= shift;
114    N2 = N>>1;
115    N4 = N>>2;
116    ALLOC(f, N2, kiss_fft_scalar);
117    /* sin(x) ~= x here */
118 #ifdef FIXED_POINT
119    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
120 #else
121    sine = (kiss_twiddle_scalar)2*M_PI*(.125f)/N;
122 #endif
123
124    /* Consider the input to be composed of four blocks: [a, b, c, d] */
125    /* Window, shuffle, fold */
126    {
127       /* Temp pointers to make it really clear to the compiler what we're doing */
128       const kiss_fft_scalar * restrict xp1 = in+(overlap>>1);
129       const kiss_fft_scalar * restrict xp2 = in+N2-1+(overlap>>1);
130       kiss_fft_scalar * restrict yp = out;
131       const celt_word16 * restrict wp1 = window+(overlap>>1);
132       const celt_word16 * restrict wp2 = window+(overlap>>1)-1;
133       for(i=0;i<(overlap>>2);i++)
134       {
135          /* Real part arranged as -d-cR, Imag part arranged as -b+aR*/
136          *yp++ = MULT16_32_Q15(*wp2, xp1[N2]) + MULT16_32_Q15(*wp1,*xp2);
137          *yp++ = MULT16_32_Q15(*wp1, *xp1)    - MULT16_32_Q15(*wp2, xp2[-N2]);
138          xp1+=2;
139          xp2-=2;
140          wp1+=2;
141          wp2-=2;
142       }
143       wp1 = window;
144       wp2 = window+overlap-1;
145       for(;i<N4-(overlap>>2);i++)
146       {
147          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
148          *yp++ = *xp2;
149          *yp++ = *xp1;
150          xp1+=2;
151          xp2-=2;
152       }
153       for(;i<N4;i++)
154       {
155          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
156          *yp++ =  -MULT16_32_Q15(*wp1, xp1[-N2]) + MULT16_32_Q15(*wp2, *xp2);
157          *yp++ = MULT16_32_Q15(*wp2, *xp1)     + MULT16_32_Q15(*wp1, xp2[N2]);
158          xp1+=2;
159          xp2-=2;
160          wp1+=2;
161          wp2-=2;
162       }
163    }
164    /* Pre-rotation */
165    {
166       kiss_fft_scalar * restrict yp = out;
167       const kiss_twiddle_scalar *t = &l->trig[0];
168       for(i=0;i<N4;i++)
169       {
170          kiss_fft_scalar re, im, yr, yi;
171          re = yp[0];
172          im = yp[1];
173          yr = -S_MUL(re,t[i<<shift])  -  S_MUL(im,t[(N4-i)<<shift]);
174          yi = -S_MUL(im,t[i<<shift])  +  S_MUL(re,t[(N4-i)<<shift]);
175          /* works because the cos is nearly one */
176          *yp++ = yr + S_MUL(yi,sine);
177          *yp++ = yi - S_MUL(yr,sine);
178       }
179    }
180
181    /* N/4 complex FFT, down-scales by 4/N */
182    kiss_fft(l->kfft[shift], (kiss_fft_cpx *)out, (kiss_fft_cpx *)f);
183
184    /* Post-rotate */
185    {
186       /* Temp pointers to make it really clear to the compiler what we're doing */
187       const kiss_fft_scalar * restrict fp = f;
188       kiss_fft_scalar * restrict yp1 = out;
189       kiss_fft_scalar * restrict yp2 = out+N2-1;
190       const kiss_twiddle_scalar *t = &l->trig[0];
191       /* Temp pointers to make it really clear to the compiler what we're doing */
192       for(i=0;i<N4;i++)
193       {
194          kiss_fft_scalar yr, yi;
195          yr = S_MUL(fp[1],t[(N4-i)<<shift]) + S_MUL(fp[0],t[i<<shift]);
196          yi = S_MUL(fp[0],t[(N4-i)<<shift]) - S_MUL(fp[1],t[i<<shift]);
197          /* works because the cos is nearly one */
198          *yp1 = yr - S_MUL(yi,sine);
199          *yp2 = yi + S_MUL(yr,sine);;
200          fp += 2;
201          yp1 += 2;
202          yp2 -= 2;
203       }
204    }
205    RESTORE_STACK;
206 }
207
208
209 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)
210 {
211    int i;
212    int N, N2, N4;
213    kiss_twiddle_scalar sine;
214    VARDECL(kiss_fft_scalar, f);
215    VARDECL(kiss_fft_scalar, f2);
216    SAVE_STACK;
217    N = l->n;
218    N >>= shift;
219    N2 = N>>1;
220    N4 = N>>2;
221    ALLOC(f, N2, kiss_fft_scalar);
222    ALLOC(f2, N2, kiss_fft_scalar);
223    /* sin(x) ~= x here */
224 #ifdef FIXED_POINT
225    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
226 #else
227    sine = (kiss_twiddle_scalar)2*M_PI*(.125f)/N;
228 #endif
229    
230    /* Pre-rotate */
231    {
232       /* Temp pointers to make it really clear to the compiler what we're doing */
233       const kiss_fft_scalar * restrict xp1 = in;
234       const kiss_fft_scalar * restrict xp2 = in+N2-1;
235       kiss_fft_scalar * restrict yp = f2;
236       const kiss_twiddle_scalar *t = &l->trig[0];
237       for(i=0;i<N4;i++) 
238       {
239          kiss_fft_scalar yr, yi;
240          yr = -S_MUL(*xp2, t[i<<shift]) + S_MUL(*xp1,t[(N4-i)<<shift]);
241          yi =  -S_MUL(*xp2, t[(N4-i)<<shift]) - S_MUL(*xp1,t[i<<shift]);
242          /* works because the cos is nearly one */
243          *yp++ = yr - S_MUL(yi,sine);
244          *yp++ = yi + S_MUL(yr,sine);
245          xp1+=2;
246          xp2-=2;
247       }
248    }
249
250    /* Inverse N/4 complex FFT. This one should *not* downscale even in fixed-point */
251    kiss_ifft(l->kfft[shift], (kiss_fft_cpx *)f2, (kiss_fft_cpx *)f);
252    
253    /* Post-rotate */
254    {
255       kiss_fft_scalar * restrict fp = f;
256       const kiss_twiddle_scalar *t = &l->trig[0];
257
258       for(i=0;i<N4;i++)
259       {
260          kiss_fft_scalar re, im, yr, yi;
261          re = fp[0];
262          im = fp[1];
263          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
264          yr = S_MUL(re,t[i<<shift]) - S_MUL(im,t[(N4-i)<<shift]);
265          yi = S_MUL(im,t[i<<shift]) + S_MUL(re,t[(N4-i)<<shift]);
266          /* works because the cos is nearly one */
267          *fp++ = yr - S_MUL(yi,sine);
268          *fp++ = yi + S_MUL(yr,sine);
269       }
270    }
271    /* De-shuffle the components for the middle of the window only */
272    {
273       const kiss_fft_scalar * restrict fp1 = f;
274       const kiss_fft_scalar * restrict fp2 = f+N2-1;
275       kiss_fft_scalar * restrict yp = f2;
276       for(i = 0; i < N4; i++)
277       {
278          *yp++ =-*fp1;
279          *yp++ = *fp2;
280          fp1 += 2;
281          fp2 -= 2;
282       }
283    }
284    out -= (N2-overlap)>>1;
285    /* Mirror on both sides for TDAC */
286    {
287       kiss_fft_scalar * restrict fp1 = f2+N4-1;
288       kiss_fft_scalar * restrict xp1 = out+N2-1;
289       kiss_fft_scalar * restrict yp1 = out+N4-overlap/2;
290       const celt_word16 * restrict wp1 = window;
291       const celt_word16 * restrict wp2 = window+overlap-1;
292       for(i = 0; i< N4-overlap/2; i++)
293       {
294          *xp1 = *fp1;
295          xp1--;
296          fp1--;
297       }
298       for(; i < N4; i++)
299       {
300          kiss_fft_scalar x1;
301          x1 = *fp1--;
302          *yp1++ +=-MULT16_32_Q15(*wp1, x1);
303          *xp1-- += MULT16_32_Q15(*wp2, x1);
304          wp1++;
305          wp2--;
306       }
307    }
308    {
309       kiss_fft_scalar * restrict fp2 = f2+N4;
310       kiss_fft_scalar * restrict xp2 = out+N2;
311       kiss_fft_scalar * restrict yp2 = out+N-1-(N4-overlap/2);
312       const celt_word16 * restrict wp1 = window;
313       const celt_word16 * restrict wp2 = window+overlap-1;
314       for(i = 0; i< N4-overlap/2; i++)
315       {
316          *xp2 = *fp2;
317          xp2++;
318          fp2++;
319       }
320       for(; i < N4; i++)
321       {
322          kiss_fft_scalar x2;
323          x2 = *fp2++;
324          *yp2--  = MULT16_32_Q15(*wp1, x2);
325          *xp2++  = MULT16_32_Q15(*wp2, x2);
326          wp1++;
327          wp2--;
328       }
329    }
330    RESTORE_STACK;
331 }
332
333