math_approx.h revision 98913fed6520d8849fb2e246be943e04474aefa4 
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 */
49static 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
64static 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
94static 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 */
122static 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
142static 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
160static 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
188static 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
208static 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
216static 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 */
250static 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 */
265static 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
279static 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 */
290static 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
316static 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