fixed-point: added cheap celt_div() division using a reciprocal
[opus.git] / libcelt / mathops.h
1 /* Copyright (C) 2002-2008 Jean-Marc Valin */
2 /**
3    @file mathops.h
4    @brief Various math functions
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 MATHOPS_H
36 #define MATHOPS_H
37
38 #include "arch.h"
39
40 #ifndef FIXED_POINT
41
42 #define celt_sqrt sqrt
43 #define celt_acos acos
44 #define celt_exp exp
45 #define celt_cos_norm(x) (cos((.5f*M_PI)*(x)))
46 #define celt_atan atan
47 #define celt_rcp(x) (1.f/(x))
48 #define celt_div(a,b) ((a)/(b))
49
50 #endif
51
52
53
54 #ifdef FIXED_POINT
55
56 #include "entcode.h"
57 #include "os_support.h"
58
59 #ifndef OVERRIDE_CELT_ILOG2
60 /** Integer log in base2. Undefined for zero and negative numbers */
61 static inline celt_int16_t celt_ilog2(celt_word32_t x)
62 {
63    celt_assert2(x>0, "celt_ilog2() only defined for strictly positive numbers");
64    return EC_ILOG(x)-1;
65 }
66 #endif
67
68 #ifndef OVERRIDE_CELT_MAXABS16
69 static inline celt_word16_t celt_maxabs16(celt_word16_t *x, int len)
70 {
71    int i;
72    celt_word16_t maxval = 0;
73    for (i=0;i<len;i++)
74       maxval = MAX16(maxval, ABS16(x[i]));
75    return maxval;
76 }
77 #endif
78
79 /** Integer log in base2. Defined for zero, but not for negative numbers */
80 static inline celt_int16_t celt_zlog2(celt_word32_t x)
81 {
82    return EC_ILOG(x)-1;
83 }
84
85 /** Sqrt approximation (QX input, QX/2 output) */
86 static inline celt_word32_t celt_sqrt(celt_word32_t x)
87 {
88    int k;
89    celt_word16_t n;
90    celt_word32_t rt;
91    const celt_word16_t C[5] = {23174, 11584, -3011, 1570, -557};
92    if (x==0)
93       return 0;
94    k = (celt_ilog2(x)>>1)-7;
95    x = VSHR32(x, (k<<1));
96    n = x-32768;
97    rt = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
98               MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
99    rt = VSHR32(rt,7-k);
100    return rt;
101 }
102
103
104 #define L1 32767
105 #define L2 -7651
106 #define L3 8277
107 #define L4 -626
108
109 static inline celt_word16_t _celt_cos_pi_2(celt_word16_t x)
110 {
111    celt_word16_t x2;
112    
113    x2 = MULT16_16_P15(x,x);
114    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
115                                                                                 ))))))));
116 }
117
118 #undef L1
119 #undef L2
120 #undef L3
121 #undef L4
122
123 static inline celt_word16_t celt_cos_norm(celt_word32_t x)
124 {
125    x = x&0x0001ffff;
126    if (x>SHL32(EXTEND32(1), 16))
127       x = SUB32(SHL32(EXTEND32(1), 17),x);
128    if (x&0x00007fff)
129    {
130       if (x<SHL32(EXTEND32(1), 15))
131       {
132          return _celt_cos_pi_2(EXTRACT16(x));
133       } else {
134          return NEG32(_celt_cos_pi_2(EXTRACT16(65536-x)));
135       }
136    } else {
137       if (x&0x0000ffff)
138          return 0;
139       else if (x&0x0001ffff)
140          return -32767;
141       else
142          return 32767;
143    }
144 }
145
146 static inline celt_word16_t celt_log2(celt_word32_t x)
147 {
148    int i;
149    celt_word16_t n, frac;
150    /*-0.41446   0.96093  -0.33981   0.15600 */
151    const celt_word16_t C[4] = {-6791, 7872, -1392, 319};
152    if (x==0)
153       return -32767;
154    i = celt_ilog2(x);
155    n = VSHR32(x,i-15)-32768-16384;
156    frac = ADD16(C[0], MULT16_16_Q14(n, ADD16(C[1], MULT16_16_Q14(n, ADD16(C[2], MULT16_16_Q14(n, (C[3])))))));
157    /*printf ("%d %d %d %d\n", x, n, ret, SHL16(i-13,8)+SHR16(ret,14-8));*/
158    return SHL16(i-13,8)+SHR16(frac,14-8);
159 }
160
161 /*
162  K0 = 1
163  K1 = log(2)
164  K2 = 3-4*log(2)
165  K3 = 3*log(2) - 2
166 */
167 #define D0 16384
168 #define D1 11356
169 #define D2 3726
170 #define D3 1301
171 /** Base-2 exponential approximation (2^x). (Q11 input, Q16 output) */
172 static inline celt_word32_t celt_exp2(celt_word16_t x)
173 {
174    int integer;
175    celt_word16_t frac;
176    integer = SHR16(x,11);
177    if (integer>14)
178       return 0x7fffffff;
179    else if (integer < -15)
180       return 0;
181    frac = SHL16(x-SHL16(integer,11),3);
182    frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
183    return VSHR32(EXTEND32(frac), -integer-2);
184 }
185
186 /** Reciprocal approximation (Q15 input, Q16 output) */
187 static inline celt_word32_t celt_rcp(celt_word32_t x)
188 {
189    int i, neg=0;
190    celt_word16_t n, frac;
191    const celt_word16_t C[5] = {21848, -7251, 2403, -934, 327};
192    if (x<0)
193    {
194       neg = 1;
195       x = NEG16(x);
196    }
197    i = celt_ilog2(x);
198    n = VSHR32(x,i-16)-SHL32(EXTEND32(3),15);
199    frac = ADD16(C[0], MULT16_16_Q15(n, ADD16(C[1], MULT16_16_Q15(n, ADD16(C[2], 
200                 MULT16_16_Q15(n, ADD16(C[3], MULT16_16_Q15(n, (C[4])))))))));
201    if (neg)
202       frac = -frac;
203    return VSHR32(EXTEND32(frac),i-16);
204 }
205
206 #define celt_div(a,b) MULT32_32_Q31(a,celt_rcp(b))
207
208 #endif /* FIXED_POINT */
209
210
211 #endif /* MATHOPS_H */