apps/codecs/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