Minor stuff: preventing float underflow in celt_exp2(), preventing the use of
[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    if (integer < -50)
148       return 0;
149    frac = x-integer;
150    /* K0 = 1, K1 = log(2), K2 = 3-4*log(2), K3 = 3*log(2) - 2 */
151    res.f = 1.f + frac * (0.696147f + frac * (0.224411f + 0.079442f*frac));
152    res.i = (res.i + (integer<<23)) & 0x7fffffff;
153    /*printf ("%f %f %f %f\n", x, frac, exp2(x), res.f);*/
154    return res.f;
155 }
156
157 #else
158 #define celt_log2(x) (1.442695*log(x))
159 #define celt_exp2(x) (exp(0.69315*(x)))
160 #endif
161
162 #endif
163
164
165
166 #ifdef FIXED_POINT
167
168 #include "os_support.h"
169
170 #ifndef OVERRIDE_CELT_MAXABS16
171 static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
172 {
173    int i;
174    celt_word16_t maxval = 0;
175    for (i=0;i<len;i++)
176       maxval = MAX16(maxval, ABS16(x[i]));
177    return maxval;
178 }
179 #endif
180
181 /** Integer log in base2. Defined for zero, but not for negative numbers */
182 static inline celt_int16_t celt_zlog2(celt_word32_t x)
183 {
184    return x <= 0 ? 0 : celt_ilog2(x);
185 }
186
187 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
188 static inline celt_word32_t celt_rsqrt(celt_word32_t x)
189 {
190    int k;
191    celt_word16_t n;
192    celt_word32_t rt;
193    const celt_word16_t C[5] = {23126, -11496, 9812, -9097, 4100};
194    k = celt_ilog2(x)>>1;
195    x = VSHR32(x, (k-7)<<1);
196    /* Range of n is [-16384,32767] */
197    n = x-32768;
198    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
199               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
200    rt = VSHR32(rt,k);
201    return rt;
202 }
203
204 /** Sqrt approximation (QX input, QX/2 output) */
205 static inline celt_word32_t celt_sqrt(celt_word32_t x)
206 {
207    int k;
208    celt_word16_t n;
209    celt_word32_t rt;
210    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
211    if (x==0)
212       return 0;
213    k = (celt_ilog2(x)>>1)-7;
214    x = VSHR32(x, (k<<1));
215    n = x-32768;
216    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
217               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
218    rt = VSHR32(rt,7-k);
219    return rt;
220 }
221
222 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
223     strictly positive */
224 static inline celt_word32_t celt_psqrt(celt_word32_t x)
225 {
226    int k;
227    celt_word16_t n;
228    celt_word32_t rt;
229    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
230    k = (celt_ilog2(x)>>1)-7;
231    x = VSHR32(x, (k<<1));
232    n = x-32768;
233    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
234               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
235    rt = VSHR32(rt,7-k);
236    return rt;
237 }
238
239 #define L1 32767
240 #define L2 -7651
241 #define L3 8277
242 #define L4 -626
243
244 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
245 {
246    celt_word16_t x2;
247    
248    x2 = MULT16_16_P15(x,x);
249    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
250                                                                                 ))))))));
251 }
252
253 #undef L1
254 #undef L2
255 #undef L3
256 #undef L4
257
258 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
259 {
260    x = x&0x0001ffff;
261    if (x>SHL32(EXTEND32(1), 16))
262       x = SUB32(SHL32(EXTEND32(1), 17),x);
263    if (x&0x00007fff)
264    {
265       if (x<SHL32(EXTEND32(1), 15))
266       {
267          return _celt_cos_pi_2(EXTRACT16(x));
268       } else {
269          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
270       }
271    } else {
272       if (x&0x0000ffff)
273          return 0;
274       else if (x&0x0001ffff)
275          return -32767;
276       else
277          return 32767;
278    }
279 }
280
281 static inline celt_word16_t celt_log2(celt_word32_t x)
282 {
283    int i;
284    celt_word16_t n, frac;
285    /*-0.41446   0.96093  -0.33981   0.15600 */
286    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
287    if (x==0)
288       return -32767;
289    i = celt_ilog2(x);
290    n = VSHR32(x,i-15)-32768-16384;
291    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
292    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
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_t celt_exp2(celt_word16_t x)
308 {
309    int integer;
310    celt_word16_t 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_t celt_rcp(celt_word32_t x)
323 {
324    int i;
325    celt_word16_t n, frac;
326    const celt_word16_t 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_t)(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_t celt_atan01(celt_word16_t 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_t celt_atan2p(celt_word16_t y, celt_word16_t x)
354 {
355    if (y < x)
356    {
357       celt_word32_t 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_t 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 */