Blame libcelt/mathops.h

Packit 664db3
/* Copyright (C) 2002-2008 Jean-Marc Valin */
Packit 664db3
/**
Packit 664db3
   @file mathops.h
Packit 664db3
   @brief Various math functions
Packit 664db3
*/
Packit 664db3
/*
Packit 664db3
   Redistribution and use in source and binary forms, with or without
Packit 664db3
   modification, are permitted provided that the following conditions
Packit 664db3
   are met:
Packit 664db3
   
Packit 664db3
   - Redistributions of source code must retain the above copyright
Packit 664db3
   notice, this list of conditions and the following disclaimer.
Packit 664db3
   
Packit 664db3
   - Redistributions in binary form must reproduce the above copyright
Packit 664db3
   notice, this list of conditions and the following disclaimer in the
Packit 664db3
   documentation and/or other materials provided with the distribution.
Packit 664db3
   
Packit 664db3
   - Neither the name of the Xiph.org Foundation nor the names of its
Packit 664db3
   contributors may be used to endorse or promote products derived from
Packit 664db3
   this software without specific prior written permission.
Packit 664db3
   
Packit 664db3
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
Packit 664db3
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
Packit 664db3
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
Packit 664db3
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
Packit 664db3
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
Packit 664db3
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
Packit 664db3
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
Packit 664db3
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
Packit 664db3
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
Packit 664db3
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
Packit 664db3
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Packit 664db3
*/
Packit 664db3
Packit 664db3
#ifndef MATHOPS_H
Packit 664db3
#define MATHOPS_H
Packit 664db3
Packit 664db3
#include "arch.h"
Packit 664db3
#include "entcode.h"
Packit 664db3
#include "os_support.h"
Packit 664db3
Packit 664db3
#ifndef OVERRIDE_CELT_ILOG2
Packit 664db3
/** Integer log in base2. Undefined for zero and negative numbers */
Packit 664db3
static inline celt_int16_t celt_ilog2(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   celt_assert2(x>0, "celt_ilog2() only defined for strictly positive numbers");
Packit 664db3
   return EC_ILOG(x)-1;
Packit 664db3
}
Packit 664db3
#endif
Packit 664db3
Packit 664db3
#ifndef OVERRIDE_FIND_MAX16
Packit 664db3
static inline int find_max16(celt_word16_t *x, int len)
Packit 664db3
{
Packit 664db3
   celt_word16_t max_corr=-VERY_LARGE16;
Packit 664db3
   int i, id = 0;
Packit 664db3
   for (i=0;i
Packit 664db3
   {
Packit 664db3
      if (x[i] > max_corr)
Packit 664db3
      {
Packit 664db3
         id = i;
Packit 664db3
         max_corr = x[i];
Packit 664db3
      }
Packit 664db3
   }
Packit 664db3
   return id;
Packit 664db3
}
Packit 664db3
#endif
Packit 664db3
Packit 664db3
#ifndef OVERRIDE_FIND_MAX32
Packit 664db3
static inline int find_max32(celt_word32_t *x, int len)
Packit 664db3
{
Packit 664db3
   celt_word32_t max_corr=-VERY_LARGE32;
Packit 664db3
   int i, id = 0;
Packit 664db3
   for (i=0;i
Packit 664db3
   {
Packit 664db3
      if (x[i] > max_corr)
Packit 664db3
      {
Packit 664db3
         id = i;
Packit 664db3
         max_corr = x[i];
Packit 664db3
      }
Packit 664db3
   }
Packit 664db3
   return id;
Packit 664db3
}
Packit 664db3
#endif
Packit 664db3
Packit 664db3
Packit 664db3
#ifndef FIXED_POINT
Packit 664db3
Packit 664db3
#define celt_sqrt(x) ((float)sqrt(x))
Packit 664db3
#define celt_psqrt(x) ((float)sqrt(x))
Packit 664db3
#define celt_rsqrt(x) (1.f/celt_sqrt(x))
Packit 664db3
#define celt_acos acos
Packit 664db3
#define celt_exp exp
Packit 664db3
#define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
Packit 664db3
#define celt_atan atan
Packit 664db3
#define celt_rcp(x) (1.f/(x))
Packit 664db3
#define celt_div(a,b) ((a)/(b))
Packit 664db3
Packit 664db3
#endif
Packit 664db3
Packit 664db3
Packit 664db3
Packit 664db3
#ifdef FIXED_POINT
Packit 664db3
Packit 664db3
#include "os_support.h"
Packit 664db3
Packit 664db3
#ifndef OVERRIDE_CELT_MAXABS16
Packit 664db3
static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
Packit 664db3
{
Packit 664db3
   int i;
Packit 664db3
   celt_word16_t maxval = 0;
Packit 664db3
   for (i=0;i
Packit 664db3
      maxval = MAX16(maxval, ABS16(x[i]));
Packit 664db3
   return maxval;
Packit 664db3
}
Packit 664db3
#endif
Packit 664db3
Packit 664db3
/** Integer log in base2. Defined for zero, but not for negative numbers */
Packit 664db3
static inline celt_int16_t celt_zlog2(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   return x <= 0 ? 0 : celt_ilog2(x);
Packit 664db3
}
Packit 664db3
Packit 664db3
/** Reciprocal sqrt approximation (Q30 input, Q0 output or equivalent) */
Packit 664db3
static inline celt_word32_t celt_rsqrt(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   int k;
Packit 664db3
   celt_word16_t n;
Packit 664db3
   celt_word32_t rt;
Packit 664db3
   const celt_word16_t C[5] = {23126, -11496, 9812, -9097, 4100};
Packit 664db3
   k = celt_ilog2(x)>>1;
Packit 664db3
   x = VSHR32(x, (k-7)<<1);
Packit 664db3
   /* Range of n is [-16384,32767] */
Packit 664db3
   n = x-32768;
Packit 664db3
   rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
Packit 664db3
              MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
Packit 664db3
   rt = VSHR32(rt,k);
Packit 664db3
   return rt;
Packit 664db3
}
Packit 664db3
Packit 664db3
/** Sqrt approximation (QX input, QX/2 output) */
Packit 664db3
static inline celt_word32_t celt_sqrt(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   int k;
Packit 664db3
   celt_word16_t n;
Packit 664db3
   celt_word32_t rt;
Packit 664db3
   const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
Packit 664db3
   if (x==0)
Packit 664db3
      return 0;
Packit 664db3
   k = (celt_ilog2(x)>>1)-7;
Packit 664db3
   x = VSHR32(x, (k<<1));
Packit 664db3
   n = x-32768;
Packit 664db3
   rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
Packit 664db3
              MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
Packit 664db3
   rt = VSHR32(rt,7-k);
Packit 664db3
   return rt;
Packit 664db3
}
Packit 664db3
Packit 664db3
/** Sqrt approximation (QX input, QX/2 output) that assumes that the input is
Packit 664db3
    strictly positive */
Packit 664db3
static inline celt_word32_t celt_psqrt(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   int k;
Packit 664db3
   celt_word16_t n;
Packit 664db3
   celt_word32_t rt;
Packit 664db3
   const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
Packit 664db3
   k = (celt_ilog2(x)>>1)-7;
Packit 664db3
   x = VSHR32(x, (k<<1));
Packit 664db3
   n = x-32768;
Packit 664db3
   rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
Packit 664db3
              MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
Packit 664db3
   rt = VSHR32(rt,7-k);
Packit 664db3
   return rt;
Packit 664db3
}
Packit 664db3
Packit 664db3
#define L1 32767
Packit 664db3
#define L2 -7651
Packit 664db3
#define L3 8277
Packit 664db3
#define L4 -626
Packit 664db3
Packit 664db3
static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
Packit 664db3
{
Packit 664db3
   celt_word16_t x2;
Packit 664db3
   
Packit 664db3
   x2 = MULT16_16_P15(x,x);
Packit 664db3
   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
Packit 664db3
                                                                                ))))))));
