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