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 333
