Accuracy improvements to the fixed-point celt_rsqrt().
[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_word16 r;
194    celt_word16 r2;
195    celt_word16 y;
196    celt_word32 rt;
197    k = celt_ilog2(x)>>1;
198    x = VSHR32(x, (k-7)<<1);
199    /* Range of n is [-16384,32767] ([-0.5,1) in Q15). */
200    n = x-32768;
201    /* Get a rough initial guess for the root.
202       The optimal minimax quadratic approximation is
203        r = 1.4288615575712422-n*(0.8452316405039975+n*0.4519141640876117).
204       Coefficients here, and the final result r, are Q14.*/
205    r = ADD16(23410, MULT16_16_Q15(n, ADD16(-13848, MULT16_16_Q15(n, 7405))));
206    /* We want y = x*r*r-1 in Q15, but x is 32-bit Q16 and r is Q14.
207       We can compute the result from n and r using Q15 multiplies with some
208        adjustment, carefully done to avoid overflow.
209       Range of y is [-2014,2362]. */
210    r2 = MULT16_16_Q15(r, r);
211    y = SHL16(SUB16(ADD16(MULT16_16_Q15(r2, n), r2), 16384), 1);
212    /* Apply a 2nd-order Householder iteration: r' = r*(1+y*(-0.5+y*0.375)). */
213    rt = ADD16(r, MULT16_16_Q15(r, MULT16_16_Q15(y,
214               ADD16(-16384, MULT16_16_Q15(y, 12288)))));
215    /* rt is now the Q14 reciprocal square root of the Q16 x, with a maximum
216        error of 2.70970/16384 and a MSE of 0.587003/16384^2. */
217    /* Most of the error in this approximation comes from the following shift: */
218    rt = PSHR32(rt,k);
219    return rt;
220 }
221
222 /** Sqrt approximation (QX input, QX/2 output) */
223 static inline celt_word32 celt_sqrt(celt_word32 x)
224 {
225    int k;
226    celt_word16 n;
227    celt_word32 rt;
228    const celt_word16 C[5] = {23174, 11584, -3011, 1570, -557};
229    if (x==0)
230       return 0;
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 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
241     strictly positive */
242 static inline celt_word32 celt_psqrt(celt_word32 x)
243 {
244    int k;
245    celt_word16 n;
246    celt_word32 rt;
247    const celt_word16 C[5] = {23174, 11584, -3011, 1570, -557};
248    k = (celt_ilog2(x)>>1)-7;
249    x = VSHR32(x, (k<<1));
250    n = x-32768;
251    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
252               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
253    rt = VSHR32(rt,7-k);
254    return rt;
255 }
256
257 #define L1 32767
258 #define L2 -7651
259 #define L3 8277
260 #define L4 -626
261
262 static inline celt_word16 _celt_cos_pi_2(celt_word16 x)
263 {
264    celt_word16 x2;
265    
266    x2 = MULT16_16_P15(x,x);
267    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
268                                                                                 ))))))));
269 }
270
271 #undef L1
272 #undef L2
273 #undef L3
274 #undef L4
275
276 static inline celt_word16 celt_cos_norm(celt_word32 x)
277 {
278    x = x&0x0001ffff;
279    if (x>SHL32(EXTEND32(1), 16))
280       x = SUB32(SHL32(EXTEND32(1), 17),x);
281    if (x&0x00007fff)
282    {
283       if (x<SHL32(EXTEND32(1), 15))
284       {
285          return _celt_cos_pi_2(EXTRACT16(x));
286       } else {
287          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
288       }
289    } else {
290       if (x&0x0000ffff)
291          return 0;
292       else if (x&0x0001ffff)
293          return -32767;
294       else
295          return 32767;
296    }
297 }
298
299 static inline celt_word16 celt_log2(celt_word32 x)
300 {
301    int i;
302    celt_word16 n, frac;
303    /*-0.41446   0.96093  -0.33981   0.15600 */
304    const celt_word16 C[4] = {-6791, 7872, -1392, 319};
305    if (x==0)
306       return -32767;
307    i = celt_ilog2(x);
308    n = VSHR32(x,i-15)-32768-16384;
309    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
310    return SHL16(i-13,8)+SHR16(frac,14-8);
311 }
312
313 /*
314  K0 = 1
315  K1 = log(2)
316  K2 = 3-4*log(2)
317  K3 = 3*log(2) - 2
318 */
319 #define D0 16384
320 #define D1 11356
321 #define D2 3726
322 #define D3 1301
323 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
324 static inline celt_word32 celt_exp2(celt_word16 x)
325 {
326    int integer;
327    celt_word16 frac;
328    integer = SHR16(x,11);
329    if (integer>14)
330       return 0x7f000000;
331    else if (integer < -15)
332       return 0;
333    frac = SHL16(x-SHL16(integer,11),3);
334    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
335    return VSHR32(EXTEND32(frac), -integer-2);
336 }
337
338 /** Reciprocal approximation (Q15 input, Q16 output) */
339 static inline celt_word32 celt_rcp(celt_word32 x)
340 {
341    int i;
342    celt_word16 n, frac;
343    const celt_word16 C[5] = {21848, -7251, 2403, -934, 327};
344    celt_assert2(x>0, "celt_rcp() only defined for positive values");
345    i = celt_ilog2(x);
346    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
347    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
348                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
349    return VSHR32(EXTEND32(frac),i-16);
350 }
351
352 #define celt_div(a,b) MULT32_32_Q31((celt_word32)(a),celt_rcp(b))
353
354
355 #define M1 32767
356 #define M2 -21
357 #define M3 -11943
358 #define M4 4936
359
360 static inline celt_word16 celt_atan01(celt_word16 x)
361 {
362    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
363 }
364
365 #undef M1
366 #undef M2
367 #undef M3
368 #undef M4
369
370 static inline celt_word16 celt_atan2p(celt_word16 y, celt_word16 x)
371 {
372    if (y < x)
373    {
374       celt_word32 arg;
375       arg = celt_div(SHL32(EXTEND32(y),15),x);
376       if (arg >= 32767)
377          arg = 32767;
378       return SHR16(celt_atan01(EXTRACT16(arg)),1);
379    } else {
380       celt_word32 arg;
381       arg = celt_div(SHL32(EXTEND32(x),15),y);
382       if (arg >= 32767)
383          arg = 32767;
384       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
385    }
386 }
387
388 #endif /* FIXED_POINT */
389
390
391 #endif /* MATHOPS_H */