math.c

在ADS环境下MiniGUI的源码

   if (x >= 0) {           /* search the first part of tan table */
      a = 0;
      b = 127;
   }
   else {                  /* search the second half instead */
      a = 128;
      b = 255;
   } 

   do {
      c = (a + b) >> 1;
      d = x - _tan_tbl[c];

      if (d > 0)
	 a = c + 1;
      else
	 if (d < 0)
	    b = c - 1;

   } while ((a <= b) && (d));

   if (x >= 0)
      return ((long)c) << 15;

   return (-0x00800000L + (((long)c) << 15));
}



/* fatan2:
 *  Like the libc atan2, but for fixed point numbers.
 */
fixed fatan2(fixed y, fixed x)
{
   fixed r;

   if (x==0) {
      if (y==0) {
	 errno = EDOM;
	 return 0L;
      }
      else
	 return ((y < 0) ? -0x00400000L : 0x00400000L);
   } 

   errno = 0;
   r = fdiv(y, x);

   if (errno) {
      errno = 0;
      return ((y < 0) ? -0x00400000L : 0x00400000L);
   }

   r = fatan(r);

   if (x >= 0)
      return r;

   if (y >= 0)
      return 0x00800000L + r;

   return r - 0x00800000L;
}

static unsigned short _sqrt_tabl[256] =
{
   /* this table is used by the fsqrt() and fhypot() routines in math.s */

   0x2D4,   0x103F,  0x16CD,  0x1BDB,  0x201F,  0x23E3,  0x274B,  0x2A6D, 
   0x2D57,  0x3015,  0x32AC,  0x3524,  0x377F,  0x39C2,  0x3BEE,  0x3E08, 
   0x400F,  0x4207,  0x43F0,  0x45CC,  0x479C,  0x4960,  0x4B19,  0x4CC9, 
   0x4E6F,  0x500C,  0x51A2,  0x532F,  0x54B6,  0x5635,  0x57AE,  0x5921, 
   0x5A8D,  0x5BF4,  0x5D56,  0x5EB3,  0x600A,  0x615D,  0x62AB,  0x63F5, 
   0x653B,  0x667D,  0x67BA,  0x68F5,  0x6A2B,  0x6B5E,  0x6C8D,  0x6DBA, 
   0x6EE3,  0x7009,  0x712C,  0x724C,  0x7369,  0x7484,  0x759C,  0x76B1, 
   0x77C4,  0x78D4,  0x79E2,  0x7AEE,  0x7BF7,  0x7CFE,  0x7E04,  0x7F07, 
   0x8007,  0x8106,  0x8203,  0x82FF,  0x83F8,  0x84EF,  0x85E5,  0x86D9, 
   0x87CB,  0x88BB,  0x89AA,  0x8A97,  0x8B83,  0x8C6D,  0x8D56,  0x8E3D, 
   0x8F22,  0x9007,  0x90E9,  0x91CB,  0x92AB,  0x938A,  0x9467,  0x9543, 
   0x961E,  0x96F8,  0x97D0,  0x98A8,  0x997E,  0x9A53,  0x9B26,  0x9BF9, 
   0x9CCA,  0x9D9B,  0x9E6A,  0x9F39,  0xA006,  0xA0D2,  0xA19D,  0xA268, 
   0xA331,  0xA3F9,  0xA4C1,  0xA587,  0xA64D,  0xA711,  0xA7D5,  0xA898, 
   0xA95A,  0xAA1B,  0xAADB,  0xAB9A,  0xAC59,  0xAD16,  0xADD3,  0xAE8F, 
   0xAF4B,  0xB005,  0xB0BF,  0xB178,  0xB230,  0xB2E8,  0xB39F,  0xB455, 
   0xB50A,  0xB5BF,  0xB673,  0xB726,  0xB7D9,  0xB88A,  0xB93C,  0xB9EC, 
   0xBA9C,  0xBB4B,  0xBBFA,  0xBCA8,  0xBD55,  0xBE02,  0xBEAE,  0xBF5A, 
   0xC005,  0xC0AF,  0xC159,  0xC202,  0xC2AB,  0xC353,  0xC3FA,  0xC4A1, 
   0xC548,  0xC5ED,  0xC693,  0xC737,  0xC7DC,  0xC87F,  0xC923,  0xC9C5, 
   0xCA67,  0xCB09,  0xCBAA,  0xCC4B,  0xCCEB,  0xCD8B,  0xCE2A,  0xCEC8, 
   0xCF67,  0xD004,  0xD0A2,  0xD13F,  0xD1DB,  0xD277,  0xD312,  0xD3AD, 
   0xD448,  0xD4E2,  0xD57C,  0xD615,  0xD6AE,  0xD746,  0xD7DE,  0xD876, 
   0xD90D,  0xD9A4,  0xDA3A,  0xDAD0,  0xDB66,  0xDBFB,  0xDC90,  0xDD24, 
   0xDDB8,  0xDE4C,  0xDEDF,  0xDF72,  0xE004,  0xE096,  0xE128,  0xE1B9, 
   0xE24A,  0xE2DB,  0xE36B,  0xE3FB,  0xE48B,  0xE51A,  0xE5A9,  0xE637, 
   0xE6C5,  0xE753,  0xE7E1,  0xE86E,  0xE8FB,  0xE987,  0xEA13,  0xEA9F, 
   0xEB2B,  0xEBB6,  0xEC41,  0xECCB,  0xED55,  0xEDDF,  0xEE69,  0xEEF2, 
   0xEF7B,  0xF004,  0xF08C,  0xF114,  0xF19C,  0xF223,  0xF2AB,  0xF332, 
   0xF3B8,  0xF43E,  0xF4C4,  0xF54A,  0xF5D0,  0xF655,  0xF6DA,  0xF75E, 
   0xF7E3,  0xF867,  0xF8EA,  0xF96E,  0xF9F1,  0xFA74,  0xFAF7,  0xFB79, 
   0xFBFB,  0xFC7D,  0xFCFF,  0xFD80,  0xFE02,  0xFE82,  0xFF03,  0xFF83
};

