maths/fixc.c

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