float approximations for log2() and exp2()
[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    /*printf ("%f %d %f %f %f %f\n", x, integer, in.f, frac, log2(x), 1+integer+frac);*/
134    return 1+integer+frac;
135 }
136
137 /** Base-2 exponential approximation (2^x). */
138 static inline float celt_exp2(float x)
139 {
140    int integer;
141    float frac;
142    union {
143       float f;
144       celt_uint32_t i;
145    } res;
146    integer = floor(x);
147    frac = x-integer;
148    /* K0 = 1, K1 = log(2), K2 = 3-4*log(2), K3 = 3*log(2) - 2 */
149    res.f = 1.f + frac * (0.696147f + frac * (0.224411f + 0.079442f*frac));
150    res.i = (res.i + (integer<<23)) & 0x7fffffff;
151    /*printf ("%f %f %f %f\n", x, frac, exp2(x), res.f);*/
152    return res.f;
153 }
154
155 #else
156 #define celt_log2(x) (1.442695*log(x))
157 #define celt_exp2(x) (exp(0.69315*(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    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
291    return SHL16(i-13,8)+SHR16(frac,14-8);
292 }
293
294 /*
295  K0 = 1
296  K1 = log(2)
297  K2 = 3-4*log(2)
298  K3 = 3*log(2) - 2
299 */
300 #define D0 16384
301 #define D1 11356
302 #define D2 3726
303 #define D3 1301
304 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
305 static inline celt_word32_t celt_exp2(celt_word16_t x)
306 {
307    int integer;
308    celt_word16_t frac;
309    integer = SHR16(x,11);
310    if (integer>14)
311       return 0x7f000000;
312    else if (integer < -15)
313       return 0;
314    frac = SHL16(x-SHL16(integer,11),3);
315    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
316    return VSHR32(EXTEND32(frac), -integer-2);
317 }
318
319 /** Reciprocal approximation (Q15 input, Q16 output) */
320 static inline celt_word32_t celt_rcp(celt_word32_t x)
321 {
322    int i;
323    celt_word16_t n, frac;
324    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
325    celt_assert2(x>0, "celt_rcp() only defined for positive values");
326    i = celt_ilog2(x);
327    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
328    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
329                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
330    return VSHR32(EXTEND32(frac),i-16);
331 }
332
333 #define celt_div(a,b) MULT32_32_Q31((celt_word32_t)(a),celt_rcp(b))
334
335
336 #define M1 32767
337 #define M2 -21
338 #define M3 -11943
339 #define M4 4936
340
341 static inline celt_word16_t celt_atan01(celt_word16_t x)
342 {
343    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
344 }
345
346 #undef M1
347 #undef M2
348 #undef M3
349 #undef M4
350
351 static inline celt_word16_t celt_atan2p(celt_word16_t y, celt_word16_t x)
352 {
353    if (y < x)
354    {
355       celt_word32_t arg;
356       arg = celt_div(SHL32(EXTEND32(y),15),x);
357       if (arg >= 32767)
358          arg = 32767;
359       return SHR16(celt_atan01(EXTRACT16(arg)),1);
360    } else {
361       celt_word32_t arg;
362       arg = celt_div(SHL32(EXTEND32(x),15),y);
363       if (arg >= 32767)
364          arg = 32767;
365       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
366    }
367 }
368
369 #endif /* FIXED_POINT */
370
371
372 #endif /* MATHOPS_H */