Enhancements the fixed-point approximations of non-linear functions.
[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.5f;
134    frac = -0.41445418f + frac*(0.95909232f
135           + frac*(-0.33951290f + frac*0.16541097f));
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 = 0.99992522f + frac * (0.69583354f
154            + frac * (0.22606716f + 0.078024523f*frac));
155    res.i = (res.i + (integer<<23)) & 0x7fffffff;
156    return res.f;
157 }
158
159 #else
160 #define celt_log2(x) (1.442695040888963387*log(x))
161 #define celt_exp2(x) (exp(0.6931471805599453094*(x)))
162 #endif
163
164 #endif
165
166
167
168 #ifdef FIXED_POINT
169
170 #include "os_support.h"
171
172 #ifndef OVERRIDE_CELT_MAXABS16
173 static inline celt_word16 celt_maxabs16(celt_word16 *x, int len)
174 {
175    int i;
176    celt_word16 maxval = 0;
177    for (i=0;i<len;i++)
178       maxval = MAX16(maxval, ABS16(x[i]));
179    return maxval;
180 }
181 #endif
182
183 /** Integer log in base2. Defined for zero, but not for negative numbers */
184 static inline celt_int16 celt_zlog2(celt_word32 x)
185 {
186    return x <= 0 ? 0 : celt_ilog2(x);
187 }
188
189 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
190 static inline celt_word32 celt_rsqrt(celt_word32 x)
191 {
192    int k;
193    celt_word16 n;
194    celt_word16 r;
195    celt_word16 r2;
196    celt_word16 y;
197    celt_word32 rt;
198    k = celt_ilog2(x)>>1;
199    x = VSHR32(x, (k-7)<<1);
200    /* Range of n is [-16384,32767] ([-0.5,1) in Q15). */
201    n = x-32768;
202    /* Get a rough initial guess for the root.
203       The optimal minimax quadratic approximation (using relative error) is
204        r = 1.437799046117536+n*(-0.823394375837328+n*0.4096419668459485).
205       Coefficients here, and the final result r, are Q14.*/
206    r = ADD16(23557, MULT16_16_Q15(n, ADD16(-13490, MULT16_16_Q15(n, 6713))));
207    /* We want y = x*r*r-1 in Q15, but x is 32-bit Q16 and r is Q14.
208       We can compute the result from n and r using Q15 multiplies with some
209        adjustment, carefully done to avoid overflow.
210       Range of y is [-1564,1594]. */
211    r2 = MULT16_16_Q15(r, r);
212    y = SHL16(SUB16(ADD16(MULT16_16_Q15(r2, n), r2), 16384), 1);
213    /* Apply a 2nd-order Householder iteration: r += r*y*(y*0.375-0.5). */
214    rt = ADD16(r, MULT16_16_Q15(r, MULT16_16_Q15(y,
215               SUB16(MULT16_16_Q15(y, 12288), 16384))));
216    /* rt is now the Q14 reciprocal square root of the Q16 x, with a maximum
217        relative error of 1.04956E-4, a (relative) RMSE of 2.80979E-5, and a
218        peak absolute error of 2.26591/16384. */
219    /* Most of the error in this function comes from the following shift: */
220    rt = PSHR32(rt,k);
221    return rt;
222 }
223
224 /** Sqrt approximation (QX input, QX/2 output) */
225 static inline celt_word32 celt_sqrt(celt_word32 x)
226 {
227    int k;
228    celt_word16 n;
229    celt_word32 rt;
230    const celt_word16 C[5] = {23175, 11561, -3011, 1699, -664};
231    if (x==0)
232       return 0;
233    k = (celt_ilog2(x)>>1)-7;
234    x = VSHR32(x, (k<<1));
235    n = x-32768;
236    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
237               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
238    rt = VSHR32(rt,7-k);
239    return rt;
240 }
241
242 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
243     strictly positive */
244 static inline celt_word32 celt_psqrt(celt_word32 x)
245 {
246    int k;
247    celt_word16 n;
248    celt_word32 rt;
249    const celt_word16 C[5] = {23175, 11561, -3011, 1699, -664};
250    k = (celt_ilog2(x)>>1)-7;
251    x = VSHR32(x, (k<<1));
252    n = x-32768;
253    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
254               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
255    rt = VSHR32(rt,7-k);
256    return rt;
257 }
258
259 #define L1 32767
260 #define L2 -7651
261 #define L3 8277
262 #define L4 -626
263
264 static inline celt_word16 _celt_cos_pi_2(celt_word16 x)
265 {
266    celt_word16 x2;
267    
268    x2 = MULT16_16_P15(x,x);
269    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
270                                                                                 ))))))));
271 }
272
273 #undef L1
274 #undef L2
275 #undef L3
276 #undef L4
277
278 static inline celt_word16 celt_cos_norm(celt_word32 x)
279 {
280    x = x&0x0001ffff;
281    if (x>SHL32(EXTEND32(1), 16))
282       x = SUB32(SHL32(EXTEND32(1), 17),x);
283    if (x&0x00007fff)
284    {
285       if (x<SHL32(EXTEND32(1), 15))
286       {
287          return _celt_cos_pi_2(EXTRACT16(x));
288       } else {
289          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
290       }
291    } else {
292       if (x&0x0000ffff)
293          return 0;
294       else if (x&0x0001ffff)
295          return -32767;
296       else
297          return 32767;
298    }
299 }
300
301 static inline celt_word16 celt_log2(celt_word32 x)
302 {
303    int i;
304    celt_word16 n, frac;
305    /*-0.41446   0.96093  -0.33981   0.15600 */
306    /* -0.4144541824871411+32/16384, 0.9590923197873218, -0.3395129038105771,
307        0.16541096501128538 */
308    const celt_word16 C[4] = {-6758, 15715, -5563, 2708};
309    if (x==0)
310       return -32767;
311    i = celt_ilog2(x);
312    n = VSHR32(x,i-15)-32768-16384;
313    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], MULT16_16_Q15(n, (C[3])))))));
314    return SHL16(i-13,8)+SHR16(frac,14-8);
315 }
316
317 /*
318  K0 = 1
319  K1 = log(2)
320  K2 = 3-4*log(2)
321  K3 = 3*log(2) - 2
322 */
323 #define D0 16383
324 #define D1 22804
325 #define D2 14819
326 #define D3 10204
327 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
328 static inline celt_word32 celt_exp2(celt_word16 x)
329 {
330    int integer;
331    celt_word16 frac;
332    integer = SHR16(x,11);
333    if (integer>14)
334       return 0x7f000000;
335    else if (integer < -15)
336       return 0;
337    frac = SHL16(x-SHL16(integer,11),3);
338    frac = ADD16(D0, MULT16_16_Q15(frac, ADD16(D1, MULT16_16_Q15(frac, ADD16(D2 , MULT16_16_Q15(D3,frac))))));
339    return VSHR32(EXTEND32(frac), -integer-2);
340 }
341
342 /** Reciprocal approximation (Q15 input, Q16 output) */
343 static inline celt_word32 celt_rcp(celt_word32 x)
344 {
345    int i;
346    celt_word16 n;
347    celt_word16 r;
348    celt_assert2(x>0, "celt_rcp() only defined for positive values");
349    i = celt_ilog2(x);
350    /* n is Q15 with range [0,1). */
351    n = VSHR32(x,i-15)-32768;
352    /* Start with a linear approximation:
353       r = 1.8823529411764706-0.9411764705882353*n.
354       The coefficients and the result are Q14 in the range [15420,30840].*/
355    r = ADD16(30840, MULT16_16_Q15(-15420, n));
356    /* Perform two Newton iterations:
357       r -= r*((r*n)-1.Q15)
358          = r*((r*n)+(r-1.Q15)). */
359    r = SUB16(r, MULT16_16_Q15(r,
360              ADD16(MULT16_16_Q15(r, n), ADD16(r, -32768))));
361    /* We subtract an extra 1 in the second iteration to avoid overflow; it also
362        neatly compensates for truncation error in the rest of the process. */
363    r = SUB16(r, ADD16(1, MULT16_16_Q15(r,
364              ADD16(MULT16_16_Q15(r, n), ADD16(r, -32768)))));
365    /* r is now the Q15 solution to 2/(n+1), with a maximum relative error
366        of 7.05346E-5, a (relative) RMSE of 2.14418E-5, and a peak absolute
367        error of 1.24665/32768. */
368    return VSHR32(EXTEND32(r),i-16);
369 }
370
371 #define celt_div(a,b) MULT32_32_Q31((celt_word32)(a),celt_rcp(b))
372
373
374 #define M1 32767
375 #define M2 -21
376 #define M3 -11943
377 #define M4 4936
378
379 static inline celt_word16 celt_atan01(celt_word16 x)
380 {
381    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
382 }
383
384 #undef M1
385 #undef M2
386 #undef M3
387 #undef M4
388
389 static inline celt_word16 celt_atan2p(celt_word16 y, celt_word16 x)
390 {
391    if (y < x)
392    {
393       celt_word32 arg;
394       arg = celt_div(SHL32(EXTEND32(y),15),x);
395       if (arg >= 32767)
396          arg = 32767;
397       return SHR16(celt_atan01(EXTRACT16(arg)),1);
398    } else {
399       celt_word32 arg;
400       arg = celt_div(SHL32(EXTEND32(x),15),y);
401       if (arg >= 32767)
402          arg = 32767;
403       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
404    }
405 }
406
407 #endif /* FIXED_POINT */
408
409
410 #endif /* MATHOPS_H */