/* fsqrt:
 *  Fixed point square root routine.
 *
 * This routine is based upon the following idea:
 *    sqrt (x) = sqrt (x/d) * sqrt(d)
 *    d = 2^(2n) 
 *    sqrt (x) = sqrt (x / 2^(2n)) * 2^n
 * `x/2^(2n)' has to fall into the range 0..255 so that we can use the
 * square root lookup table. So `2n' is the number of bits `x' has to be
 * shifted to the left to become smaller than 256. The best way to find `2n'
 * is to do a reverse bit scan on `x'. This is achieved by the i386 ASM 
 * instruction `bsr'. 
 *
 */
fixed fsqrt(fixed x)
{
    int i, dx;
    int cx = 0;             /* if no bit set: default %cl = 2n = 0 */

    /* check whether `x' is negative... */
    if (x <= 0) {
        if (x < 0)
            errno = EDOM;
        return 0;
    }

    /* bit-scan is done on dx */
    dx = x >> 6;
    for (i = 0; i < 32; i++) {
        if (dx << i & 0x80000000) {
            cx = 32 - i;
            break;
        }
    }

    cx &= 0xFE;             /* make result even -->  %cl = 2n */
    x >>= cx;               /* shift x to fall into range 0..255 */

    x = _sqrt_tabl [x];     /* table lookup... */

    cx >>= 1;               /* %cl = n */
    x <<= cx;               /* multiply `sqrt(x/2^(2n))' by `2^n' */

    return x >> 4;          /* adjust the result */
}

/* fhypot:
 *  Fixed point sqrt (x*x+y*y).
 *
 *  Return fixed point sqrt (x*x+y*y), which is the length of the
 *  hypotenuse of a right triangle with sides of length x and y, or the
 *  distance of point (x|y) from the origin. This routine is faster and more
 *  accurate than using the direct formula fixsqrt (fixmul (x,x), fixmul(y,y)).
 *  It will also return correct results for x>=256 or y>=256 where fixmul(x)
 *  or fixmul(y) would overflow.
 *
 * The idea of this routine is:
 *    sqrt (x^2+y^2) = sqrt ((x/d)^2+(y/d)^2) * d
 *    d = 2^n
 * Since `x' and `y' are fixed point numbers, they are multiplied in the
 * following way:
 *    x^2 = (x*x)/2^16
 * so we come to the formula:
 *    sqrt(x^2+y^2) = sqrt((x*x + y*y)/2^(16+2n)) * 2^n
 * and this is almost the same problem as calculating the square root in
 * `fsqrt': find `2n' so that `(x*x+y*y)/2^(16+2n)' is in the range 0..255
 * so that we can use the square root lookup table.
 *
 */
fixed fhypot (fixed x, fixed y)
{
    int i, cx = 0;
    Uint64 sum, _x = x, _y = y;
    fixed hi_sum, lo_sum;

    /* check for overflow */
    sum = _x*_x + _y*_y;
    if ((sum >> 32) > 0x3FFFFFFF) {
        errno = ERANGE;
        return 0x7FFFFFFF;
    }

   /* And now we're doing a bit-scan on `x*x+y*y' to find out by how
    * many bits it needs to be shifted to fall into the range 0..255.
    * Since the intermediate result is 64 bit we may need two bitscans
    * in case that no bit is set in the upper 32 bit.
    */
    hi_sum = (sum >> 32) & 0xFFFFFFFF;
    lo_sum = sum & 0xFFFFFFFF;

    for (i = 0; i < 32; i++) {
        if (hi_sum << i & 0x80000000) {
            cx = 32 - i;
            break;
        }
    }

    if (cx) {
        cx += 2;                        /* make cl even */
        cx &= 0xFE;

        sum >>= cx;
        lo_sum = sum & 0xFFFFFFFF;      /* make lo_sum fall into range 0..255 */
        lo_sum >>= 24;

        x = _sqrt_tabl [lo_sum];        /* table lookup... */

        cx >>= 1;                       /* adjust the result */
        x <<= cx;
    }
    else {
        fixed dx;

        /* bit-scan is done on dx */
        lo_sum = lo_sum >> 16;
        dx = lo_sum >> 6;

        for (i = 0; i < 32; i++) {
            if (dx << i & 0x80000000) {
                cx = 32 - i;
                break;
            }
        }

        cx &= 0xFE;                     /* make result even -->  %cl = 2n */
        lo_sum >>= cx;                  /* shift lo_sum to fall into range 0..255 */

        x = _sqrt_tabl [lo_sum];        /* table lookup... */

        cx >>= 1;                       /* adjust the result */
        x <<= cx;
        x >>= 4;
    }

    return x;
}

#endif /* _FIXED_MATH */