removed useless comments
[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 #ifdef FLOAT_APPROX
114
115 /* Note: This assumes radix-2 floating point with the exponent at bits 23..30 and an offset of 127
116          denorm, +/- inf and NaN are *not* handled */
117
118 /** Base-2 log approximation (log2(x)). */
119 static inline float celt_log2(float x)
120 {
121    int integer;
122    float frac;
123    union {
124       float f;
125       celt_uint32_t i;
126    } in;
127    in.f = x;
128    integer = (in.i>>23)-127;
129    in.i -= integer<<23;
130    frac = in.f - 1.5;
131    /* -0.41446   0.96093  -0.33981   0.15600 */
132    frac = -0.41446 + frac*(0.96093 + frac*(-0.33981 + frac*0.15600));
133    return 1+integer+frac;
134 }
135
136 /** Base-2 exponential approximation (2^x). */
137 static inline float celt_exp2(float x)
138 {
139    int integer;
140    float frac;
141    union {
142       float f;
143       celt_uint32_t i;
144    } res;
145    integer = floor(x);
146    if (integer < -50)
147       return 0;
148    frac = x-integer;
149    /* K0 = 1, K1 = log(2), K2 = 3-4*log(2), K3 = 3*log(2) - 2 */
150    res.f = 1.f + frac * (0.696147f + frac * (0.224411f + 0.079442f*frac));
151    res.i = (res.i + (integer<<23)) & 0x7fffffff;
152    return res.f;
153 }
154
155 #else
156 #define celt_log2(x) (1.442695040888963387*log(x))
157 #define celt_exp2(x) (exp(0.6931471805599453094*(x)))
158 #endif
159
160 #endif
161
162
163
164 #ifdef FIXED_POINT
165
166 #include "os_support.h"
167
168 #ifndef OVERRIDE_CELT_MAXABS16
169 static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
170 {
171    int i;
172    celt_word16_t maxval = 0;
173    for (i=0;i<len;i++)
174       maxval = MAX16(maxval, ABS16(x[i]));
175    return maxval;
176 }
177 #endif
178
179 /** Integer log in base2. Defined for zero, but not for negative numbers */
180 static inline celt_int16_t celt_zlog2(celt_word32_t x)
181 {
182    return x <= 0 ? 0 : celt_ilog2(x);
183 }
184
185 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
186 static inline celt_word32_t celt_rsqrt(celt_word32_t x)
187 {
188    int k;
189    celt_word16_t n;
190    celt_word32_t rt;
191    const celt_word16_t C[5] = {23126, -11496, 9812, -9097, 4100};
192    k = celt_ilog2(x)>>1;
193    x = VSHR32(x, (k-7)<<1);
194    /* Range of n is [-16384,32767] */
195    n = x-32768;
196    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
197               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
198    rt = VSHR32(rt,k);
199    return rt;
200 }
201
202 /** Sqrt approximation (QX input, QX/2 output) */
203 static inline celt_word32_t celt_sqrt(celt_word32_t x)
204 {
205    int k;
206    celt_word16_t n;
207    celt_word32_t rt;
208    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
209    if (x==0)
210       return 0;
211    k = (celt_ilog2(x)>>1)-7;
212    x = VSHR32(x, (k<<1));
213    n = x-32768;
214    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
215               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
216    rt = VSHR32(rt,7-k);
217    return rt;
218 }
219
220 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
221     strictly positive */
222 static inline celt_word32_t celt_psqrt(celt_word32_t x)
223 {
224    int k;
225    celt_word16_t n;
226    celt_word32_t rt;
227    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
228    k = (celt_ilog2(x)>>1)-7;
229    x = VSHR32(x, (k<<1));
230    n = x-32768;
231    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
232               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
233    rt = VSHR32(rt,7-k);
234    return rt;
235 }
236
237 #define L1 32767
238 #define L2 -7651
239 #define L3 8277
240 #define L4 -626
241
242 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
243 {
244    celt_word16_t x2;
245    
246    x2 = MULT16_16_P15(x,x);
247    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
248                                                                                 ))))))));
249 }
250
251 #undef L1
252 #undef L2
253 #undef L3
254 #undef L4
255
256 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
257 {
258    x = x&0x0001ffff;
259    if (x>SHL32(EXTEND32(1), 16))
260       x = SUB32(SHL32(EXTEND32(1), 17),x);
261    if (x&0x00007fff)
262    {
263       if (x<SHL32(EXTEND32(1), 15))
264       {
265          return _celt_cos_pi_2(EXTRACT16(x));
266       } else {
267          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
268       }
269    } else {
270       if (x&0x0000ffff)
271          return 0;
272       else if (x&0x0001ffff)
273          return -32767;
274       else
275          return 32767;
276    }
277 }
278
279 static inline celt_word16_t celt_log2(celt_word32_t x)
280 {
281    int i;
282    celt_word16_t n, frac;
283    /*-0.41446   0.96093  -0.33981   0.15600 */
284    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
285    if (x==0)
286       return -32767;
287    i = celt_ilog2(x);
288    n = VSHR32(x,i-15)-32768-16384;
289    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
290    return SHL16(i-13,8)+SHR16(frac,14-8);
291 }
292
293 /*
294  K0 = 1
295  K1 = log(2)
296  K2 = 3-4*log(2)
297  K3 = 3*log(2) - 2
298 */
299 #define D0 16384
300 #define D1 11356
301 #define D2 3726
302 #define D3 1301
303 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
304 static inline celt_word32_t celt_exp2(celt_word16_t x)
305 {
306    int integer;
307    celt_word16_t frac;
308    integer = SHR16(x,11);
309    if (integer>14)
310       return 0x7f000000;
311    else if (integer < -15)
312       return 0;
313    frac = SHL16(x-SHL16(integer,11),3);
314    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
315    return VSHR32(EXTEND32(frac), -integer-2);
316 }
317
318 /** Reciprocal approximation (Q15 input, Q16 output) */
319 static inline celt_word32_t celt_rcp(celt_word32_t x)
320 {
321    int i;
322    celt_word16_t n, frac;
323    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
324    celt_assert2(x>0, "celt_rcp() only defined for positive values");
325    i = celt_ilog2(x);
326    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
327    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
328                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
329    return VSHR32(EXTEND32(frac),i-16);
330 }
331
332 #define celt_div(a,b) MULT32_32_Q31((celt_word32_t)(a),celt_rcp(b))
333
334
335 #define M1 32767
336 #define M2 -21
337 #define M3 -11943
338 #define M4 4936
339
340 static inline celt_word16_t celt_atan01(celt_word16_t x)
341 {
342    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
343 }
344
345 #undef M1
346 #undef M2
347 #undef M3
348 #undef M4
349
350 static inline celt_word16_t celt_atan2p(celt_word16_t y, celt_word16_t x)
351 {
352    if (y < x)
353    {
354       celt_word32_t arg;
355       arg = celt_div(SHL32(EXTEND32(y),15),x);
356       if (arg >= 32767)
357          arg = 32767;
358       return SHR16(celt_atan01(EXTRACT16(arg)),1);
359    } else {
360       celt_word32_t arg;
361       arg = celt_div(SHL32(EXTEND32(x),15),y);
362       if (arg >= 32767)
363          arg = 32767;
364       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
365    }
366 }
367
368 #endif /* FIXED_POINT */
369
370
371 #endif /* MATHOPS_H */