1 |
cebix |
1.1 |
/* |
2 |
cebix |
1.2 |
* FixPoint.h - Provides fixed point arithmetic (for use in SID.cpp) |
3 |
cebix |
1.1 |
* |
4 |
|
|
* (C) 1997 Andreas Dehmel |
5 |
|
|
* |
6 |
cebix |
1.4 |
* Frodo (C) 1994-1997,2002-2004 Christian Bauer |
7 |
cebix |
1.1 |
* |
8 |
|
|
* This program is free software; you can redistribute it and/or modify |
9 |
|
|
* it under the terms of the GNU General Public License as published by |
10 |
|
|
* the Free Software Foundation; either version 2 of the License, or |
11 |
|
|
* (at your option) any later version. |
12 |
|
|
* |
13 |
|
|
* This program is distributed in the hope that it will be useful, |
14 |
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
15 |
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
16 |
|
|
* GNU General Public License for more details. |
17 |
|
|
* |
18 |
|
|
* You should have received a copy of the GNU General Public License |
19 |
|
|
* along with this program; if not, write to the Free Software |
20 |
|
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
21 |
|
|
*/ |
22 |
|
|
|
23 |
|
|
/* |
24 |
|
|
* You need to define FIXPOINT_PREC (number of fractional bits) and |
25 |
|
|
* ldSINTAB (ld of the size of the sinus table) as well M_PI |
26 |
|
|
* _before_ including this file. |
27 |
|
|
* Requires at least 32bit ints! |
28 |
|
|
*/ |
29 |
|
|
|
30 |
|
|
|
31 |
|
|
#define FIXPOINT_BITS 32 |
32 |
|
|
// Sign-bit |
33 |
|
|
#define FIXPOINT_SIGN (1<<(FIXPOINT_BITS-1)) |
34 |
|
|
|
35 |
|
|
|
36 |
|
|
/* |
37 |
|
|
* Elementary functions for the FixPoint class |
38 |
|
|
*/ |
39 |
|
|
|
40 |
|
|
// Multiplies two fixpoint numbers, result is a fixpoint number. |
41 |
|
|
static inline int fixmult(int x, int y) |
42 |
|
|
{ |
43 |
|
|
register unsigned int a,b; |
44 |
|
|
register bool sign; |
45 |
|
|
|
46 |
|
|
sign = (x ^ y) < 0; |
47 |
|
|
if (x < 0) {x = -x;} |
48 |
|
|
if (y < 0) {y = -y;} |
49 |
|
|
// a, b : integer part; x, y : fractional part. All unsigned now (for shift right)!!! |
50 |
|
|
a = (((unsigned int)x) >> FIXPOINT_PREC); x &= ~(a << FIXPOINT_PREC); |
51 |
|
|
b = (((unsigned int)y) >> FIXPOINT_PREC); y &= ~(b << FIXPOINT_PREC); |
52 |
|
|
x = ((a*b) << FIXPOINT_PREC) + (a*y + b*x) + |
53 |
|
|
((unsigned int)((x*y) + (1 << (FIXPOINT_PREC-1))) >> FIXPOINT_PREC); |
54 |
|
|
#ifdef FIXPOINT_SIGN |
55 |
|
|
if (x < 0) {x ^= FIXPOINT_SIGN;} |
56 |
|
|
#endif |
57 |
|
|
if (sign) {x = -x;} |
58 |
|
|
return(x); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
|
62 |
|
|
// Multiplies a fixpoint number with an integer, result is a 32 bit (!) integer in |
63 |
|
|
// contrast to using the standard member-functions which can provide only (32-FIXPOINT_PREC) |
64 |
|
|
// valid bits. |
65 |
|
|
static inline int intmult(int x, int y) // x is fixpoint, y integer |
66 |
|
|
{ |
67 |
|
|
register unsigned int i,j; |
68 |
|
|
register bool sign; |
69 |
|
|
|
70 |
|
|
sign = (x ^ y) < 0; |
71 |
|
|
if (x < 0) {x = -x;} |
72 |
|
|
if (y < 0) {y = -y;} |
73 |
|
|
i = (((unsigned int)x) >> 16); x &= ~(i << 16); // split both into 16.16 parts |
74 |
|
|
j = (((unsigned int)y) >> 16); y &= ~(j << 16); |
75 |
|
|
#if FIXPOINT_PREC <= 16 |
76 |
|
|
// This '32' is independent of the number of bits used, it's due to the 16 bit shift |
77 |
|
|
i = ((i*j) << (32 - FIXPOINT_PREC)) + ((i*y + j*x) << (16 - FIXPOINT_PREC)) + |
78 |
|
|
((unsigned int)(x*y + (1 << (FIXPOINT_PREC - 1))) >> FIXPOINT_PREC); |
79 |
|
|
#else |
80 |
|
|
{ |
81 |
|
|
register unsigned int h; |
82 |
|
|
|
83 |
|
|
h = (i*y + j*x); |
84 |
|
|
i = ((i*j) << (32 - FIXPOINT_PREC)) + (h >> (FIXPOINT_PREC - 16)); |
85 |
|
|
h &= ((1 << (FIXPOINT_PREC - 16)) - 1); x *= y; |
86 |
|
|
i += (x >> FIXPOINT_PREC); x &= ((1 << FIXPOINT_PREC) - 1); |
87 |
|
|
i += (((h + (x >> 16)) + (1 << (FIXPOINT_PREC - 17))) >> (FIXPOINT_PREC - 16)); |
88 |
|
|
} |
89 |
|
|
#endif |
90 |
|
|
#ifdef FIXPOINT_SIGN |
91 |
|
|
if (i < 0) {i ^= FIXPOINT_SIGN;} |
92 |
|
|
#endif |
93 |
|
|
if (sign) {i = -i;} |
94 |
|
|
return(i); |
95 |
|
|
} |
96 |
|
|
|
97 |
|
|
|
98 |
|
|
// Computes the product of a fixpoint number with itself. |
99 |
|
|
static inline int fixsquare(int x) |
100 |
|
|
{ |
101 |
|
|
register unsigned int a; |
102 |
|
|
|
103 |
|
|
if (x < 0) {x = -x;} |
104 |
|
|
a = (((unsigned int)x) >> FIXPOINT_PREC); x &= ~(a << FIXPOINT_PREC); |
105 |
|
|
x = ((a*a) << FIXPOINT_PREC) + ((a*x) << 1) + |
106 |
|
|
((unsigned int)((x*x) + (1 << (FIXPOINT_PREC-1))) >> FIXPOINT_PREC); |
107 |
|
|
#ifdef FIXPOINT_SIGN |
108 |
|
|
if (x < 0) {x ^= FIXPOINT_SIGN;} |
109 |
|
|
#endif |
110 |
|
|
return(x); |
111 |
|
|
} |
112 |
|
|
|
113 |
|
|
|
114 |
|
|
// Computes the square root of a fixpoint number. |
115 |
|
|
static inline int fixsqrt(int x) |
116 |
|
|
{ |
117 |
|
|
register int test, step; |
118 |
|
|
|
119 |
|
|
if (x < 0) return(-1); if (x == 0) return(0); |
120 |
|
|
step = (x <= (1<<FIXPOINT_PREC)) ? (1<<FIXPOINT_PREC) : (1<<((FIXPOINT_BITS - 2 + FIXPOINT_PREC)>>1)); |
121 |
|
|
test = 0; |
122 |
|
|
while (step != 0) |
123 |
|
|
{ |
124 |
|
|
register int h; |
125 |
|
|
|
126 |
|
|
h = fixsquare(test + step); |
127 |
|
|
if (h <= x) {test += step;} |
128 |
|
|
if (h == x) break; |
129 |
|
|
step >>= 1; |
130 |
|
|
} |
131 |
|
|
return(test); |
132 |
|
|
} |
133 |
|
|
|
134 |
|
|
|
135 |
|
|
// Divides a fixpoint number by another fixpoint number, yielding a fixpoint result. |
136 |
|
|
static inline int fixdiv(int x, int y) |
137 |
|
|
{ |
138 |
|
|
register int res, mask; |
139 |
|
|
register bool sign; |
140 |
|
|
|
141 |
|
|
sign = (x ^ y) < 0; |
142 |
|
|
if (x < 0) {x = -x;} |
143 |
|
|
if (y < 0) {y = -y;} |
144 |
|
|
mask = (1<<FIXPOINT_PREC); res = 0; |
145 |
|
|
while (x > y) {y <<= 1; mask <<= 1;} |
146 |
|
|
while (mask != 0) |
147 |
|
|
{ |
148 |
|
|
if (x >= y) {res |= mask; x -= y;} |
149 |
|
|
mask >>= 1; y >>= 1; |
150 |
|
|
} |
151 |
|
|
#ifdef FIXPOINT_SIGN |
152 |
|
|
if (res < 0) {res ^= FIXPOINT_SIGN;} |
153 |
|
|
#endif |
154 |
|
|
if (sign) {res = -res;} |
155 |
|
|
return(res); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
|
|
159 |
|
|
|
160 |
|
|
|
161 |
|
|
|
162 |
|
|
/* |
163 |
|
|
* The C++ Fixpoint class. By no means exhaustive... |
164 |
|
|
* Since it contains only one int data, variables of type FixPoint can be |
165 |
|
|
* passed directly rather than as a reference. |
166 |
|
|
*/ |
167 |
|
|
|
168 |
|
|
class FixPoint |
169 |
|
|
{ |
170 |
|
|
private: |
171 |
|
|
int x; |
172 |
|
|
|
173 |
|
|
public: |
174 |
|
|
FixPoint(void); |
175 |
|
|
FixPoint(int y); |
176 |
|
|
~FixPoint(void); |
177 |
|
|
|
178 |
|
|
// conversions |
179 |
|
|
int Value(void); |
180 |
|
|
int round(void); |
181 |
|
|
operator int(void); |
182 |
|
|
|
183 |
|
|
// unary operators |
184 |
|
|
FixPoint sqrt(void); |
185 |
|
|
FixPoint sqr(void); |
186 |
|
|
FixPoint abs(void); |
187 |
|
|
FixPoint operator+(void); |
188 |
|
|
FixPoint operator-(void); |
189 |
|
|
FixPoint operator++(void); |
190 |
|
|
FixPoint operator--(void); |
191 |
|
|
|
192 |
|
|
// binary operators |
193 |
|
|
int imul(int y); |
194 |
|
|
FixPoint operator=(FixPoint y); |
195 |
|
|
FixPoint operator=(int y); |
196 |
|
|
FixPoint operator+(FixPoint y); |
197 |
|
|
FixPoint operator+(int y); |
198 |
|
|
FixPoint operator-(FixPoint y); |
199 |
|
|
FixPoint operator-(int y); |
200 |
|
|
FixPoint operator/(FixPoint y); |
201 |
|
|
FixPoint operator/(int y); |
202 |
|
|
FixPoint operator*(FixPoint y); |
203 |
|
|
FixPoint operator*(int y); |
204 |
|
|
FixPoint operator+=(FixPoint y); |
205 |
|
|
FixPoint operator+=(int y); |
206 |
|
|
FixPoint operator-=(FixPoint y); |
207 |
|
|
FixPoint operator-=(int y); |
208 |
|
|
FixPoint operator*=(FixPoint y); |
209 |
|
|
FixPoint operator*=(int y); |
210 |
|
|
FixPoint operator/=(FixPoint y); |
211 |
|
|
FixPoint operator/=(int y); |
212 |
|
|
FixPoint operator<<(int y); |
213 |
|
|
FixPoint operator>>(int y); |
214 |
|
|
FixPoint operator<<=(int y); |
215 |
|
|
FixPoint operator>>=(int y); |
216 |
|
|
|
217 |
|
|
// conditional operators |
218 |
|
|
bool operator<(FixPoint y); |
219 |
|
|
bool operator<(int y); |
220 |
|
|
bool operator<=(FixPoint y); |
221 |
|
|
bool operator<=(int y); |
222 |
|
|
bool operator>(FixPoint y); |
223 |
|
|
bool operator>(int y); |
224 |
|
|
bool operator>=(FixPoint y); |
225 |
|
|
bool operator>=(int y); |
226 |
|
|
bool operator==(FixPoint y); |
227 |
|
|
bool operator==(int y); |
228 |
|
|
bool operator!=(FixPoint y); |
229 |
|
|
bool operator!=(int y); |
230 |
|
|
}; |
231 |
|
|
|
232 |
|
|
|
233 |
|
|
/* |
234 |
|
|
* int gets treated differently according to the case: |
235 |
|
|
* |
236 |
|
|
* a) Equations (=) or condition checks (==, <, <= ...): raw int (i.e. no conversion) |
237 |
|
|
* b) As an argument for an arithmetic operation: conversion to fixpoint by shifting |
238 |
|
|
* |
239 |
|
|
* Otherwise loading meaningful values into FixPoint variables would be very awkward. |
240 |
|
|
*/ |
241 |
|
|
|
242 |
|
|
FixPoint::FixPoint(void) {x = 0;} |
243 |
|
|
|
244 |
|
|
FixPoint::FixPoint(int y) {x = y;} |
245 |
|
|
|
246 |
|
|
FixPoint::~FixPoint(void) {;} |
247 |
|
|
|
248 |
|
|
inline int FixPoint::Value(void) {return(x);} |
249 |
|
|
|
250 |
|
|
inline int FixPoint::round(void) {return((x + (1 << (FIXPOINT_PREC-1))) >> FIXPOINT_PREC);} |
251 |
|
|
|
252 |
|
|
inline FixPoint::operator int(void) {return(x);} |
253 |
|
|
|
254 |
|
|
|
255 |
|
|
// unary operators |
256 |
|
|
inline FixPoint FixPoint::sqrt(void) {return(fixsqrt(x));} |
257 |
|
|
|
258 |
|
|
inline FixPoint FixPoint::sqr(void) {return(fixsquare(x));} |
259 |
|
|
|
260 |
|
|
inline FixPoint FixPoint::abs(void) {return((x < 0) ? -x : x);} |
261 |
|
|
|
262 |
|
|
inline FixPoint FixPoint::operator+(void) {return(x);} |
263 |
|
|
|
264 |
|
|
inline FixPoint FixPoint::operator-(void) {return(-x);} |
265 |
|
|
|
266 |
|
|
inline FixPoint FixPoint::operator++(void) {x += (1 << FIXPOINT_PREC); return x;} |
267 |
|
|
|
268 |
|
|
inline FixPoint FixPoint::operator--(void) {x -= (1 << FIXPOINT_PREC); return x;} |
269 |
|
|
|
270 |
|
|
|
271 |
|
|
// binary operators |
272 |
|
|
inline int FixPoint::imul(int y) {return(intmult(x,y));} |
273 |
|
|
|
274 |
|
|
inline FixPoint FixPoint::operator=(FixPoint y) {x = y.Value(); return x;} |
275 |
|
|
|
276 |
|
|
inline FixPoint FixPoint::operator=(int y) {x = y; return x;} |
277 |
|
|
|
278 |
|
|
inline FixPoint FixPoint::operator+(FixPoint y) {return(x + y.Value());} |
279 |
|
|
|
280 |
|
|
inline FixPoint FixPoint::operator+(int y) {return(x + (y << FIXPOINT_PREC));} |
281 |
|
|
|
282 |
|
|
inline FixPoint FixPoint::operator-(FixPoint y) {return(x - y.Value());} |
283 |
|
|
|
284 |
|
|
inline FixPoint FixPoint::operator-(int y) {return(x - (y << FIXPOINT_PREC));} |
285 |
|
|
|
286 |
|
|
inline FixPoint FixPoint::operator/(FixPoint y) {return(fixdiv(x,y.Value()));} |
287 |
|
|
|
288 |
|
|
inline FixPoint FixPoint::operator/(int y) {return(x/y);} |
289 |
|
|
|
290 |
|
|
inline FixPoint FixPoint::operator*(FixPoint y) {return(fixmult(x,y.Value()));} |
291 |
|
|
|
292 |
|
|
inline FixPoint FixPoint::operator*(int y) {return(x*y);} |
293 |
|
|
|
294 |
|
|
inline FixPoint FixPoint::operator+=(FixPoint y) {x += y.Value(); return x;} |
295 |
|
|
|
296 |
|
|
inline FixPoint FixPoint::operator+=(int y) {x += (y << FIXPOINT_PREC); return x;} |
297 |
|
|
|
298 |
|
|
inline FixPoint FixPoint::operator-=(FixPoint y) {x -= y.Value(); return x;} |
299 |
|
|
|
300 |
|
|
inline FixPoint FixPoint::operator-=(int y) {x -= (y << FIXPOINT_PREC); return x;} |
301 |
|
|
|
302 |
|
|
inline FixPoint FixPoint::operator*=(FixPoint y) {x = fixmult(x,y.Value()); return x;} |
303 |
|
|
|
304 |
|
|
inline FixPoint FixPoint::operator*=(int y) {x *= y; return x;} |
305 |
|
|
|
306 |
|
|
inline FixPoint FixPoint::operator/=(FixPoint y) {x = fixdiv(x,y.Value()); return x;} |
307 |
|
|
|
308 |
|
|
inline FixPoint FixPoint::operator/=(int y) {x /= y; return x;} |
309 |
|
|
|
310 |
|
|
inline FixPoint FixPoint::operator<<(int y) {return(x << y);} |
311 |
|
|
|
312 |
|
|
inline FixPoint FixPoint::operator>>(int y) {return(x >> y);} |
313 |
|
|
|
314 |
|
|
inline FixPoint FixPoint::operator<<=(int y) {x <<= y; return x;} |
315 |
|
|
|
316 |
|
|
inline FixPoint FixPoint::operator>>=(int y) {x >>= y; return x;} |
317 |
|
|
|
318 |
|
|
|
319 |
|
|
// conditional operators |
320 |
|
|
inline bool FixPoint::operator<(FixPoint y) {return(x < y.Value());} |
321 |
|
|
|
322 |
|
|
inline bool FixPoint::operator<(int y) {return(x < y);} |
323 |
|
|
|
324 |
|
|
inline bool FixPoint::operator<=(FixPoint y) {return(x <= y.Value());} |
325 |
|
|
|
326 |
|
|
inline bool FixPoint::operator<=(int y) {return(x <= y);} |
327 |
|
|
|
328 |
|
|
inline bool FixPoint::operator>(FixPoint y) {return(x > y.Value());} |
329 |
|
|
|
330 |
|
|
inline bool FixPoint::operator>(int y) {return(x > y);} |
331 |
|
|
|
332 |
|
|
inline bool FixPoint::operator>=(FixPoint y) {return(x >= y.Value());} |
333 |
|
|
|
334 |
|
|
inline bool FixPoint::operator>=(int y) {return(x >= y);} |
335 |
|
|
|
336 |
|
|
inline bool FixPoint::operator==(FixPoint y) {return(x == y.Value());} |
337 |
|
|
|
338 |
|
|
inline bool FixPoint::operator==(int y) {return(x == y);} |
339 |
|
|
|
340 |
|
|
inline bool FixPoint::operator!=(FixPoint y) {return(x != y.Value());} |
341 |
|
|
|
342 |
|
|
inline bool FixPoint::operator!=(int y) {return(x != y);} |
343 |
|
|
|
344 |
|
|
|
345 |
|
|
|
346 |
|
|
/* |
347 |
|
|
* In case the first argument is an int (i.e. member-operators not applicable): |
348 |
|
|
* Not supported: things like int/FixPoint. The same difference in conversions |
349 |
|
|
* applies as mentioned above. |
350 |
|
|
*/ |
351 |
|
|
|
352 |
|
|
|
353 |
|
|
// binary operators |
354 |
|
|
inline FixPoint operator+(int x, FixPoint y) {return((x << FIXPOINT_PREC) + y.Value());} |
355 |
|
|
|
356 |
|
|
inline FixPoint operator-(int x, FixPoint y) {return((x << FIXPOINT_PREC) - y.Value());} |
357 |
|
|
|
358 |
|
|
inline FixPoint operator*(int x, FixPoint y) {return(x*y.Value());} |
359 |
|
|
|
360 |
|
|
|
361 |
|
|
// conditional operators |
362 |
|
|
inline bool operator==(int x, FixPoint y) {return(x == y.Value());} |
363 |
|
|
|
364 |
|
|
inline bool operator!=(int x, FixPoint y) {return(x != y.Value());} |
365 |
|
|
|
366 |
|
|
inline bool operator<(int x, FixPoint y) {return(x < y.Value());} |
367 |
|
|
|
368 |
|
|
inline bool operator<=(int x, FixPoint y) {return(x <= y.Value());} |
369 |
|
|
|
370 |
|
|
inline bool operator>(int x, FixPoint y) {return(x > y.Value());} |
371 |
|
|
|
372 |
|
|
inline bool operator>=(int x, FixPoint y) {return(x >= y.Value());} |
373 |
|
|
|
374 |
|
|
|
375 |
|
|
|
376 |
|
|
/* |
377 |
|
|
* For more convenient creation of constant fixpoint numbers from constant floats. |
378 |
|
|
*/ |
379 |
|
|
|
380 |
|
|
#define FixNo(n) (FixPoint)((int)(n*(1<<FIXPOINT_PREC))) |
381 |
|
|
|
382 |
|
|
|
383 |
|
|
|
384 |
|
|
|
385 |
|
|
|
386 |
|
|
|
387 |
|
|
/* |
388 |
|
|
* Stuff re. the sinus table used with fixpoint arithmetic |
389 |
|
|
*/ |
390 |
|
|
|
391 |
|
|
|
392 |
|
|
// define as global variable |
393 |
|
|
FixPoint SinTable[(1<<ldSINTAB)]; |
394 |
|
|
|
395 |
|
|
|
396 |
|
|
#define FIXPOINT_SIN_COS_GENERIC \ |
397 |
|
|
if (angle >= 3*(1<<ldSINTAB)) {return(-SinTable[(1<<(ldSINTAB+2)) - angle]);}\ |
398 |
|
|
if (angle >= 2*(1<<ldSINTAB)) {return(-SinTable[angle - 2*(1<<ldSINTAB)]);}\ |
399 |
|
|
if (angle >= (1<<ldSINTAB)) {return(SinTable[2*(1<<ldSINTAB) - angle]);}\ |
400 |
|
|
return(SinTable[angle]); |
401 |
|
|
|
402 |
|
|
|
403 |
|
|
// sin and cos: angle is fixpoint number 0 <= angle <= 2 (*PI) |
404 |
|
|
static inline FixPoint fixsin(FixPoint x) |
405 |
|
|
{ |
406 |
|
|
int angle = x; |
407 |
|
|
|
408 |
|
|
angle = (angle >> (FIXPOINT_PREC - ldSINTAB - 1)) & ((1<<(ldSINTAB+2))-1); |
409 |
|
|
FIXPOINT_SIN_COS_GENERIC |
410 |
|
|
} |
411 |
|
|
|
412 |
|
|
|
413 |
|
|
static inline FixPoint fixcos(FixPoint x) |
414 |
|
|
{ |
415 |
|
|
int angle = x; |
416 |
|
|
|
417 |
|
|
// cos(x) = sin(x+PI/2) |
418 |
|
|
angle = (angle + (1<<(FIXPOINT_PREC-1)) >> (FIXPOINT_PREC - ldSINTAB - 1)) & ((1<<(ldSINTAB+2))-1); |
419 |
|
|
FIXPOINT_SIN_COS_GENERIC |
420 |
|
|
} |
421 |
|
|
|
422 |
|
|
|
423 |
|
|
|
424 |
|
|
static inline void InitFixSinTab(void) |
425 |
|
|
{ |
426 |
|
|
int i; |
427 |
|
|
float step; |
428 |
|
|
|
429 |
|
|
for (i=0, step=0; i<(1<<ldSINTAB); i++, step+=0.5/(1<<ldSINTAB)) |
430 |
|
|
{ |
431 |
|
|
SinTable[i] = FixNo(sin(M_PI * step)); |
432 |
|
|
} |
433 |
|
|
} |