Direct users at the opus repository.
[celt.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
46
47 #ifndef OVERRIDE_FIND_MAX16
48 static inline int find_max16(celt_word16 *x, int len)
49 {
50    celt_word16 max_corr=-VERY_LARGE16;
51    int i, id = 0;
52    for (i=0;i<len;i++)
53    {
54       if (x[i] > max_corr)
55       {
56          id = i;
57          max_corr = x[i];
58       }
59    }
60    return id;
61 }
62 #endif
63
64 #ifndef OVERRIDE_FIND_MAX32
65 static inline int find_max32(celt_word32 *x, int len)
66 {
67    celt_word32 max_corr=-VERY_LARGE32;
68    int i, id = 0;
69    for (i=0;i<len;i++)
70    {
71       if (x[i] > max_corr)
72       {
73          id = i;
74          max_corr = x[i];
75       }
76    }
77    return id;
78 }
79 #endif
80
81 /* Multiplies two 16-bit fractional values. Bit-exactness of this macro is important */
82 #define FRAC_MUL16(a,b) ((16384+((celt_int32)(celt_int16)(a)*(celt_int16)(b)))>>15)
83
84 /* This is a cos() approximation designed to be bit-exact on any platform. Bit exactness
85    with this approximation is important because it has an impact on the bit allocation */
86 static inline celt_int16 bitexact_cos(celt_int16 x)
87 {
88    celt_int32 tmp;
89    celt_int16 x2;
90    tmp = (4096+((celt_int32)(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_rsqrt_norm(x) (celt_rsqrt(x))
107 #define celt_acos acos
108 #define celt_exp exp
109 #define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
110 #define celt_atan atan
111 #define celt_rcp(x) (1.f/(x))
112 #define celt_div(a,b) ((a)/(b))
113 #define frac_div32(a,b) ((float)(a)/(b))
114
115 #ifdef FLOAT_APPROX
116
117 /* Note: This assumes radix-2 floating point with the exponent at bits 23..30 and an offset of 127
118          denorm, +/- inf and NaN are *not* handled */
119
120 /** Base-2 log approximation (log2(x)). */
121 static inline float celt_log2(float x)
122 {
123    int integer;
124    float frac;
125    union {
126       float f;
127       celt_uint32 i;
128    } in;
129    in.f = x;
130    integer = (in.i>>23)-127;
131    in.i -= integer<<23;
132    frac = in.f - 1.5f;
133    frac = -0.41445418f + frac*(0.95909232f
134           + frac*(-0.33951290f + frac*0.16541097f));
135    return 1+integer+frac;
136 }
137
138 /** Base-2 exponential approximation (2^x). */
139 static inline float celt_exp2(float x)
140 {
141    int integer;
142    float frac;
143    union {
144       float f;
145       celt_uint32 i;
146    } res;
147    integer = floor(x);
148    if (integer < -50)
149       return 0;
150    frac = x-integer;
151    /* K0 = 1, K1 = log(2), K2 = 3-4*log(2), K3 = 3*log(2) - 2 */
152    res.f = 0.99992522f + frac * (0.69583354f
153            + frac * (0.22606716f + 0.078024523f*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_ILOG2
172 /** Integer log in base2. Undefined for zero and negative numbers */
173 static inline celt_int16 celt_ilog2(celt_int32 x)
174 {
175    celt_assert2(x>0, "celt_ilog2() only defined for strictly positive numbers");
176    return EC_ILOG(x)-1;
177 }
178 #endif
179
180
181 #ifndef OVERRIDE_CELT_MAXABS16
182 static inline celt_word16 celt_maxabs16(celt_word16 *x, int len)
183 {
184    int i;
185    celt_word16 maxval = 0;
186    for (i=0;i<len;i++)
187       maxval = MAX16(maxval, ABS16(x[i]));
188    return maxval;
189 }
190 #endif
191
192 /** Integer log in base2. Defined for zero, but not for negative numbers */
193 static inline celt_int16 celt_zlog2(celt_word32 x)
194 {
195    return x <= 0 ? 0 : celt_ilog2(x);
196 }
197
198 /** Reciprocal sqrt approximation in the range [0.25,1) (Q16 in, Q14 out) */
199 static inline celt_word16 celt_rsqrt_norm(celt_word32 x)
200 {
201    celt_word16 n;
202    celt_word16 r;
203    celt_word16 r2;
204    celt_word16 y;
205    /* Range of n is [-16384,32767] ([-0.5,1) in Q15). */
206    n = x-32768;
207    /* Get a rough initial guess for the root.
208       The optimal minimax quadratic approximation (using relative error) is
209        r = 1.437799046117536+n*(-0.823394375837328+n*0.4096419668459485).
210       Coefficients here, and the final result r, are Q14.*/
211    r = ADD16(23557, MULT16_16_Q15(n, ADD16(-13490, MULT16_16_Q15(n, 6713))));
212    /* We want y = x*r*r-1 in Q15, but x is 32-bit Q16 and r is Q14.
213       We can compute the result from n and r using Q15 multiplies with some
214        adjustment, carefully done to avoid overflow.
215       Range of y is [-1564,1594]. */
216    r2 = MULT16_16_Q15(r, r);
217    y = SHL16(SUB16(ADD16(MULT16_16_Q15(r2, n), r2), 16384), 1);
218    /* Apply a 2nd-order Householder iteration: r += r*y*(y*0.375-0.5).
219       This yields the Q14 reciprocal square root of the Q16 x, with a maximum
220        relative error of 1.04956E-4, a (relative) RMSE of 2.80979E-5, and a
221        peak absolute error of 2.26591/16384. */
222    return ADD16(r, MULT16_16_Q15(r, MULT16_16_Q15(y,
223               SUB16(MULT16_16_Q15(y, 12288), 16384))));
224 }
225
226 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
227 static inline celt_word32 celt_rsqrt(celt_word32 x)
228 {
229    int k;
230    k = celt_ilog2(x)>>1;
231    x = VSHR32(x, (k-7)<<1);
232    return PSHR32(celt_rsqrt_norm(x), k);
233 }
234
235 /** Sqrt approximation (QX input, QX/2 output) */
236 static inline celt_word32 celt_sqrt(celt_word32 x)
237 {
238    int k;
239    celt_word16 n;
240    celt_word32 rt;
241    static const celt_word16 C[5] = {23175, 11561, -3011, 1699, -664};
242    if (x==0)
243       return 0;
244    k = (celt_ilog2(x)>>1)-7;
245    x = VSHR32(x, (k<<1));
246    n = x-32768;
247    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
248               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
249    rt = VSHR32(rt,7-k);
250    return rt;
251 }
252
253 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
254     strictly positive */
255 static inline celt_word32 celt_psqrt(celt_word32 x)
256 {
257    int k;
258    celt_word16 n;
259    celt_word32 rt;
260    static const celt_word16 C[5] = {23175, 11561, -3011, 1699, -664};
261    k = (celt_ilog2(x)>>1)-7;
262    x = VSHR32(x, (k<<1));
263    n = x-32768;
264    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
265               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
266    rt = VSHR32(rt,7-k);
267    return rt;
268 }
269
270 #define L1 32767
271 #define L2 -7651
272 #define L3 8277
273 #define L4 -626
274
275 static inline celt_word16 _celt_cos_pi_2(celt_word16 x)
276 {
277    celt_word16 x2;
278    
279    x2 = MULT16_16_P15(x,x);
280    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
281                                                                                 ))))))));
282 }
283
284 #undef L1
285 #undef L2
286 #undef L3
287 #undef L4
288
289 static inline celt_word16 celt_cos_norm(celt_word32 x)
290 {
291    x = x&0x0001ffff;
292    if (x>SHL32(EXTEND32(1), 16))
293       x = SUB32(SHL32(EXTEND32(1), 17),x);
294    if (x&0x00007fff)
295    {
296       if (x<SHL32(EXTEND32(1), 15))
297       {
298          return _celt_cos_pi_2(EXTRACT16(x));
299       } else {
300          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
301       }
302    } else {
303       if (x&0x0000ffff)
304          return 0;
305       else if (x&0x0001ffff)
306          return -32767;
307       else
308          return 32767;
309    }
310 }
311
312 static inline celt_word16 celt_log2(celt_word32 x)
313 {
314    int i;
315    celt_word16 n, frac;
316    /* -0.41509302963303146, 0.9609890551383969, -0.31836011537636605,
317        0.15530808010959576, -0.08556153059057618 */
318    static const celt_word16 C[5] = {-6801+(1<<13-DB_SHIFT), 15746, -5217, 2545, -1401};
319    if (x==0)
320       return -32767;
321    i = celt_ilog2(x);
322    n = VSHR32(x,i-15)-32768-16384;
323    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, C[4]))))))));
324    return SHL16(i-13,DB_SHIFT)+SHR16(frac,14-DB_SHIFT);
325 }
326
327 /*
328  K0 = 1
329  K1 = log(2)
330  K2 = 3-4*log(2)
331  K3 = 3*log(2) - 2
332 */
333 #define D0 16383
334 #define D1 22804
335 #define D2 14819
336 #define D3 10204
337 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
338 static inline celt_word32 celt_exp2(celt_word16 x)
339 {
340    int integer;
341    celt_word16 frac;
342    integer = SHR16(x,11);
343    if (integer>14)
344       return 0x7f000000;
345    else if (integer < -15)
346       return 0;
347    frac = SHL16(x-SHL16(integer,11),3);
348    frac = ADD16(D0, MULT16_16_Q15(frac, ADD16(D1, MULT16_16_Q15(frac, ADD16(D2 , MULT16_16_Q15(D3,frac))))));
349    return VSHR32(EXTEND32(frac), -integer-2);
350 }
351
352 /** Reciprocal approximation (Q15 input, Q16 output) */
353 static inline celt_word32 celt_rcp(celt_word32 x)
354 {
355    int i;
356    celt_word16 n;
357    celt_word16 r;
358    celt_assert2(x>0, "celt_rcp() only defined for positive values");
359    i = celt_ilog2(x);
360    /* n is Q15 with range [0,1). */
361    n = VSHR32(x,i-15)-32768;
362    /* Start with a linear approximation:
363       r = 1.8823529411764706-0.9411764705882353*n.
364       The coefficients and the result are Q14 in the range [15420,30840].*/
365    r = ADD16(30840, MULT16_16_Q15(-15420, n));
366    /* Perform two Newton iterations:
367       r -= r*((r*n)-1.Q15)
368          = r*((r*n)+(r-1.Q15)). */
369    r = SUB16(r, MULT16_16_Q15(r,
370              ADD16(MULT16_16_Q15(r, n), ADD16(r, -32768))));
371    /* We subtract an extra 1 in the second iteration to avoid overflow; it also
372        neatly compensates for truncation error in the rest of the process. */
373    r = SUB16(r, ADD16(1, MULT16_16_Q15(r,
374              ADD16(MULT16_16_Q15(r, n), ADD16(r, -32768)))));
375    /* r is now the Q15 solution to 2/(n+1), with a maximum relative error
376        of 7.05346E-5, a (relative) RMSE of 2.14418E-5, and a peak absolute
377        error of 1.24665/32768. */
378    return VSHR32(EXTEND32(r),i-16);
379 }
380
381 #define celt_div(a,b) MULT32_32_Q31((celt_word32)(a),celt_rcp(b))
382
383 static inline celt_word32 frac_div32(celt_word32 a, celt_word32 b)
384 {
385    celt_word16 rcp;
386    celt_word32 result, rem;
387    int shift = 30-celt_ilog2(b);
388    a = SHL32(a,shift);
389    b = SHL32(b,shift);
390
391    /* 16-bit reciprocal */
392    rcp = ROUND16(celt_rcp(ROUND16(b,16)),2);
393    result = SHL32(MULT16_32_Q15(rcp, a),1);
394    rem = a-MULT32_32_Q31(result, b);
395    result += SHL32(MULT16_32_Q15(rcp, rem),1);
396    return result;
397 }
398
399 #define M1 32767
400 #define M2 -21
401 #define M3 -11943
402 #define M4 4936
403
404 /* Atan approximation using a 4th order polynomial. Input is in Q15 format
405    and normalized by pi/4. Output is in Q15 format */
406 static inline celt_word16 celt_atan01(celt_word16 x)
407 {
408    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
409 }
410
411 #undef M1
412 #undef M2
413 #undef M3
414 #undef M4
415
416 /* atan2() approximation valid for positive input values */
417 static inline celt_word16 celt_atan2p(celt_word16 y, celt_word16 x)
418 {
419    if (y < x)
420    {
421       celt_word32 arg;
422       arg = celt_div(SHL32(EXTEND32(y),15),x);
423       if (arg >= 32767)
424          arg = 32767;
425       return SHR16(celt_atan01(EXTRACT16(arg)),1);
426    } else {
427       celt_word32 arg;
428       arg = celt_div(SHL32(EXTEND32(x),15),y);
429       if (arg >= 32767)
430          arg = 32767;
431       return 25736-SHR16(celt_atan01(EXTRACT16(arg)),1);
432    }
433 }
434
435 #endif /* FIXED_POINT */
436
437
438 #endif /* MATHOPS_H */