fixed-point: using TI intrinsic for celt_ilog2() if available.
[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
40 #ifndef FIXED_POINT
41
42 #define celt_sqrt sqrt
43 #define celt_acos acos
44 #define celt_exp exp
45 #define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
46 #define celt_atan atan
47 #define celt_rcp(x) (1.f/(x))
48
49 #endif
50
51
52
53 #ifdef FIXED_POINT
54
55 #include "entcode.h"
56 #include "os_support.h"
57
58 #ifndef OVERRIDE_CELT_ILOG2
59 /** Integer log in base2. Undefined for zero and negative numbers */
60 static inline celt_int16_t celt_ilog2(celt_word32_t x)
61 {
62    celt_assert2(x>0, "celt_ilog2() only defined for strictly positive numbers");
63    return EC_ILOG(x)-1;
64 }
65 #endif
66
67
68 /** Integer log in base2. Defined for zero, but not for negative numbers */
69 static inline celt_int16_t celt_zlog2(celt_word32_t x)
70 {
71    return EC_ILOG(x)-1;
72 }
73
74 /** Sqrt approximation (QX input, QX/2 output) */
75 static inline celt_word32_t celt_sqrt(celt_word32_t x)
76 {
77    int k;
78    celt_word16_t n;
79    celt_word32_t rt;
80    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
81    if (x==0)
82       return 0;
83    k = (celt_ilog2(x)>>1)-7;
84    x = VSHR32(x, (k<<1));
85    n = x-32768;
86    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
87               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
88    rt = VSHR32(rt,7-k);
89    return rt;
90 }
91
92
93 #define L1 32767
94 #define L2 -7651
95 #define L3 8277
96 #define L4 -626
97
98 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
99 {
100    celt_word16_t x2;
101    
102    x2 = MULT16_16_P15(x,x);
103    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
104                                                                                 ))))))));
105 }
106
107 #undef L1
108 #undef L2
109 #undef L3
110 #undef L4
111
112 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
113 {
114    x = x&0x0001ffff;
115    if (x>SHL32(EXTEND32(1), 16))
116       x = SUB32(SHL32(EXTEND32(1), 17),x);
117    if (x&0x00007fff)
118    {
119       if (x<SHL32(EXTEND32(1), 15))
120       {
121          return _celt_cos_pi_2(EXTRACT16(x));
122       } else {
123          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
124       }
125    } else {
126       if (x&0x0000ffff)
127          return 0;
128       else if (x&0x0001ffff)
129          return -32767;
130       else
131          return 32767;
132    }
133 }
134
135 static inline celt_word16_t celt_log2(celt_word32_t x)
136 {
137    int i;
138    celt_word16_t n, frac;
139    /*-0.41446   0.96093  -0.33981   0.15600 */
140    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
141    if (x==0)
142       return -32767;
143    i = celt_ilog2(x);
144    n = VSHR32(x,i-15)-32768-16384;
145    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
146    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
147    return SHL16(i-13,8)+SHR16(frac,14-8);
148 }
149
150 /*
151  K0 = 1
152  K1 = log(2)
153  K2 = 3-4*log(2)
154  K3 = 3*log(2) - 2
155 */
156 #define D0 16384
157 #define D1 11356
158 #define D2 3726
159 #define D3 1301
160 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
161 static inline celt_word32_t celt_exp2(celt_word16_t x)
162 {
163    int integer;
164    celt_word16_t frac;
165    integer = SHR16(x,11);
166    if (integer>14)
167       return 0x7fffffff;
168    else if (integer < -15)
169       return 0;
170    frac = SHL16(x-SHL16(integer,11),3);
171    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
172    return VSHR32(EXTEND32(frac), -integer-2);
173 }
174
175 /** Reciprocal approximation (Q15 input, Q16 output) */
176 static inline celt_word32_t celt_rcp(celt_word32_t x)
177 {
178    int i, neg=0;
179    celt_word16_t n, frac;
180    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
181    if (x<0)
182    {
183       neg = 1;
184       x = NEG16(x);
185    }
186    i = celt_ilog2(x);
187    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
188    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
189                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
190    if (neg)
191       frac = -frac;
192    return VSHR32(EXTEND32(frac),i-16);
193 }
194
195 #endif /* FIXED_POINT */
196
197
198 #endif /* MATHOPS_H */