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