jitter buffer: make tick() a tiny bit smarter.
[speexdsp.git] / libspeex / math_approx.h
1 /* Copyright (C) 2002 Jean-Marc Valin */
2 /**
3    @file math_approx.h
4    @brief Various math approximation functions for Speex
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 MATH_APPROX_H
36 #define MATH_APPROX_H
37
38 #include "arch.h"
39
40 #ifndef FIXED_POINT
41
42 #define spx_sqrt sqrt
43 #define spx_acos acos
44 #define spx_exp exp
45 #define spx_cos_norm(x) (cos((.5f*M_PI)*(x)))
46 #define spx_atan atan
47
48 /** Generate a pseudo-random number */
49 static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed)
50 {
51    const unsigned int jflone = 0x3f800000;
52    const unsigned int jflmsk = 0x007fffff;
53    union {int i; float f;} ran;
54    *seed = 1664525 * *seed + 1013904223;
55    ran.i = jflone | (jflmsk & *seed);
56    ran.f -= 1.5;
57    return 3.4642*std*ran.f;
58 }
59
60
61 #endif
62
63
64 static inline spx_int16_t spx_ilog2(spx_uint32_t x)
65 {
66    int r=0;
67    if (x>=(spx_int32_t)65536)
68    {
69       x >>= 16;
70       r += 16;
71    }
72    if (x>=256)
73    {
74       x >>= 8;
75       r += 8;
76    }
77    if (x>=16)
78    {
79       x >>= 4;
80       r += 4;
81    }
82    if (x>=4)
83    {
84       x >>= 2;
85       r += 2;
86    }
87    if (x>=2)
88    {
89       r += 1;
90    }
91    return r;
92 }
93
94 static inline spx_int16_t spx_ilog4(spx_uint32_t x)
95 {
96    int r=0;
97    if (x>=(spx_int32_t)65536)
98    {
99       x >>= 16;
100       r += 8;
101    }
102    if (x>=256)
103    {
104       x >>= 8;
105       r += 4;
106    }
107    if (x>=16)
108    {
109       x >>= 4;
110       r += 2;
111    }
112    if (x>=4)
113    {
114       r += 1;
115    }
116    return r;
117 }
118
119 #ifdef FIXED_POINT
120
121 /** Generate a pseudo-random number */
122 static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed)
123 {
124    spx_word32_t res;
125    *seed = 1664525 * *seed + 1013904223;
126    res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std);
127    return EXTRACT16(PSHR32(SUB32(res, SHR32(res, 3)),14));
128 }
129
130 /* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25723*x^3 (for .25 < x < 1) */
131 /*#define C0 3634
132 #define C1 21173
133 #define C2 -12627
134 #define C3 4215*/
135
136 /* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */
137 #define C0 3634
138 #define C1 21173
139 #define C2 -12627
140 #define C3 4204
141
142 static inline spx_word16_t spx_sqrt(spx_word32_t x)
143 {
144    int k;
145    spx_word32_t rt;
146    k = spx_ilog4(x)-6;
147    x = VSHR32(x, (k<<1));
148    rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3)))))));
149    rt = VSHR32(rt,7-k);
150    return rt;
151 }
152
153 /* log(x) ~= -2.18151 + 4.20592*x - 2.88938*x^2 + 0.86535*x^3 (for .5 < x < 1) */
154
155
156 #define A1 16469
157 #define A2 2242
158 #define A3 1486
159
160 static inline spx_word16_t spx_acos(spx_word16_t x)
161 {
162    int s=0;
163    spx_word16_t ret;
164    spx_word16_t sq;
165    if (x<0)
166    {
167       s=1;
168       x = NEG16(x);
169    }
170    x = SUB16(16384,x);
171    
172    x = x >> 1;
173    sq = MULT16_16_Q13(x, ADD16(A1, MULT16_16_Q13(x, ADD16(A2, MULT16_16_Q13(x, (A3))))));
174    ret = spx_sqrt(SHL32(EXTEND32(sq),13));
175    
176    /*ret = spx_sqrt(67108864*(-1.6129e-04 + 2.0104e+00*f + 2.7373e-01*f*f + 1.8136e-01*f*f*f));*/
177    if (s)
178       ret = SUB16(25736,ret);
179    return ret;
180 }
181
182
183 #define K1 8192
184 #define K2 -4096
185 #define K3 340
186 #define K4 -10
187
188 static inline spx_word16_t spx_cos(spx_word16_t x)
189 {
190    spx_word16_t x2;
191
192    if (x<12868)
193    {
194       x2 = MULT16_16_P13(x,x);
195       return ADD32(K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2))))));
196    } else {
197       x = SUB16(25736,x);
198       x2 = MULT16_16_P13(x,x);
199       return SUB32(-K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2))))));
200    }
201 }
202
203 #define L1 32767
204 #define L2 -7651
205 #define L3 8277
206 #define L4 -626
207
208 static inline spx_word16_t _spx_cos_pi_2(spx_word16_t x)
209 {
210    spx_word16_t x2;
211    
212    x2 = MULT16_16_P15(x,x);
213    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))))))));
214 }
215
216 static inline spx_word16_t spx_cos_norm(spx_word32_t x)
217 {
218    x = x&0x0001ffff;
219    if (x>SHL32(EXTEND32(1), 16))
220       x = SUB32(SHL32(EXTEND32(1), 17),x);
221    if (x&0x00007fff)
222    {
223       if (x<SHL32(EXTEND32(1), 15))
224       {
225          return _spx_cos_pi_2(EXTRACT16(x));
226       } else {
227          return NEG32(_spx_cos_pi_2(EXTRACT16(65536-x)));
228       }
229    } else {
230       if (x&0x0000ffff)
231          return 0;
232       else if (x&0x0001ffff)
233          return -32767;
234       else
235          return 32767;
236    }
237 }
238
239 /*
240  K0 = 1
241  K1 = log(2)
242  K2 = 3-4*log(2)
243  K3 = 3*log(2) - 2
244 */
245 #define D0 16384
246 #define D1 11356
247 #define D2 3726
248 #define D3 1301
249 /* Input in Q11 format, output in Q16 */
250 static inline spx_word32_t spx_exp2(spx_word16_t x)
251 {
252    int integer;
253    spx_word16_t frac;
254    integer = SHR16(x,11);
255    if (integer>14)
256       return 0x7fffffff;
257    else if (integer < -15)
258       return 0;
259    frac = SHL16(x-SHL16(integer,11),3);
260    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
261    return VSHR32(EXTEND32(frac), -integer-2);
262 }
263
264 /* Input in Q11 format, output in Q16 */
265 static inline spx_word32_t spx_exp(spx_word16_t x)
266 {
267    if (x>21290)
268       return 0x7fffffff;
269    else if (x<-21290)
270       return 0;
271    else
272       return spx_exp2(MULT16_16_P14(23637,x));
273 }
274 #define M1 32767
275 #define M2 -21
276 #define M3 -11943
277 #define M4 4936
278
279 static inline spx_word16_t spx_atan01(spx_word16_t x)
280 {
281    return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
282 }
283
284 #undef M1
285 #undef M2
286 #undef M3
287 #undef M4
288
289 /* Input in Q15, output in Q14 */
290 static inline spx_word16_t spx_atan(spx_word32_t x)
291 {
292    if (x <= 32767)
293    {
294       return SHR16(spx_atan01(x),1);
295    } else {
296       int e = spx_ilog2(x);
297       if (e>=29)
298          return 25736;
299       x = DIV32_16(SHL32(EXTEND32(32767),29-e), EXTRACT16(SHR32(x, e-14)));
300       return SUB16(25736, SHR16(spx_atan01(x),1));
301    }
302 }
303 #else
304
305 #ifndef M_PI
306 #define M_PI           3.14159265358979323846  /* pi */
307 #endif
308
309 #define C1 0.9999932946f
310 #define C2 -0.4999124376f
311 #define C3 0.0414877472f
312 #define C4 -0.0012712095f
313
314
315 #define SPX_PI_2 1.5707963268
316 static inline spx_word16_t spx_cos(spx_word16_t x)
317 {
318    if (x<SPX_PI_2)
319    {
320       x *= x;
321       return C1 + x*(C2+x*(C3+C4*x));
322    } else {
323       x = M_PI-x;
324       x *= x;
325       return NEG16(C1 + x*(C2+x*(C3+C4*x)));
326    }
327 }
328
329 #endif
330
331
332 #endif