1 /* $Id: fixc.c,v 1.7 2004-08-28 23:17:45 schaffner Exp $ */
3 THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX
4 SOFTWARE CORPORATION ("PARALLAX"). PARALLAX, IN DISTRIBUTING THE CODE TO
5 END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A
6 ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS
7 IN USING, DISPLAYING, AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS
8 SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE
9 FREE PURPOSES. IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE
10 CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES. THE END-USER UNDERSTANDS
11 AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE.
12 COPYRIGHT 1993-1998 PARALLAX SOFTWARE CORPORATION. ALL RIGHTS RESERVED.
17 * C version of fixed point library
26 static char rcsid[] = "$Id: fixc.c,v 1.7 2004-08-28 23:17:45 schaffner Exp $";
37 #pragma message ("warning: FIX NOT INLINED")
39 // #warning "FIX NOT INLINED" fixc is now stable
43 extern ubyte guess_table[];
44 extern short sincos_table[];
45 extern ushort asin_table[];
46 extern ushort acos_table[];
47 extern fix isqrt_guess_table[];
50 void fixquadnegate(quadint *q)
53 q->high = 0 - q->high - (q->low != 0);
56 //multiply two ints & add 64-bit result to 64-bit sum
57 void fixmulaccum(quadint *q,fix a,fix b)
60 uint32_t ah, al, bh, bl;
61 uint32_t t, c = 0, old;
66 aa = labs(a); bb = labs(b);
68 ah = aa>>16; al = aa&0xffff;
69 bh = bb>>16; bl = bb&0xffff;
78 if (q->low < old) q->high++;
82 if (q->low < old) q->high++;
84 q->high += ah*bh + (t>>16) + c;
91 //extract a fix from a quad product
92 fix fixquadadjust(quadint *q)
94 return (q->high<<16) + (q->low>>16);
98 #define EPSILON (F1_0/100)
101 #define QLONG __int64
103 #define QLONG long long
107 fix fixmul(fix a, fix b) {
108 /* return (fix)(((double)a*(double)b)/65536.0);*/
110 asm("imul %%edx; shrd $16,%%edx,%%eax" : "=a" (ret) : "a" (a), "d" (b) : "%edx");
112 return (fix)((((QLONG)a)*b) >> 16);
115 fix fixdiv(fix a, fix b)
117 /* return (fix)(((double)a * 65536.0) / (double)b);*/
118 return (fix)((((QLONG)a) << 16)/b);
120 asm("mov %%eax,%%edx; sar $16,%%edx; shl $16,%%eax; idiv %%ebx" : "=a" (ret) : "a" (a), "b" (b) : "%edx");
124 fix fixmuldiv(fix a, fix b, fix c)
127 asm("imul %%edx; idiv %%ebx" : "=a" (ret) : "a" (a), "d" (b), "b" (c) : "%edx");
132 d = (double)a * (double) b;
133 return (fix)(d / (double) c);
135 return (fix)((((QLONG)a)*b)/c);
139 //given cos & sin of an angle, return that angle.
140 //parms need not be normalized, that is, the ratio of the parms cos/sin must
141 //equal the ratio of the actual cos & sin for the result angle, but the parms
142 //need not be the actual cos & sin.
143 //NOTE: this is different from the standard C atan2, since it is left-handed.
145 fixang fix_atan2(fix cos,fix sin)
147 double d, dsin, dcos;
150 //Assert(!(cos==0 && sin==0));
152 //find smaller of two
156 d = sqrt((dsin * dsin) + (dcos * dcos));
161 if (labs(sin) < labs(cos)) { //sin is smaller, use arcsin
162 t = fix_asin((fix)((dsin / d) * 65536.0));
168 t = fix_acos((fix)((dcos / d) * 65536.0));
176 //divide a quadint by a fix, returning a fix
177 int32_t fixdivquadlong(uint32_t nl, uint32_t nh, uint32_t d)
187 Q = ((nh&0x80000000)!=0);
188 M = ((d&0x80000000)!=0);
193 for (i=0; i<32; i++ ) {
197 T = ((nl&0x80000000L)!=0);
203 Q = (unsigned char)((0x80000000L & nh) != 0 );
204 nh = (nh << 1) | (uint32_t)T;
212 Q = (unsigned char)(tmp1 == 0);
215 Q = (unsigned char)((0x80000000L & nh) != 0 );
216 nh = (nh << 1) | (uint32_t)T;
224 Q = (unsigned char)(tmp1 == 0);
232 for (i=0; i<32; i++ ) {
236 T = ((nl&0x80000000L)!=0);
242 Q = (unsigned char)((0x80000000L & nh) != 0 );
243 nh = (nh << 1) | (uint32_t)T;
251 Q = (unsigned char)(tmp1 == 0);
254 Q = (unsigned char)((0x80000000L & nh) != 0 );
255 nh = (nh << 1) | (uint32_t)T;
263 Q = (unsigned char)(tmp1 == 0);
276 // this version caused inf loop with:
277 // quad_sqrt(0x27eb7121/*low=669741345*/,
278 // 0x4cd40ad8/*high=1288964824*/);
279 unsigned int fixdivquadlongu(uint nl, uint nh, uint d)
281 return fixdivquadlong((uint32_t) nl,(uint32_t) nh,(uint32_t) d);
285 unsigned int fixdivquadlongu(uint nl, uint nh, uint d)
287 uint64_t n = (uint64_t)nl | (((uint64_t)nh) << 32 );
288 return n / ((uint64_t)d);
291 #else //of ifdef NO_FIX_INLINE
292 int32_t fixdivquadlong(uint32_t nl, uint32_t nh, uint32_t d)
297 :"a" (nl), "d" (nh), "r" (d)
302 static inline uint32_t fixdivquadlongu(uint32_t nl, uint32_t nh, uint32_t d)
307 :"a" (nl), "d" (nh), "r" (d)
312 #endif //def NO_FIX_INLINE
314 uint32_t quad_sqrt(uint32_t low, int32_t high)
317 uint32_t r, old_r, t;
323 if (high==0 && (int32_t)low>=0)
324 return long_sqrt((int32_t)low);
326 if (high & 0xff000000) {
327 cnt=12+16; i = high >> 24;
328 } else if (high & 0xff0000) {
329 cnt=8+16; i = high >> 16;
330 } else if (high & 0xff00) {
331 cnt=4+16; i = high >> 8;
336 r = guess_table[i]<<cnt;
338 //quad loop usually executed 4 times
340 r = fixdivquadlongu(low,high,r)/2 + r/2;
341 r = fixdivquadlongu(low,high,r)/2 + r/2;
342 r = fixdivquadlongu(low,high,r)/2 + r/2;
347 t = fixdivquadlongu(low,high,r);
354 } while (!(r==t || r==old_r));
356 t = fixdivquadlongu(low,high,r);
357 //edited 05/17/99 Matt Mueller - tq.high is undefined here.. so set them to = 0
360 fixmulaccum(&tq,r,t);
361 if (tq.low!=low || tq.high!=high)
367 //computes the square root of a long, returning a short
368 ushort long_sqrt(int32_t a)
377 else if (a & 0xff0000)
384 r = guess_table[(a>>cnt)&0xff]<<cnt;
386 //the loop nearly always executes 3 times, so we'll unroll it 2 times and
387 //not do any checking until after the third time. By my calcutations, the
388 //loop is executed 2 times in 99.97% of cases, 3 times in 93.65% of cases,
389 //four times in 16.18% of cases, and five times in 0.44% of cases. It never
390 //executes more than five times. By timing, I determined that is is faster
391 //to always execute three times and not check for termination the first two
392 //times through. This means that in 93.65% of cases, we save 6 cmp/jcc pairs,
393 //and in 6.35% of cases we do an extra divide. In real life, these numbers
394 //might not be the same.
409 } while (!(r==t || r==old_r));
417 //computes the square root of a fix, returning a fix
420 return ((fix) long_sqrt(a)) << 8;
424 //compute sine and cosine of an angle, filling in the variables
425 //either of the pointers can be NULL
427 void fix_sincos(fix a,fix *s,fix *c)
435 ss = sincos_table[i];
436 if (s) *s = (ss + (((sincos_table[i+1] - ss) * f)>>8))<<2;
438 cc = sincos_table[i+64];
439 if (c) *c = (cc + (((sincos_table[i+64+1] - cc) * f)>>8))<<2;
442 //compute sine and cosine of an angle, filling in the variables
443 //either of the pointers can be NULL
445 void fix_fastsincos(fix a,fix *s,fix *c)
451 if (s) *s = sincos_table[i] << 2;
452 if (c) *c = sincos_table[i+64] << 2;
455 //compute inverse sine
456 fixang fix_asin(fix v)
463 if (vv >= f1_0) //check for out of range
470 aa = aa + (((asin_table[i+1] - aa) * f)>>8);
478 //compute inverse cosine
479 fixang fix_acos(fix v)
486 if (vv >= f1_0) //check for out of range
493 aa = aa + (((acos_table[i+1] - aa) * f)>>8);
501 #define TABLE_SIZE 1024
503 //for passed value a, returns 1/sqrt(a)
504 fix fix_isqrt( fix a )
510 if ( a == 0 ) return 0;
512 while( b >= TABLE_SIZE ) {
517 //printf( "Count = %d (%d>>%d)\n", cnt, b, (cnt+1)/2 );
518 r = isqrt_guess_table[b] >> ((cnt+1)/2);
520 //printf( "Initial r = %d\n", r );
522 for (i=0; i<3; i++ ) {
524 r = fixmul( ( (3*65536) - fixmul(fixmul(r,r),a) ), r) / 2;
525 //printf( "r %d = %d\n", i, r );
526 if ( old_r >= r ) return (r+old_r)/2;