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