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