9ff1b75108638c4fca8aeea47d785f8beb4c4dfb
[speexdsp.git] / libspeex / pseudofloat.h
1 /* Copyright (C) 2005 Jean-Marc Valin */
2 /**
3    @file pseudofloat.h
4    @brief Pseudo-floating point
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 PSEUDOFLOAT_H
36 #define PSEUDOFLOAT_H
37
38 #include "misc.h"
39 #include "math_approx.h"
40 #include <math.h>
41
42 #ifdef FIXED_POINT
43
44 typedef struct {
45    spx_int16_t m;
46    spx_int16_t e;
47 } spx_float_t;
48
49 static const spx_float_t FLOAT_ZERO = {0,0};
50 static const spx_float_t FLOAT_ONE = {16384,-14};
51 static const spx_float_t FLOAT_HALF = {16384,-15};
52
53 #define MIN(a,b) ((a)<(b)?(a):(b))
54 static inline spx_float_t PSEUDOFLOAT(spx_int32_t x)
55 {
56    int e=0;
57    int sign=0;
58    if (x<0)
59    {
60       sign = 1;
61       x = -x;
62    }
63    if (x==0)
64    {
65       spx_float_t r = {0,0};
66       return r;
67    }
68    while (x>32767)
69    {
70       x >>= 1;
71       /*x *= .5;*/
72       e++;
73    }
74    while (x<16383)
75    {
76       x <<= 1;
77       /*x *= 2;*/
78       e--;
79    }
80    if (sign)
81    {
82       spx_float_t r;
83       r.m = -x;
84       r.e = e;
85       return r;
86    }
87    else      
88    {
89       spx_float_t r;
90       r.m = x;
91       r.e = e;
92       return r;
93    }
94 }
95
96
97 static inline spx_float_t FLOAT_ADD(spx_float_t a, spx_float_t b)
98 {
99    spx_float_t r;
100    if (a.m==0)
101       return b;
102    else if (b.m==0)
103       return a;
104    if ((a).e > (b).e) 
105    {
106       r.m = ((a).m>>1) + ((b).m>>MIN(15,(a).e-(b).e+1));
107       r.e = (a).e+1;
108    }
109    else 
110    {
111       r.m = ((b).m>>1) + ((a).m>>MIN(15,(b).e-(a).e+1));
112       r.e = (b).e+1;
113    }
114    if (r.m>0)
115    {
116       if (r.m<16384)
117       {
118          r.m<<=1;
119          r.e-=1;
120       }
121    } else {
122       if (r.m>-16384)
123       {
124          r.m<<=1;
125          r.e-=1;
126       }
127    }
128    /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
129    return r;
130 }
131
132 static inline spx_float_t FLOAT_SUB(spx_float_t a, spx_float_t b)
133 {
134    spx_float_t r;
135    if (a.m==0)
136       return b;
137    else if (b.m==0)
138       return a;
139    if ((a).e > (b).e)
140    {
141       r.m = ((a).m>>1) - ((b).m>>MIN(15,(a).e-(b).e+1));
142       r.e = (a).e+1;
143    }
144    else 
145    {
146       r.m = ((a).m>>MIN(15,(b).e-(a).e+1)) - ((b).m>>1);
147       r.e = (b).e+1;
148    }
149    if (r.m>0)
150    {
151       if (r.m<16384)
152       {
153          r.m<<=1;
154          r.e-=1;
155       }
156    } else {
157       if (r.m>-16384)
158       {
159          r.m<<=1;
160          r.e-=1;
161       }
162    }
163    /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
164    return r;
165 }
166
167 static inline int FLOAT_LT(spx_float_t a, spx_float_t b)
168 {
169    if (a.m==0)
170       return b.m<0;
171    else if (b.m==0)
172       return a.m>0;   
173    if ((a).e > (b).e)
174       return ((a).m>>1) < ((b).m>>MIN(15,(a).e-(b).e+1));
175    else 
176       return ((b).m>>1) > ((a).m>>MIN(15,(b).e-(a).e+1));
177
178 }
179
180 static inline int FLOAT_GT(spx_float_t a, spx_float_t b)
181 {
182    return FLOAT_LT(b,a);
183 }
184
185 static inline spx_float_t FLOAT_MULT(spx_float_t a, spx_float_t b)
186 {
187    spx_float_t r;
188    r.m = (spx_int16_t)((spx_int32_t)(a).m*(b).m>>15);
189    r.e = (a).e+(b).e+15;
190    if (r.m>0)
191    {
192       if (r.m<16384)
193       {
194          r.m<<=1;
195          r.e-=1;
196       }
197    } else {
198       if (r.m>-16384)
199       {
200          r.m<<=1;
201          r.e-=1;
202       }
203    }
204    /*printf ("%f * %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
205    return r;   
206 }
207
208
209 static inline spx_float_t FLOAT_SHL(spx_float_t a, int b)
210 {
211    spx_float_t r;
212    r.m = a.m;
213    r.e = a.e+b;
214    return r;
215 }
216
217 static inline spx_int16_t FLOAT_EXTRACT16(spx_float_t a)
218 {
219    if (a.e<0)
220       return EXTRACT16((EXTEND32(a.m)+(1<<(-a.e-1)))>>-a.e);
221    else
222       return a.m<<a.e;
223 }
224
225 static inline spx_int32_t FLOAT_MUL32(spx_float_t a, spx_word32_t b)
226 {
227    if (a.e<-15)
228       return SHR32(MULT16_32_Q15(a.m, b),-a.e-15);
229    else
230       return SHL32(MULT16_32_Q15(a.m, b),15+a.e);
231 }
232
233 static inline spx_float_t FLOAT_MUL32U(spx_word32_t a, spx_word32_t b)
234 {
235    int e=0;
236    spx_float_t r;
237    /* FIXME: Handle the sign */
238    if (a==0)
239    {
240       return FLOAT_ZERO;
241    }
242    while (a>32767)
243    {
244       a >>= 1;
245       e++;
246    }
247    while (a<16384)
248    {
249       a <<= 1;
250       e--;
251    }
252    while (b>32767)
253    {
254       b >>= 1;
255       e++;
256    }
257    while (b<16384)
258    {
259       b <<= 1;
260       e--;
261    }
262    r.m = MULT16_16_Q15(a,b);
263    r.e = e+15;
264    return r;
265 }
266
267 static inline spx_float_t FLOAT_DIV32_FLOAT(spx_word32_t a, spx_float_t b)
268 {
269    int e=0;
270    spx_float_t r;
271    /* FIXME: Handle the sign */
272    if (a==0)
273    {
274       return FLOAT_ZERO;
275    }
276    while (a<SHL32(EXTEND32(b.m),14))
277    {
278       a <<= 1;
279       e--;
280    }
281    while (a>=SHL32(EXTEND32(b.m-1),15))
282    {
283       a >>= 1;
284       e++;
285    }
286    r.m = DIV32_16(a,b.m);
287    r.e = e-b.e;
288    return r;
289 }
290
291
292 static inline spx_float_t FLOAT_DIV32(spx_word32_t a, spx_word32_t b)
293 {
294    int e=0;
295    spx_float_t r;
296    /* FIXME: Handle the sign */
297    if (a==0)
298    {
299       return FLOAT_ZERO;
300    }
301    while (b>32767)
302    {
303       b >>= 1;
304       e--;
305    }
306    while (a<SHL32(b,14))
307    {
308       a <<= 1;
309       e--;
310    }
311    while (a>=SHL32(b-1,15))
312    {
313       a >>= 1;
314       e++;
315    }
316    r.m = DIV32_16(a,b);
317    r.e = e;
318    return r;
319 }
320
321 static inline spx_float_t FLOAT_DIVU(spx_float_t a, spx_float_t b)
322 {
323    int e=0;
324    spx_int32_t num;
325    spx_float_t r;
326    num = a.m;
327    while (a.m >= b.m)
328    {
329       e++;
330       a.m >>= 1;
331    }
332    num = num << (15-e);
333    r.m = DIV32_16(num,b.m);
334    r.e = a.e-b.e-15+e;
335    return r;
336 }
337
338 static inline spx_float_t FLOAT_SQRT(spx_float_t a)
339 {
340    spx_float_t r;
341    spx_int32_t m;
342    m = a.m << 14;
343    r.e = a.e - 14;
344    if (r.e & 1)
345    {
346       r.e -= 1;
347       m <<= 1;
348    }
349    r.e >>= 1;
350    r.m = spx_sqrt(m);
351    return r;
352 }
353
354 #else
355
356 #define spx_float_t float
357 #define FLOAT_ZERO 0.f
358 #define FLOAT_ONE 1.f
359 #define FLOAT_HALF 0.5f
360 #define PSEUDOFLOAT(x) (x)
361 #define FLOAT_MULT(a,b) ((a)*(b))
362 #define FLOAT_MUL32(a,b) ((a)*(b))
363 #define FLOAT_DIV32(a,b) ((a)/(b))
364 #define FLOAT_EXTRACT16(a) (a)
365 #define FLOAT_ADD(a,b) ((a)+(b))
366 #define FLOAT_SUB(a,b) ((a)-(b))
367 #define REALFLOAT(x) (x)
368 #define FLOAT_DIV32_FLOAT(a,b) ((a)/(b))
369 #define FLOAT_MUL32U(a,b) ((a)*(b))
370 #define FLOAT_SHL(a,b) (a)
371 #define FLOAT_LT(a,b) ((a)<(b))
372 #define FLOAT_GT(a,b) ((a)>(b))
373 #define FLOAT_DIVU(a,b) ((a)/(b))
374 #define FLOAT_SQRT(a) (spx_sqrt(a))
375
376 #endif
377
378 #endif