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 */
speex_rand(spx_word16_t std,spx_int32_t * seed)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
spx_ilog2(spx_uint32_t x)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
spx_ilog4(spx_uint32_t x)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 */
speex_rand(spx_word16_t std,spx_int32_t * seed)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
spx_sqrt(spx_word32_t x)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
spx_acos(spx_word16_t x)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
spx_cos(spx_word16_t x)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
_spx_cos_pi_2(spx_word16_t x)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
spx_cos_norm(spx_word32_t x)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 */
spx_exp2(spx_word16_t x)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 */
spx_exp(spx_word16_t x)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
spx_atan01(spx_word16_t x)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 */
spx_atan(spx_word32_t x)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
spx_cos(spx_word16_t x)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
333