00001
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef PSEUDOFLOAT_H
00036 #define PSEUDOFLOAT_H
00037
00038 #include "misc.h"
00039 #include <math.h>
00040
00041 #ifdef FIXED_POINT
00042
00043 typedef struct {
00044 spx_int16_t m;
00045 spx_int16_t e;
00046 } spx_float_t;
00047
00048 #define FLOAT_ZERO ((spx_float_t){0,0})
00049 #define FLOAT_ONE ((spx_float_t){16384,-14})
00050 #define FLOAT_HALF ((spx_float_t){16384,-15})
00051
00052 #define MIN(a,b) ((a)<(b)?(a):(b))
00053 static inline spx_float_t PSEUDOFLOAT(spx_int32_t x)
00054 {
00055 int e=0;
00056 int sign=0;
00057 if (x<0)
00058 {
00059 sign = 1;
00060 x = -x;
00061 }
00062 if (x==0)
00063 return (spx_float_t) {0,0};
00064 while (x>32767)
00065 {
00066 x >>= 1;
00067
00068 e++;
00069 }
00070 while (x<16383)
00071 {
00072 x <<= 1;
00073
00074 e--;
00075 }
00076 if (sign)
00077 return (spx_float_t) {-x,e};
00078 else
00079 return (spx_float_t) {x,e};
00080 }
00081
00082
00083 static inline spx_float_t FLOAT_ADD(spx_float_t a, spx_float_t b)
00084 {
00085 spx_float_t r;
00086 if (a.m==0)
00087 return b;
00088 else if (b.m==0)
00089 return a;
00090 r = (a).e > (b).e ? (spx_float_t) {((a).m>>1) + ((b).m>>MIN(15,(a).e-(b).e+1)),(a).e+1} : (spx_float_t) {((b).m>>1) + ((a).m>>MIN(15,(b).e-(a).e+1)),(b).e+1};
00091 if (r.m>0)
00092 {
00093 if (r.m<16384)
00094 {
00095 r.m<<=1;
00096 r.e-=1;
00097 }
00098 } else {
00099 if (r.m>-16384)
00100 {
00101 r.m<<=1;
00102 r.e-=1;
00103 }
00104 }
00105
00106 return r;
00107 }
00108
00109 static inline spx_float_t FLOAT_SUB(spx_float_t a, spx_float_t b)
00110 {
00111 spx_float_t r;
00112 if (a.m==0)
00113 return b;
00114 else if (b.m==0)
00115 return a;
00116 r = (a).e > (b).e ? (spx_float_t) {((a).m>>1) - ((b).m>>MIN(15,(a).e-(b).e+1)),(a).e+1} : (spx_float_t) {((a).m>>MIN(15,(b).e-(a).e+1)) - ((b).m>>1) ,(b).e+1};
00117 if (r.m>0)
00118 {
00119 if (r.m<16384)
00120 {
00121 r.m<<=1;
00122 r.e-=1;
00123 }
00124 } else {
00125 if (r.m>-16384)
00126 {
00127 r.m<<=1;
00128 r.e-=1;
00129 }
00130 }
00131
00132 return r;
00133 }
00134
00135 static inline int FLOAT_LT(spx_float_t a, spx_float_t b)
00136 {
00137 if (a.m==0)
00138 return b.m<0;
00139 else if (b.m==0)
00140 return a.m>0;
00141 if ((a).e > (b).e)
00142 return ((a).m>>1) < ((b).m>>MIN(15,(a).e-(b).e+1));
00143 else
00144 return ((b).m>>1) > ((a).m>>MIN(15,(b).e-(a).e+1));
00145
00146 }
00147
00148 static inline int FLOAT_GT(spx_float_t a, spx_float_t b)
00149 {
00150 return FLOAT_LT(b,a);
00151 }
00152
00153 static inline spx_float_t FLOAT_MULT(spx_float_t a, spx_float_t b)
00154 {
00155 spx_float_t r = (spx_float_t) {(spx_int16_t)((spx_int32_t)(a).m*(b).m>>15), (a).e+(b).e+15};
00156 if (r.m>0)
00157 {
00158 if (r.m<16384)
00159 {
00160 r.m<<=1;
00161 r.e-=1;
00162 }
00163 } else {
00164 if (r.m>-16384)
00165 {
00166 r.m<<=1;
00167 r.e-=1;
00168 }
00169 }
00170
00171 return r;
00172 }
00173
00174
00175 static inline spx_float_t FLOAT_SHL(spx_float_t a, int b)
00176 {
00177 return (spx_float_t) {a.m,a.e+b};
00178 }
00179
00180 static inline spx_int16_t FLOAT_EXTRACT16(spx_float_t a)
00181 {
00182 if (a.e<0)
00183 return (a.m+(1<<(-a.e-1)))>>-a.e;
00184 else
00185 return a.m<<a.e;
00186 }
00187
00188 static inline spx_int32_t FLOAT_MUL32(spx_float_t a, spx_word32_t b)
00189 {
00190 if (a.e<-15)
00191 return SHR32(MULT16_32_Q15(a.m, b),-a.e-15);
00192 else
00193 return SHL32(MULT16_32_Q15(a.m, b),15+a.e);
00194 }
00195
00196 static inline spx_float_t FLOAT_MUL32U(spx_word32_t a, spx_word32_t b)
00197 {
00198 int e=0;
00199
00200 if (a==0)
00201 return (spx_float_t) {0,0};
00202 while (a>32767)
00203 {
00204 a >>= 1;
00205 e++;
00206 }
00207 while (a<16384)
00208 {
00209 a <<= 1;
00210 e--;
00211 }
00212 while (b>32767)
00213 {
00214 b >>= 1;
00215 e++;
00216 }
00217 while (b<16384)
00218 {
00219 b <<= 1;
00220 e--;
00221 }
00222 return (spx_float_t) {MULT16_16_Q15(a,b),e+15};
00223 }
00224
00225 static inline spx_float_t FLOAT_DIV32_FLOAT(spx_word32_t a, spx_float_t b)
00226 {
00227 int e=0;
00228
00229 if (a==0)
00230 return (spx_float_t) {0,0};
00231 while (a<SHL32(b.m,14))
00232 {
00233 a <<= 1;
00234 e--;
00235 }
00236 while (a>=SHL32(b.m-1,15))
00237 {
00238 a >>= 1;
00239 e++;
00240 }
00241 return (spx_float_t) {DIV32_16(a,b.m),e-b.e};
00242 }
00243
00244
00245 static inline spx_float_t FLOAT_DIV32(spx_word32_t a, spx_word32_t b)
00246 {
00247 int e=0;
00248
00249 if (a==0)
00250 return (spx_float_t) {0,0};
00251 while (b>32767)
00252 {
00253 b >>= 1;
00254 e--;
00255 }
00256 while (a<SHL32(b,14))
00257 {
00258 a <<= 1;
00259 e--;
00260 }
00261 while (a>=SHL32(b-1,15))
00262 {
00263 a >>= 1;
00264 e++;
00265 }
00266 return (spx_float_t) {DIV32_16(a,b),e};
00267 }
00268
00269 static inline spx_float_t FLOAT_DIVU(spx_float_t a, spx_float_t b)
00270 {
00271 int e=0;
00272 spx_int32_t num;
00273 num = a.m;
00274 while (a.m >= b.m)
00275 {
00276 e++;
00277 a.m >>= 1;
00278 }
00279 num = num << (15-e);
00280 return (spx_float_t) {DIV32_16(num,b.m),a.e-b.e-15+e};
00281 }
00282
00283 #else
00284
00285 #define spx_float_t float
00286 #define FLOAT_ZERO 0.f
00287 #define FLOAT_ONE 1.f
00288 #define FLOAT_HALF 0.5f
00289 #define PSEUDOFLOAT(x) (x)
00290 #define FLOAT_MULT(a,b) ((a)*(b))
00291 #define FLOAT_MUL32(a,b) ((a)*(b))
00292 #define FLOAT_DIV32(a,b) ((a)/(b))
00293 #define FLOAT_EXTRACT16(a) (a)
00294 #define FLOAT_ADD(a,b) ((a)+(b))
00295 #define FLOAT_SUB(a,b) ((a)-(b))
00296 #define REALFLOAT(x) (x)
00297 #define FLOAT_DIV32_FLOAT(a,b) ((a)/(b))
00298 #define FLOAT_MUL32U(a,b) ((a)*(b))
00299 #define FLOAT_SHL(a,b) (a)
00300 #define FLOAT_LT(a,b) ((a)<(b))
00301 #define FLOAT_GT(a,b) ((a)>(b))
00302 #define FLOAT_DIVU(a,b) ((a)/(b))
00303
00304 #endif
00305
00306 #endif