maths/fixc.c

   1 /* $Id: fixc.c,v 1.7 2004-08-28 23:17:45 schaffner Exp $ */
   2 /*
   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.
  13 */
  14
  15 /*
  16  *
  17  * C version of fixed point library
  18  *
  19  */
  20
  21 #ifdef HAVE_CONFIG_H
  22 #include <conf.h>
  23 #endif
  24
  25 #ifdef RCS
  26 static char rcsid[] = "$Id: fixc.c,v 1.7 2004-08-28 23:17:45 schaffner Exp $";
  27 #endif
  28
  29 #include <stdlib.h>
  30 #include <math.h>
  31
  32 #include "error.h"
  33 #include "maths.h"
  34
  35 #ifdef NO_FIX_INLINE
  36 #ifdef _MSC_VER
  37 #pragma message ("warning: FIX NOT INLINED")
  38 #else
  39 // #warning "FIX NOT INLINED"        fixc is now stable
  40 #endif
  41 #endif
  42
  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[];
  48
  49 //negate a quad
  50 void fixquadnegate(quadint *q)
  51 {
  52         q->low  = 0 - q->low;
  53         q->high = 0 - q->high - (q->low != 0);
  54 }
  55
  56 //multiply two ints & add 64-bit result to 64-bit sum
  57 void fixmulaccum(quadint *q,fix a,fix b)
  58 {
  59         u_int32_t aa,bb;
  60         u_int32_t ah,al,bh,bl;
  61         u_int32_t t,c=0,old;
  62         int neg;
  63
  64         neg = ((a^b) < 0);
  65
  66         aa = labs(a); bb = labs(b);
  67
  68         ah = aa>>16;  al = aa&0xffff;
  69         bh = bb>>16;  bl = bb&0xffff;
  70
  71         t = ah*bl + bh*al;
  72
  73         if (neg)
  74                 fixquadnegate(q);
  75
  76         old = q->low;
  77         q->low += al*bl;
  78         if (q->low < old) q->high++;
  79
  80         old = q->low;
  81         q->low += (t<<16);
  82         if (q->low < old) q->high++;
  83
  84         q->high += ah*bh + (t>>16) + c;
  85
  86         if (neg)
  87                 fixquadnegate(q);
  88
  89 }
  90
  91 //extract a fix from a quad product
  92 fix fixquadadjust(quadint *q)
  93 {
  94         return (q->high<<16) + (q->low>>16);
  95 }
  96
  97
  98 #define EPSILON (F1_0/100)
  99
 100 #ifdef _MSC_VER
 101 #define QLONG __int64
 102 #else
 103 #define QLONG long long
 104 #endif
 105
 106 #ifdef NO_FIX_INLINE
 107 fix fixmul(fix a, fix b) {
 108 /*        return (fix)(((double)a*(double)b)/65536.0);*/
 109 /*        register fix ret;
 110         asm("imul %%edx; shrd $16,%%edx,%%eax" : "=a" (ret) : "a" (a), "d" (b) : "%edx");
 111         return ret;                                 */
 112         return (fix)((((QLONG)a)*b) >> 16);
 113 }
 114
 115 fix fixdiv(fix a, fix b)
 116 {
 117 /*        return (fix)(((double)a * 65536.0) / (double)b);*/
 118         return (fix)((((QLONG)a) << 16)/b);
 119 /*        register fix ret;
 120         asm("mov %%eax,%%edx; sar $16,%%edx; shl $16,%%eax; idiv %%ebx" : "=a" (ret) : "a" (a), "b" (b) : "%edx");
 121     return ret; */
 122 }
 123
 124 fix fixmuldiv(fix a, fix b, fix c)
 125 {
 126 /*        register fix ret;
 127         asm("imul %%edx; idiv %%ebx" : "=a" (ret) : "a" (a), "d" (b), "b" (c) : "%edx");
 128     return ret;*/
 129
 130 /*        double d;
 131
 132         d = (double)a * (double) b;
 133         return (fix)(d / (double) c);
 134 */
 135         return (fix)((((QLONG)a)*b)/c);
 136 }
 137 #endif
 138
 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.
 144
 145 fixang fix_atan2(fix cos,fix sin)
 146 {
 147         double d, dsin, dcos;
 148         fixang t;
 149
 150         //Assert(!(cos==0 && sin==0));
 151
 152         //find smaller of two
 153
 154         dsin = (double)sin;
 155         dcos = (double)cos;
 156         d = sqrt((dsin * dsin) + (dcos * dcos));
 157
 158         if (d==0.0)
 159                 return 0;
 160
 161         if (labs(sin) < labs(cos)) {                            //sin is smaller, use arcsin
 162                 t = fix_asin((fix)((dsin / d) * 65536.0));
 163                 if (cos<0)
 164                         t = 0x8000 - t;
 165                 return t;
 166         }
 167         else {
 168                 t = fix_acos((fix)((dcos / d) * 65536.0));
 169                 if (sin<0)
 170                         t = -t;
 171                 return t;
 172         }
 173 }
 174
 175 #ifdef NO_FIX_INLINE
 176 //divide a quadint by a fix, returning a fix
 177 int32_t fixdivquadlong(u_int32_t nl,u_int32_t nh,u_int32_t d)
 178 {
 179         int i;
 180         u_int32_t tmp0;
 181         ubyte tmp1;
 182         u_int32_t r;
 183         ubyte T,Q,M;
 184
 185         r = 0;
 186
 187         Q = ((nh&0x80000000)!=0);
 188         M = ((d&0x80000000)!=0);
 189         T = (M!=Q);
 190
 191         if (M == 0)
 192         {
 193                 for (i=0; i<32; i++ )   {
 194
 195                         r <<= 1;
 196                         r |= T;
 197                         T = ((nl&0x80000000L)!=0);
 198                         nl <<= 1;
 199
 200                         switch( Q ) {
 201
 202                         case 0:
 203                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
 204                                 nh = (nh << 1) | (u_int32_t)T;
 205
 206                                 tmp0 = nh;
 207                                 nh -= d;
 208                                 tmp1 = (nh>tmp0);
 209                                 if (Q == 0)
 210                                         Q = tmp1;
 211                                 else
 212                                         Q = (unsigned char)(tmp1 == 0);
 213                                 break;
 214                         case 1:
 215                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
 216                                 nh = (nh << 1) | (u_int32_t)T;
 217
 218                                 tmp0 = nh;
 219                                 nh += d;
 220                                 tmp1 = (nh<tmp0);
 221                                 if (Q == 0)
 222                                         Q = tmp1;
 223                                 else
 224                                         Q = (unsigned char)(tmp1 == 0);
 225                                 break;
 226                         }
 227                         T = (Q==M);
 228                 }
 229         }
 230         else
 231         {
 232                 for (i=0; i<32; i++ )   {
 233
 234                         r <<= 1;
 235                         r |= T;
 236                         T = ((nl&0x80000000L)!=0);
 237                         nl <<= 1;
 238
 239                         switch( Q ) {
 240
 241                         case 0:
 242                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
 243                                 nh = (nh << 1) | (u_int32_t)T;
 244
 245                                 tmp0 = nh;
 246                                 nh += d;
 247                                 tmp1 = (nh<tmp0);
 248                                 if (Q == 1)
 249                                         Q = tmp1;
 250                                 else
 251                                         Q = (unsigned char)(tmp1 == 0);
 252                                 break;
 253                         case 1:
 254                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
 255                                 nh = (nh << 1) | (u_int32_t)T;
 256
 257                                 tmp0 = nh;
 258                                 nh = nh - d;
 259                                 tmp1 = (nh>tmp0);
 260                                 if (Q == 1)
 261                                         Q = tmp1;
 262                                 else
 263                                         Q = (unsigned char)(tmp1 == 0);
 264                                 break;
 265                         }
 266                         T = (Q==M);
 267                 }
 268         }
 269
 270         r = (r << 1) | T;
 271
 272         return r;
 273 }
 274
 275 #if 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)
 280 {
 281   return fixdivquadlong((u_int32_t) nl,(u_int32_t) nh,(u_int32_t) d);
 282 }
 283 #endif
 284
 285 unsigned int fixdivquadlongu(uint nl, uint nh, uint d)
 286 {
 287   u_int64_t n = (u_int64_t)nl | (((u_int64_t)nh) << 32 );
 288   return n / ((u_int64_t)d);
 289 }
 290
 291 #else //of ifdef NO_FIX_INLINE
 292 int32_t fixdivquadlong(u_int32_t nl,u_int32_t nh,u_int32_t d) {
 293 int32_t a;
 294 __asm__("idivl %3"
 295          :"=a" (a)
 296          :"a" (nl), "d" (nh), "r" (d)
 297          :"ax", "dx"
 298          );
 299 return (a);
 300 }
 301 static inline u_int32_t fixdivquadlongu(u_int32_t nl,u_int32_t nh,u_int32_t d) {
 302 u_int32_t a;
 303 __asm__("divl %3"
 304          :"=a" (a)
 305          :"a" (nl), "d" (nh), "r" (d)
 306          :"ax", "dx"
 307          );
 308 return (a);
 309 }
 310 #endif //def NO_FIX_INLINE
 311
 312 u_int32_t quad_sqrt(u_int32_t low,int32_t high)
 313 {
 314         int i, cnt;
 315         u_int32_t r,old_r,t;
 316         quadint tq;
 317
 318         if (high<0)
 319                 return 0;
 320
 321         if (high==0 && (int32_t)low>=0)
 322                 return long_sqrt((int32_t)low);
 323
 324         if (high & 0xff000000) {
 325                 cnt=12+16; i = high >> 24;
 326         } else if (high & 0xff0000) {
 327                 cnt=8+16; i = high >> 16;
 328         } else if (high & 0xff00) {
 329                 cnt=4+16; i = high >> 8;
 330         } else {
 331                 cnt=0+16; i = high;
 332         }
 333
 334         r = guess_table[i]<<cnt;
 335
 336         //quad loop usually executed 4 times
 337
 338         r = fixdivquadlongu(low,high,r)/2 + r/2;
 339         r = fixdivquadlongu(low,high,r)/2 + r/2;
 340         r = fixdivquadlongu(low,high,r)/2 + r/2;
 341
 342         do {
 343
 344                 old_r = r;
 345                 t = fixdivquadlongu(low,high,r);
 346
 347                 if (t==r)       //got it!
 348                         return r;
 349
 350                 r = t/2 + r/2;
 351
 352         } while (!(r==t || r==old_r));
 353
 354         t = fixdivquadlongu(low,high,r);
 355         //edited 05/17/99 Matt Mueller - tq.high is undefined here.. so set them to = 0
 356         tq.low=tq.high=0;
 357         //end edit -MM
 358         fixmulaccum(&tq,r,t);
 359         if (tq.low!=low || tq.high!=high)
 360                 r++;
 361
 362         return r;
 363 }
 364
 365 //computes the square root of a long, returning a short
 366 ushort long_sqrt(int32_t a)
 367 {
 368         int cnt,r,old_r,t;
 369
 370         if (a<=0)
 371                 return 0;
 372
 373         if (a & 0xff000000)
 374                 cnt=12;
 375         else if (a & 0xff0000)
 376                 cnt=8;
 377         else if (a & 0xff00)
 378                 cnt=4;
 379         else
 380                 cnt=0;
 381
 382         r = guess_table[(a>>cnt)&0xff]<<cnt;
 383
 384         //the loop nearly always executes 3 times, so we'll unroll it 2 times and
 385         //not do any checking until after the third time.  By my calcutations, the
 386         //loop is executed 2 times in 99.97% of cases, 3 times in 93.65% of cases,
 387         //four times in 16.18% of cases, and five times in 0.44% of cases.  It never
 388         //executes more than five times.  By timing, I determined that is is faster
 389         //to always execute three times and not check for termination the first two
 390         //times through.  This means that in 93.65% of cases, we save 6 cmp/jcc pairs,
 391         //and in 6.35% of cases we do an extra divide.  In real life, these numbers
 392         //might not be the same.
 393
 394         r = ((a/r)+r)/2;
 395         r = ((a/r)+r)/2;
 396
 397         do {
 398
 399                 old_r = r;
 400                 t = a/r;
 401
 402                 if (t==r)       //got it!
 403                         return r;
 404
 405                 r = (t+r)/2;
 406
 407         } while (!(r==t || r==old_r));
 408
 409         if (a % r)
 410                 r++;
 411
 412         return r;
 413 }
 414
 415 //computes the square root of a fix, returning a fix
 416 fix fix_sqrt(fix a)
 417 {
 418         return ((fix) long_sqrt(a)) << 8;
 419 }
 420
 421
 422 //compute sine and cosine of an angle, filling in the variables
 423 //either of the pointers can be NULL
 424 //with interpolation
 425 void fix_sincos(fix a,fix *s,fix *c)
 426 {
 427         int i,f;
 428         fix ss,cc;
 429
 430         i = (a>>8)&0xff;
 431         f = a&0xff;
 432
 433         ss = sincos_table[i];
 434         if (s) *s = (ss + (((sincos_table[i+1] - ss) * f)>>8))<<2;
 435
 436         cc = sincos_table[i+64];
 437         if (c) *c = (cc + (((sincos_table[i+64+1] - cc) * f)>>8))<<2;
 438 }
 439
 440 //compute sine and cosine of an angle, filling in the variables
 441 //either of the pointers can be NULL
 442 //no interpolation
 443 void fix_fastsincos(fix a,fix *s,fix *c)
 444 {
 445         int i;
 446
 447         i = (a>>8)&0xff;
 448
 449         if (s) *s = sincos_table[i] << 2;
 450         if (c) *c = sincos_table[i+64] << 2;
 451 }
 452
 453 //compute inverse sine
 454 fixang fix_asin(fix v)
 455 {
 456         fix vv;
 457         int i,f,aa;
 458
 459         vv = labs(v);
 460
 461         if (vv >= f1_0)         //check for out of range
 462                 return 0x4000;
 463
 464         i = (vv>>8)&0xff;
 465         f = vv&0xff;
 466
 467         aa = asin_table[i];
 468         aa = aa + (((asin_table[i+1] - aa) * f)>>8);
 469
 470         if (v < 0)
 471                 aa = -aa;
 472
 473         return aa;
 474 }
 475
 476 //compute inverse cosine
 477 fixang fix_acos(fix v)
 478 {
 479         fix vv;
 480         int i,f,aa;
 481
 482         vv = labs(v);
 483
 484         if (vv >= f1_0)         //check for out of range
 485                 return 0;
 486
 487         i = (vv>>8)&0xff;
 488         f = vv&0xff;
 489
 490         aa = acos_table[i];
 491         aa = aa + (((acos_table[i+1] - aa) * f)>>8);
 492
 493         if (v < 0)
 494                 aa = 0x8000 - aa;
 495
 496         return aa;
 497 }
 498
 499 #define TABLE_SIZE 1024
 500
 501 //for passed value a, returns 1/sqrt(a)
 502 fix fix_isqrt( fix a )
 503 {
 504         int i, b = a;
 505         int cnt = 0;
 506         int r;
 507
 508         if ( a == 0 ) return 0;
 509
 510         while( b >= TABLE_SIZE )        {
 511                 b >>= 1;
 512                 cnt++;
 513         }
 514
 515         //printf( "Count = %d (%d>>%d)\n", cnt, b, (cnt+1)/2 );
 516         r = isqrt_guess_table[b] >> ((cnt+1)/2);
 517
 518         //printf( "Initial r = %d\n", r );
 519
 520         for (i=0; i<3; i++ )    {
 521                 int old_r = r;
 522                 r = fixmul( ( (3*65536) - fixmul(fixmul(r,r),a) ), r) / 2;
 523                 //printf( "r %d  = %d\n", i, r );
 524                 if ( old_r >= r ) return (r+old_r)/2;
 525         }
 526
 527         return r;
 528 }