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