maths/fixc.c

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