Packit 664db3
}
Packit 664db3
Packit 664db3
#undef L1
Packit 664db3
#undef L2
Packit 664db3
#undef L3
Packit 664db3
#undef L4
Packit 664db3
Packit 664db3
static inline celt_word16_t celt_cos_norm(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   x = x&0x0001ffff;
Packit 664db3
   if (x>SHL32(EXTEND32(1), 16))
Packit 664db3
      x = SUB32(SHL32(EXTEND32(1), 17),x);
Packit 664db3
   if (x&0x00007fff)
Packit 664db3
   {
Packit 664db3
      if (x
Packit 664db3
      {
Packit 664db3
         return _celt_cos_pi_2(EXTRACT16(x));
Packit 664db3
      } else {
Packit 664db3
         return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
Packit 664db3
      }
Packit 664db3
   } else {
Packit 664db3
      if (x&0x0000ffff)
Packit 664db3
         return 0;
Packit 664db3
      else if (x&0x0001ffff)
Packit 664db3
         return -32767;
Packit 664db3
      else
Packit 664db3
         return 32767;
Packit 664db3
   }
Packit 664db3
}
Packit 664db3
Packit 664db3
static inline celt_word16_t celt_log2(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   int i;
Packit 664db3
   celt_word16_t n, frac;
Packit 664db3
   /*-0.41446   0.96093  -0.33981   0.15600 */
Packit 664db3
   const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
Packit 664db3
   if (x==0)
Packit 664db3
      return -32767;
Packit 664db3
   i = celt_ilog2(x);
Packit 664db3
   n = VSHR32(x,i-15)-32768-16384;
Packit 664db3
   frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
Packit 664db3
   /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
Packit 664db3
   return SHL16(i-13,8)+SHR16(frac,14-8);
Packit 664db3
}
Packit 664db3
Packit 664db3
/*
Packit 664db3
 K0 = 1
Packit 664db3
 K1 = log(2)
Packit 664db3
 K2 = 3-4*log(2)
Packit 664db3
 K3 = 3*log(2) - 2
Packit 664db3
*/
Packit 664db3
#define D0 16384
Packit 664db3
#define D1 11356
Packit 664db3
#define D2 3726
Packit 664db3
#define D3 1301
Packit 664db3
/** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
Packit 664db3
static inline celt_word32_t celt_exp2(celt_word16_t x)
Packit 664db3
{
Packit 664db3
   int integer;
Packit 664db3
   celt_word16_t frac;
Packit 664db3
   integer = SHR16(x,11);
Packit 664db3
   if (integer>14)
Packit 664db3
      return 0x7f000000;
Packit 664db3
   else if (integer < -15)
Packit 664db3
      return 0;
Packit 664db3
   frac = SHL16(x-SHL16(integer,11),3);
Packit 664db3
   frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
Packit 664db3
   return VSHR32(EXTEND32(frac), -integer-2);
Packit 664db3
}
Packit 664db3
Packit 664db3
/** Reciprocal approximation (Q15 input, Q16 output) */
Packit 664db3
static inline celt_word32_t celt_rcp(celt_word32_t x)
Packit 664db3
{
Packit 664db3
   int i;
Packit 664db3
   celt_word16_t n, frac;
Packit 664db3
   const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
Packit 664db3
   celt_assert2(x>0, "celt_rcp() only defined for positive values");
Packit 664db3
   i = celt_ilog2(x);
Packit 664db3
   n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
Packit 664db3
   frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
Packit 664db3
                MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
Packit 664db3
   return VSHR32(EXTEND32(frac),i-16);
Packit 664db3
}
Packit 664db3
Packit 664db3
#define celt_div(a,b) MULT32_32_Q31((celt_word32_t)(a),celt_rcp(b))
Packit 664db3
Packit 664db3
#endif /* FIXED_POINT */
Packit 664db3
Packit 664db3
Packit 664db3
#endif /* MATHOPS_H */