134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* Copyright (C) 2002 Jean-Marc Valin */ 234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/** 334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) @file math_approx.h 434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) @brief Various math approximation functions for Speex 534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)*/ 634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* 734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) Redistribution and use in source and binary forms, with or without 834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) modification, are permitted provided that the following conditions 934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) are met: 1034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 1134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) - Redistributions of source code must retain the above copyright 1234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) notice, this list of conditions and the following disclaimer. 1334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 1434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) - Redistributions in binary form must reproduce the above copyright 1534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) notice, this list of conditions and the following disclaimer in the 1634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) documentation and/or other materials provided with the distribution. 1734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 1834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) - Neither the name of the Xiph.org Foundation nor the names of its 1934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) contributors may be used to endorse or promote products derived from 2034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) this software without specific prior written permission. 2134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 2234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 2334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 2434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 2534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR 2634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 2734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 2834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 2934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 3034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 3134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 3234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 3334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)*/ 3434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 3534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#ifndef MATH_APPROX_H 3634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define MATH_APPROX_H 3734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 3834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#include "arch.h" 3934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 4034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#ifndef FIXED_POINT 4134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 4234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define spx_sqrt sqrt 4334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define spx_acos acos 4434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define spx_exp exp 4534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define spx_cos_norm(x) (cos((.5f*M_PI)*(x))) 4634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define spx_atan atan 4734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 4834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/** Generate a pseudo-random number */ 4934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed) 5034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 5134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) const unsigned int jflone = 0x3f800000; 5234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) const unsigned int jflmsk = 0x007fffff; 5334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) union {int i; float f;} ran; 5434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) *seed = 1664525 * *seed + 1013904223; 5534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) ran.i = jflone | (jflmsk & *seed); 5634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) ran.f -= 1.5; 5734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 3.4642*std*ran.f; 5834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 5934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 6034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 6134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#endif 6234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 6334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 6434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_int16_t spx_ilog2(spx_uint32_t x) 6534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 6634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int r=0; 6734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=(spx_int32_t)65536) 6834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 6934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 16; 7034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 16; 7134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 7234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=256) 7334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 7434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 8; 7534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 8; 7634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 7734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=16) 7834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 7934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 4; 8034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 4; 8134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 8234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=4) 8334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 8434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 2; 8534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 2; 8634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 8734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=2) 8834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 8934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 1; 9034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 9134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return r; 9234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 9334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 9434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_int16_t spx_ilog4(spx_uint32_t x) 9534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 9634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int r=0; 9734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=(spx_int32_t)65536) 9834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 9934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 16; 10034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 8; 10134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 10234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=256) 10334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 10434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 8; 10534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 4; 10634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 10734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=16) 10834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 10934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x >>= 4; 11034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 2; 11134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 11234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>=4) 11334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 11434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) r += 1; 11534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 11634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return r; 11734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 11834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 11934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#ifdef FIXED_POINT 12034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 12134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/** Generate a pseudo-random number */ 12234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed) 12334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 12434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word32_t res; 12534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) *seed = 1664525 * *seed + 1013904223; 12634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std); 12734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return EXTRACT16(PSHR32(SUB32(res, SHR32(res, 3)),14)); 12834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 12934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 13034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25723*x^3 (for .25 < x < 1) */ 13134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/*#define C0 3634 13234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C1 21173 13334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C2 -12627 13434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C3 4215*/ 13534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 13634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */ 13734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C0 3634 13834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C1 21173 13934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C2 -12627 14034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C3 4204 14134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 14234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_sqrt(spx_word32_t x) 14334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 14434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int k; 14534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word32_t rt; 14634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) k = spx_ilog4(x)-6; 14734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = VSHR32(x, (k<<1)); 14834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3))))))); 14934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) rt = VSHR32(rt,7-k); 15034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return rt; 15134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 15234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 15334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* log(x) ~= -2.18151 + 4.20592*x - 2.88938*x^2 + 0.86535*x^3 (for .5 < x < 1) */ 15434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 15534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 15634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define A1 16469 15734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define A2 2242 15834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define A3 1486 15934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 16034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_acos(spx_word16_t x) 16134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 16234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int s=0; 16334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word16_t ret; 16434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word16_t sq; 16534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x<0) 16634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 16734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) s=1; 16834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = NEG16(x); 16934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 17034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = SUB16(16384,x); 17134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 17234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = x >> 1; 17334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) sq = MULT16_16_Q13(x, ADD16(A1, MULT16_16_Q13(x, ADD16(A2, MULT16_16_Q13(x, (A3)))))); 17434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) ret = spx_sqrt(SHL32(EXTEND32(sq),13)); 17534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 17634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) /*ret = spx_sqrt(67108864*(-1.6129e-04 + 2.0104e+00*f + 2.7373e-01*f*f + 1.8136e-01*f*f*f));*/ 17734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (s) 17834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) ret = SUB16(25736,ret); 17934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return ret; 18034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 18134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 18234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 18334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define K1 8192 18434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define K2 -4096 18534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define K3 340 18634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define K4 -10 18734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 18834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_cos(spx_word16_t x) 18934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 19034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word16_t x2; 19134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 19234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x<12868) 19334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 19434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x2 = MULT16_16_P13(x,x); 19534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return ADD32(K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2)))))); 19634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } else { 19734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = SUB16(25736,x); 19834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x2 = MULT16_16_P13(x,x); 19934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return SUB32(-K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2)))))); 20034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 20134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 20234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 20334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define L1 32767 20434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define L2 -7651 20534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define L3 8277 20634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define L4 -626 20734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 20834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t _spx_cos_pi_2(spx_word16_t x) 20934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 21034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word16_t x2; 21134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 21234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x2 = MULT16_16_P15(x,x); 21334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 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)))))))); 21434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 21534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 21634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_cos_norm(spx_word32_t x) 21734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 21834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = x&0x0001ffff; 21934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>SHL32(EXTEND32(1), 16)) 22034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = SUB32(SHL32(EXTEND32(1), 17),x); 22134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x&0x00007fff) 22234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 22334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x<SHL32(EXTEND32(1), 15)) 22434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 22534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return _spx_cos_pi_2(EXTRACT16(x)); 22634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } else { 22734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return NEG32(_spx_cos_pi_2(EXTRACT16(65536-x))); 22834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 22934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } else { 23034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x&0x0000ffff) 23134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 0; 23234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) else if (x&0x0001ffff) 23334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return -32767; 23434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) else 23534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 32767; 23634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 23734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 23834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 23934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* 24034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) K0 = 1 24134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) K1 = log(2) 24234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) K2 = 3-4*log(2) 24334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) K3 = 3*log(2) - 2 24434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)*/ 24534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define D0 16384 24634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define D1 11356 24734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define D2 3726 24834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define D3 1301 24934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* Input in Q11 format, output in Q16 */ 25034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word32_t spx_exp2(spx_word16_t x) 25134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 25234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int integer; 25334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) spx_word16_t frac; 25434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) integer = SHR16(x,11); 25534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (integer>14) 25634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 0x7fffffff; 25734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) else if (integer < -15) 25834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 0; 25934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) frac = SHL16(x-SHL16(integer,11),3); 26034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac)))))); 26134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return VSHR32(EXTEND32(frac), -integer-2); 26234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 26334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 26434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* Input in Q11 format, output in Q16 */ 26534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word32_t spx_exp(spx_word16_t x) 26634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 26734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x>21290) 26834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 0x7fffffff; 26934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) else if (x<-21290) 27034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 0; 27134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) else 27234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return spx_exp2(MULT16_16_P14(23637,x)); 27334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 27434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define M1 32767 27534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define M2 -21 27634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define M3 -11943 27734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define M4 4936 27834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 27934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_atan01(spx_word16_t x) 28034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 28134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x))))))); 28234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 28334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 28434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#undef M1 28534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#undef M2 28634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#undef M3 28734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#undef M4 28834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 28934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)/* Input in Q15, output in Q14 */ 29034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_atan(spx_word32_t x) 29134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 29234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x <= 32767) 29334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 29434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return SHR16(spx_atan01(x),1); 29534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } else { 29634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) int e = spx_ilog2(x); 29734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (e>=29) 29834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return 25736; 29934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = DIV32_16(SHL32(EXTEND32(32767),29-e), EXTRACT16(SHR32(x, e-14))); 30034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return SUB16(25736, SHR16(spx_atan01(x),1)); 30134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 30234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 30334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#else 30434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 30534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#ifndef M_PI 30634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define M_PI 3.14159265358979323846 /* pi */ 30734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#endif 30834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 30934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C1 0.9999932946f 31034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C2 -0.4999124376f 31134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C3 0.0414877472f 31234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define C4 -0.0012712095f 31334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 31434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 31534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#define SPX_PI_2 1.5707963268 31634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)static inline spx_word16_t spx_cos(spx_word16_t x) 31734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles){ 31834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) if (x<SPX_PI_2) 31934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) { 32034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x *= x; 32134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return C1 + x*(C2+x*(C3+C4*x)); 32234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } else { 32334680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x = M_PI-x; 32434680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) x *= x; 32534680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) return NEG16(C1 + x*(C2+x*(C3+C4*x))); 32634680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) } 32734680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)} 32834680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 32934680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#endif 33034680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 33134680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles) 33234680572440d7894ef8dafce81d8039ed80726a2Torne (Richard Coles)#endif 333