Squashed commit of the following:
[opus.git] / libcelt / mathops.h
1 /* Copyright (C) 2002-2008 Jean-Marc Valin */
2 /**
3    @file mathops.h
4    @brief Various math functions
5 */
6 /*
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions
9    are met:
10    
11    - Redistributions of source code must retain the above copyright
12    notice, this list of conditions and the following disclaimer.
13    
14    - Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17    
18    - Neither the name of the Xiph.org Foundation nor the names of its
19    contributors may be used to endorse or promote products derived from
20    this software without specific prior written permission.
21    
22    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
25    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
26    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
27    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
28    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
29    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
30    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
31    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #ifndef MATHOPS_H
36 #define MATHOPS_H
37
38 #include "arch.h"
39 #include "entcode.h"
40 #include "os_support.h"
41
42 #ifndef OVERRIDE_CELT_ILOG2
43 /** Integer log in base2. Undefined for zero and negative numbers */
44 static inline celt_int16_t celt_ilog2(celt_word32_t x)
45 {
46    celt_assert2(x>0, "celt_ilog2() only defined for strictly positive numbers");
47    return EC_ILOG(x)-1;
48 }
49 #endif
50
51 #ifndef OVERRIDE_FIND_MAX16
52 static inline int find_max16(celt_word16_t *x, int len)
53 {
54    celt_word16_t max_corr=-VERY_LARGE16;
55    int i, id = 0;
56    for (i=0;i<len;i++)
57    {
58       if (x[i] > max_corr)
59       {
60          id = i;
61          max_corr = x[i];
62       }
63    }
64    return id;
65 }
66 #endif
67
68 #ifndef OVERRIDE_FIND_MAX32
69 static inline int find_max32(celt_word32_t *x, int len)
70 {
71    celt_word32_t max_corr=-VERY_LARGE32;
72    int i, id = 0;
73    for (i=0;i<len;i++)
74    {
75       if (x[i] > max_corr)
76       {
77          id = i;
78          max_corr = x[i];
79       }
80    }
81    return id;
82 }
83 #endif
84
85 #define FRAC_MUL16(a,b) ((16384+((celt_int32_t)(celt_int16_t)(a)*(celt_int16_t)(b)))>>15)
86 static inline celt_int16_t bitexact_cos(celt_int16_t x)
87 {
88    celt_int32_t tmp;
89    celt_int16_t x2;
90    tmp = (4096+((celt_int32_t)(x)*(x)))>>13;
91    if (tmp > 32767)
92       tmp = 32767;
93    x2 = tmp;
94    x2 = (32767-x2) + FRAC_MUL16(x2, (-7651 + FRAC_MUL16(x2, (8277 + FRAC_MUL16(-626, x2)))));
95    if (x2 > 32766)
96       x2 = 32766;
97    return 1+x2;
98 }
99
100
101 #ifndef FIXED_POINT
102
103 #define celt_sqrt(x) ((float)sqrt(x))
104 #define celt_psqrt(x) ((float)sqrt(x))
105 #define celt_rsqrt(x) (1.f/celt_sqrt(x))
106 #define celt_acos acos
107 #define celt_exp exp
108 #define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
109 #define celt_atan atan
110 #define celt_rcp(x) (1.f/(x))
111 #define celt_div(a,b) ((a)/(b))
112
113 #endif
114
115
116
117 #ifdef FIXED_POINT
118
119 #include "os_support.h"
120
121 #ifndef OVERRIDE_CELT_MAXABS16
122 static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
123 {
124    int i;
125    celt_word16_t maxval = 0;
126    for (i=0;i<len;i++)
127       maxval = MAX16(maxval, ABS16(x[i]));
128    return maxval;
129 }
130 #endif
131
132 /** Integer log in base2. Defined for zero, but not for negative numbers */
133 static inline celt_int16_t celt_zlog2(celt_word32_t x)
134 {
135    return x <= 0 ? 0 : celt_ilog2(x);
136 }
137
138 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
139 static inline celt_word32_t celt_rsqrt(celt_word32_t x)
140 {
141    int k;
142    celt_word16_t n;
143    celt_word32_t rt;
144    const celt_word16_t C[5] = {23126, -11496, 9812, -9097, 4100};
145    k = celt_ilog2(x)>>1;
146    x = VSHR32(x, (k-7)<<1);
147    /* Range of n is [-16384,32767] */
148    n = x-32768;
149    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
150               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
151    rt = VSHR32(rt,k);
152    return rt;
153 }
154
155 /** Sqrt approximation (QX input, QX/2 output) */
156 static inline celt_word32_t celt_sqrt(celt_word32_t x)
157 {
158    int k;
159    celt_word16_t n;
160    celt_word32_t rt;
161    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
162    if (x==0)
163       return 0;
164    k = (celt_ilog2(x)>>1)-7;
165    x = VSHR32(x, (k<<1));
166    n = x-32768;
167    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
168               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
169    rt = VSHR32(rt,7-k);
170    return rt;
171 }
172
173 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
174     strictly positive */
175 static inline celt_word32_t celt_psqrt(celt_word32_t x)
176 {
177    int k;
178    celt_word16_t n;
179    celt_word32_t rt;
180    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
181    k = (celt_ilog2(x)>>1)-7;
182    x = VSHR32(x, (k<<1));
183    n = x-32768;
184    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
185               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
186    rt = VSHR32(rt,7-k);
187    return rt;
188 }
189
190 #define L1 32767
191 #define L2 -7651
192 #define L3 8277
193 #define L4 -626
194
195 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
196 {
197    celt_word16_t x2;
198    
199    x2 = MULT16_16_P15(x,x);
200    return ADD16(1,MIN16(32766,ADD32(SUB16(L1,x2), MULT16_16_P15(x2, ADD32(L2, MULT16_16_P15(x2, ADD32(L3, MULT16_16_P15(L4, x2
201                                                                                 ))))))));
202 }
203
204 #undef L1
205 #undef L2
206 #undef L3
207 #undef L4
208
209 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
210 {
211    x = x&0x0001ffff;
212    if (x>SHL32(EXTEND32(1), 16))
213       x = SUB32(SHL32(EXTEND32(1), 17),x);
214    if (x&0x00007fff)
215    {
216       if (x<SHL32(EXTEND32(1), 15))
217       {
218          return _celt_cos_pi_2(EXTRACT16(x));
219       } else {
220          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
221       }
222    } else {
223       if (x&0x0000ffff)
224          return 0;
225       else if (x&0x0001ffff)
226          return -32767;
227       else
228          return 32767;
229    }
230 }
231
232 static inline celt_word16_t celt_log2(celt_word32_t x)
233 {
234    int i;
235    celt_word16_t n, frac;
236    /*-0.41446   0.96093  -0.33981   0.15600 */
237    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
238    if (x==0)
239       return -32767;
240    i = celt_ilog2(x);
241    n = VSHR32(x,i-15)-32768-16384;
242    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
243    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
244    return SHL16(i-13,8)+SHR16(frac,14-8);
245 }
246
247 /*
248  K0 = 1
249  K1 = log(2)
250  K2 = 3-4*log(2)
251  K3 = 3*log(2) - 2
252 */
253 #define D0 16384
254 #define D1 11356
255 #define D2 3726
256 #define D3 1301
257 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
258 static inline celt_word32_t celt_exp2(celt_word16_t x)
259 {
260    int integer;
261    celt_word16_t frac;
262    integer = SHR16(x,11);
263    if (integer>14)
264       return 0x7f000000;
265    else if (integer < -15)
266       return 0;
267    frac = SHL16(x-SHL16(integer,11),3);
268    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
269    return VSHR32(EXTEND32(frac), -integer-2);
270 }
271
272 /** Reciprocal approximation (Q15 input, Q16 output) */
273 static inline celt_word32_t celt_rcp(celt_word32_t x)
274 {
275    int i;
276    celt_word16_t n, frac;
277    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
278    celt_assert2(x>0, "celt_rcp() only defined for positive values");
279    i = celt_ilog2(x);
280    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
281    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
282                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
283    return VSHR32(EXTEND32(frac),i-16);
284 }
285
286 #define celt_div(a,b) MULT32_32_Q31((celt_word32_t)(a),celt_rcp(b))
287
288
289 #define M1 32767
290 #define M2 -21
291 #define M3 -11943
292 #define M4 4936
293
294 static inline celt_word16_t celt_atan01(celt_word16_t x)
295 {
296    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
297 }
298
299 #undef M1
300 #undef M2
301 #undef M3
302 #undef M4
303
304 static inline celt_word16_t celt_atan2p(celt_word16_t y, celt_word16_t x)
305 {
306    if (y < x)
307    {
308       celt_word32_t arg;
309       arg = celt_div(SHL32(EXTEND32(y),15),x);
310       if (arg >= 32767)
311          arg = 32767;
312       return SHR16(celt_atan01(EXTRACT16(arg)),1);
313    } else {
314       celt_word32_t arg;
315       arg = celt_div(SHL32(EXTEND32(x),15),y);
316       if (arg >= 32767)
317          arg = 32767;
318       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
319    }
320 }
321
322 #endif /* FIXED_POINT */
323
324
325 #endif /* MATHOPS_H */