encoder pre-emphasis now in 16-bits
[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
86 #ifndef FIXED_POINT
87
88 #define celt_sqrt(x) ((float)sqrt(x))
89 #define celt_psqrt(x) ((float)sqrt(x))
90 #define celt_rsqrt(x) (1.f/celt_sqrt(x))
91 #define celt_acos acos
92 #define celt_exp exp
93 #define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
94 #define celt_atan atan
95 #define celt_rcp(x) (1.f/(x))
96 #define celt_div(a,b) ((a)/(b))
97
98 #endif
99
100
101
102 #ifdef FIXED_POINT
103
104 #include "os_support.h"
105
106 #ifndef OVERRIDE_CELT_MAXABS16
107 static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
108 {
109    int i;
110    celt_word16_t maxval = 0;
111    for (i=0;i<len;i++)
112       maxval = MAX16(maxval, ABS16(x[i]));
113    return maxval;
114 }
115 #endif
116
117 /** Integer log in base2. Defined for zero, but not for negative numbers */
118 static inline celt_int16_t celt_zlog2(celt_word32_t x)
119 {
120    return x <= 0 ? 0 : celt_ilog2(x);
121 }
122
123 /** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
124 static inline celt_word32_t celt_rsqrt(celt_word32_t x)
125 {
126    int k;
127    celt_word16_t n;
128    celt_word32_t rt;
129    const celt_word16_t C[5] = {23126, -11496, 9812, -9097, 4100};
130    k = celt_ilog2(x)>>1;
131    x = VSHR32(x, (k-7)<<1);
132    /* Range of n is [-16384,32767] */
133    n = x-32768;
134    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
135               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
136    rt = VSHR32(rt,k);
137    return rt;
138 }
139
140 /** Sqrt approximation (QX input, QX/2 output) */
141 static inline celt_word32_t celt_sqrt(celt_word32_t x)
142 {
143    int k;
144    celt_word16_t n;
145    celt_word32_t rt;
146    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
147    if (x==0)
148       return 0;
149    k = (celt_ilog2(x)>>1)-7;
150    x = VSHR32(x, (k<<1));
151    n = x-32768;
152    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
153               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
154    rt = VSHR32(rt,7-k);
155    return rt;
156 }
157
158 /** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
159     strictly positive */
160 static inline celt_word32_t celt_psqrt(celt_word32_t x)
161 {
162    int k;
163    celt_word16_t n;
164    celt_word32_t rt;
165    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
166    k = (celt_ilog2(x)>>1)-7;
167    x = VSHR32(x, (k<<1));
168    n = x-32768;
169    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
170               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
171    rt = VSHR32(rt,7-k);
172    return rt;
173 }
174
175 #define L1 32767
176 #define L2 -7651
177 #define L3 8277
178 #define L4 -626
179
180 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
181 {
182    celt_word16_t x2;
183    
184    x2 = MULT16_16_P15(x,x);
185    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
186                                                                                 ))))))));
187 }
188
189 #undef L1
190 #undef L2
191 #undef L3
192 #undef L4
193
194 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
195 {
196    x = x&0x0001ffff;
197    if (x>SHL32(EXTEND32(1), 16))
198       x = SUB32(SHL32(EXTEND32(1), 17),x);
199    if (x&0x00007fff)
200    {
201       if (x<SHL32(EXTEND32(1), 15))
202       {
203          return _celt_cos_pi_2(EXTRACT16(x));
204       } else {
205          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
206       }
207    } else {
208       if (x&0x0000ffff)
209          return 0;
210       else if (x&0x0001ffff)
211          return -32767;
212       else
213          return 32767;
214    }
215 }
216
217 static inline celt_word16_t celt_log2(celt_word32_t x)
218 {
219    int i;
220    celt_word16_t n, frac;
221    /*-0.41446   0.96093  -0.33981   0.15600 */
222    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
223    if (x==0)
224       return -32767;
225    i = celt_ilog2(x);
226    n = VSHR32(x,i-15)-32768-16384;
227    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
228    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
229    return SHL16(i-13,8)+SHR16(frac,14-8);
230 }
231
232 /*
233  K0 = 1
234  K1 = log(2)
235  K2 = 3-4*log(2)
236  K3 = 3*log(2) - 2
237 */
238 #define D0 16384
239 #define D1 11356
240 #define D2 3726
241 #define D3 1301
242 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
243 static inline celt_word32_t celt_exp2(celt_word16_t x)
244 {
245    int integer;
246    celt_word16_t frac;
247    integer = SHR16(x,11);
248    if (integer>14)
249       return 0x7fffffff;
250    else if (integer < -15)
251       return 0;
252    frac = SHL16(x-SHL16(integer,11),3);
253    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
254    return VSHR32(EXTEND32(frac), -integer-2);
255 }
256
257 /** Reciprocal approximation (Q15 input, Q16 output) */
258 static inline celt_word32_t celt_rcp(celt_word32_t x)
259 {
260    int i;
261    celt_word16_t n, frac;
262    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
263    celt_assert2(x>0, "celt_rcp() only defined for positive values");
264    i = celt_ilog2(x);
265    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
266    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
267                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
268    return VSHR32(EXTEND32(frac),i-16);
269 }
270
271 #define celt_div(a,b) MULT32_32_Q31((celt_word32_t)(a),celt_rcp(b))
272
273 #endif /* FIXED_POINT */
274
275
276 #endif /* MATHOPS_H */