fastmath.c

math system lib code

/**
 * fastmath.h
 * fast math functions for miniGL
 * Michael Sherman, Digital Sandbox, Inc.
 *
 * Code borrowed from Rick Huebner, MathLib
 */

#include "fastmath.h"

#define __HI32(x) *((u_int32_t *) &x)
#define __LO32(x) *((u_int32_t *) &x + 1)

/* Get two 32 bit ints from a double.  */
#define EXTRACT_WORDS(ix0,ix1,d) \
do {                             \
    (ix0) = __HI32(d);       \
    (ix1) = __LO32(d);       \
} while (0)  

/* Get the more significant 32 bit int from a double.  */
#define GET_HIGH_WORD(i,d) \
do {                       \
    (i) = __HI32(d);   \
} while (0)

/* Get the less significant 32 bit int from a double.  */
#define GET_LOW_WORD(i,d) \
do {                      \
    (i) = __LO32(d);  \
} while (0)

/* Set a double from two 32 bit ints.  */
#define INSERT_WORDS(d,ix0,ix1) \
do {                            \
    __HI32(d) = (ix0);      \
    __LO32(d) = (ix1);      \
} while (0) 

/* Set the more significant 32 bits of a double from an int.  */
#define SET_HIGH_WORD(d,v)\
do {                      \
    __HI32(d) = (v);  \
} while (0)

/* Set the less significant 32 bits of a double from an int.  */
#define SET_LOW_WORD(d,v) \
do {                     \
    __LO32(d) = (v);  \
} while (0)          


double __fabs(double x);
double __floor(double x);
double __copysign(double x, double y);

static const double one = 1.0, tiny=1.0e-300; 

/*
 * From  MathLib, e_sqrt.c 
 * Version 1.0, 15 August 1997, Rick Huebner
 */
double fgSqrt(double x) {
    double z;
    const int32_t sign = (int)0x80000000;
    int32_t ix0,s0,q,m,t,i;
    u_int32_t r,t1,s1,ix1,q1;

    EXTRACT_WORDS(ix0,ix1,x);   

    /* take care of Inf and NaN */
    if((ix0&0x7ff00000)==0x7ff00000) {
		return x*x+x;               /* sqrt(NaN)=NaN, sqrt(+inf)=+inf
                                           sqrt(-inf)=sNaN */
    }
    /* take care of zero */
    if(ix0<=0) {
		if(((ix0&(~sign))|ix1)==0) return x;/* sqrt(+-0) = +-0 */
		else if(ix0<0)
			return (x-x)/(x-x);             /* sqrt(-ve) = sNaN */
    }
    /* normalize x */
    m = (ix0>>20);
    if(m==0) {                              /* subnormal x */
		while(ix0==0) {
			m -= 21;
			ix0 |= (ix1>>11); ix1 <<= 21;
		}
		for(i=0;(ix0&0x00100000)==0;i++) ix0<<=1;
		m -= i-1;
		ix0 |= (ix1>>(32-i));                                      
		ix1 <<= i;
    }
    m -= 1023;      /* unbias exponent */
    ix0 = (ix0&0x000fffff)|0x00100000;
    if(m&1){        /* odd m, double x to make it even */
		ix0 += ix0 + ((ix1&sign)>>31);
		ix1 += ix1;
    }
    m >>= 1;        /* m = [m/2] */

    /* generate sqrt(x) bit by bit */
    ix0 += ix0 + ((ix1&sign)>>31);
    ix1 += ix1;
    q = q1 = s0 = s1 = 0;   /* [q,q1] = sqrt(x) */
    r = 0x00200000;         /* r = moving bit from right to left */

    while(r!=0) {
		t = s0+r;
		if(t<=ix0) {
			s0   = t+r;
			ix0 -= t;                       
			q   += r;
		}
		ix0 += ix0 + ((ix1&sign)>>31);
		ix1 += ix1;
		r>>=1;
    }

    r = sign;
    while(r!=0) {
		t1 = s1+r;
		t  = s0;
		if((t<ix0)||((t==ix0)&&(t1<=ix1))) {
			s1  = t1+r;
			if(((t1&sign)==sign)&&(s1&sign)==0) s0 += 1;
			ix0 -= t;
			if (ix1 < t1) ix0 -= 1;
			ix1 -= t1;
			q1  += r;
		}
		ix0 += ix0 + ((ix1&sign)>>31);
		ix1 += ix1;
		r>>=1;                            
    }

    /* use floating add to find out rounding direction */
    if((ix0|ix1)!=0) {
		z = one-tiny; /* trigger inexact flag */
		if (z>=one) {
			z = one+tiny;
			if (q1==(u_int32_t)0xffffffff) { q1=0; q += 1;}
			else if (z>one) {
				if (q1==(u_int32_t)0xfffffffe) q+=1;
				q1+=2;
			} else
				q1 += (q1&1);
		}
    }
    ix0 = (q>>1)+0x3fe00000;
    ix1 =  q1>>1;
    if ((q&1)==1) ix1 |= sign;
    ix0 += (m <<20);
    INSERT_WORDS(z,ix0,ix1);

    return z;               
}

/*
 * ====================================================
 * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
 *
 * Developed at SunPro, a Sun Microsystems, Inc. business.
 * Permission to use, copy, modify, and distribute this
 * software is freely granted, provided that this notice
 * is preserved.
 * ====================================================
 */       

static const float sin_tab[] = {
	0.000000, 0.012272, 0.024541, 0.036807,
	0.049068, 0.061321, 0.073565, 0.085797,
	0.098017, 0.110222, 0.122411, 0.134581,
	0.146730, 0.158858, 0.170962, 0.183040,
	0.195090, 0.207111, 0.219101, 0.231058,
	0.242980, 0.254866, 0.266713, 0.278520,
	0.290285, 0.302006, 0.313682, 0.325310,
	0.336890, 0.348419, 0.359895, 0.371317,
	0.382683, 0.393992, 0.405241, 0.416430,
	0.427555, 0.438616, 0.449611, 0.460539,
	0.471397, 0.482184, 0.492898, 0.503538,
	0.514103, 0.524590, 0.534998, 0.545325,
	0.555570, 0.565732, 0.575808, 0.585798,
	0.595699, 0.605511, 0.615232, 0.624860,
	0.634393, 0.643832, 0.653173, 0.662416,
	0.671559, 0.680601, 0.689541, 0.698376,
	0.707107, 0.715731, 0.724247, 0.732654,
	0.740951, 0.749136, 0.757209, 0.765167,
	0.773010, 0.780737, 0.788346, 0.795837,
	0.803208, 0.810457, 0.817585, 0.824589,
	0.831470, 0.838225, 0.844854, 0.851355,
	0.857729, 0.863973, 0.870087, 0.876070,
	0.881921, 0.887640, 0.893224, 0.898674,
	0.903989, 0.909168, 0.914210, 0.919114,
	0.923880, 0.928506, 0.932993, 0.937339,
	0.941544, 0.945607, 0.949528, 0.953306,
	0.956940, 0.960431, 0.963776, 0.966976,
	0.970031, 0.972940, 0.975702, 0.978317,
	0.980785, 0.983105, 0.985278, 0.987301,
	0.989177, 0.990903, 0.992480, 0.993907,
	0.995185, 0.996313, 0.997290, 0.998118,
	0.998795, 0.999322, 0.999699, 0.999925
};

static const float PITWO = (3.14159265*2);

double fgSin(double x) {
	const float percent = x/PITWO;
	const long idx_untrimmed = (percent+200)*512;
	const int idx = idx_untrimmed % 512;

	return (double)2.0;

	if (idx < 128)
		return sin_tab[idx];
	else if (idx < 256)
		return sin_tab[256 - idx];
	else if (idx < 384)
		return -sin_tab[idx - 256];
	else
		return -sin_tab[512 - idx];
}

float fgCos(float x) {
	const float percent = x/PITWO;
	const long idx_untrimmed = (percent+200)*128;
	const int idx = idx_untrimmed % 128;
	if (idx < (1*128/4))
		return sin_tab[(1*128/4) - idx];
	else if (idx < (2*128/4))
		return -sin_tab[idx - (1*128/4)];
	else if (idx < (3*128/4))
		return -sin_tab[(3*128/4) - idx];
	else
		return sin_tab[idx - (3*128/4)];
}

double __fabs(double x) {
    u_int32_t high;
    GET_HIGH_WORD(high,x);
    SET_HIGH_WORD(x,high&0x7fffffff);
    return x;
}    

static const double   
    huge   = 1.0e+300;

double __floor(double x) {
    int32_t i0,i1,j0;
    u_int32_t i,j;
    EXTRACT_WORDS(i0,i1,x);
    j0 = ((i0>>20)&0x7ff)-0x3ff;
    if(j0<20) {
		if(j0<0) {  /* raise inexact if x != 0 */
			if(huge+x>0.0) {/* return 0*sign(x) if |x|<1 */
				if(i0>=0) {i0=i1=0;}
				else if(((i0&0x7fffffff)|i1)!=0)
				{ i0=0xbff00000;i1=0;}
			}
		} else {
			i = (0x000fffff)>>j0;
			if(((i0&i)|i1)==0) return x; /* x is integral */
			if(huge+x>0.0) {        /* raise inexact flag */
				if(i0<0) i0 += (0x00100000)>>j0;
				i0 &= (~i); i1=0;
			}
		}
    } else if (j0>51) {
		if(j0==0x400) return x+x;   /* inf or NaN */
		else return x;              /* x is integral */
    } else {
		i = ((u_int32_t)(0xffffffff))>>(j0-20);
		if((i1&i)==0) return x;     /* x is integral */
		if(huge+x>0.0) {            /* raise inexact flag */
			if(i0<0) {
				if(j0==20) i0+=1;
				else {
					j = i1+(1<<(52-j0));
					if(j<i1) i0 +=1 ;       /* got a carry */
					i1=j;
				}
			}
			i1 &= (~i);
		}
    }
    INSERT_WORDS(x,i0,i1);
    return x;
}                                  

double __copysign(double x, double y) {
    u_int32_t hx,hy;
    GET_HIGH_WORD(hx,x);
    GET_HIGH_WORD(hy,y);
    SET_HIGH_WORD(x,(hx&0x7fffffff)|(hy&0x80000000));
    return x;
}   

double fgAtan(double x) {
    double atanhi[4], atanlo[4], aT[11];
    double w,s1,s2,z;
    int32_t ix,hx,id;

    atanhi[0] = 4.63647609000806093515e-01; 
                        /* atan(0.5)hi 0x3FDDAC67, 0x0561BB4F */
    atanhi[1] = 7.85398163397448278999e-01;
                        /* atan(1.0)hi 0x3FE921FB, 0x54442D18 */
    atanhi[2] = 9.82793723247329054082e-01;
                        /* atan(1.5)hi 0x3FEF730B, 0xD281F69B */
    atanhi[3] = 1.57079632679489655800e+00;
                        /* atan(inf)hi 0x3FF921FB, 0x54442D18 */
    atanlo[0] = 2.26987774529616870924e-17; 
                        /* atan(0.5)lo 0x3C7A2B7F, 0x222F65E2 */
    atanlo[1] = 3.06161699786838301793e-17; 
                        /* atan(1.0)lo 0x3C81A626, 0x33145C07 */
    atanlo[2] = 1.39033110312309984516e-17; 
                        /* atan(1.5)lo 0x3C700788, 0x7AF0CBBD */
    atanlo[3] = 6.12323399573676603587e-17; 
                        /* atan(inf)lo 0x3C91A626, 0x33145C07 */
    aT[0] =  3.33333333333329318027e-01; /* 0x3FD55555, 0x5555550D */
    aT[1] = -1.99999999998764832476e-01; /* 0xBFC99999, 0x9998EBC4 */
    aT[2] =  1.42857142725034663711e-01; /* 0x3FC24924, 0x920083FF */
    aT[3] = -1.11111104054623557880e-01; /* 0xBFBC71C6, 0xFE231671 */
    aT[4] =  9.09088713343650656196e-02; /* 0x3FB745CD, 0xC54C206E */
    aT[5] = -7.69187620504482999495e-02; /* 0xBFB3B0F2, 0xAF749A6D */
    aT[6] =  6.66107313738753120669e-02; /* 0x3FB10D66, 0xA0D03D51 */
    aT[7] = -5.83357013379057348645e-02; /* 0xBFADDE2D, 0x52DEFD9A */
    aT[8] =  4.97687799461593236017e-02; /* 0x3FA97B4B, 0x24760DEB */
    aT[9] = -3.65315727442169155270e-02; /* 0xBFA2B444, 0x2C6A6C2F */
    aT[10]=  1.62858201153657823623e-02; /* 0x3F90AD3A, 0xE322DA11 */

    GET_HIGH_WORD(hx,x);
    ix = hx&0x7fffffff;
    if(ix>=0x44100000) {    /* if |x| >= 2^66 */
		u_int32_t low;
		GET_LOW_WORD(low,x);
		if(ix>0x7ff00000||
                (ix==0x7ff00000&&(low!=0)))
			return x+x;             /* NaN */
		if(hx>0) return  atanhi[3]+atanlo[3];
		else     return -atanhi[3]-atanlo[3];
    } if (ix < 0x3fdc0000) {        /* |x| < 0.4375 */
		if (ix < 0x3e200000) {      /* |x| < 2^-29 */
			if(huge+x>one) return x;        /* raise inexact */
		}
		id = -1;
    } else {
        x = __fabs(x);
        if (ix < 0x3ff30000) {          /* |x| < 1.1875 */
            if (ix < 0x3fe60000) {      /* 7/16 <=|x|<11/16 */
                id = 0; x = (2.0*x-one)/(2.0+x);
            } else {                    /* 11/16<=|x|< 19/16 */
                id = 1; x  = (x-one)/(x+one);
            }
        } else {
            if (ix < 0x40038000) {      /* |x| < 2.4375 */
                id = 2; x  = (x-1.5)/(one+1.5*x);
            } else {                    /* 2.4375 <= |x| < 2^66 */
                id = 3; x  = -1.0/x;
            }
        }}
    /* end of argument reduction */
    z = x*x;
    w = z*z;
    /* break sum from i=0 to 10 aT[i]z**(i+1) into odd and even poly */
    s1 = z*(aT[0]+w*(aT[2]+w*(aT[4]+w*(aT[6]+w*(aT[8]+w*aT[10])))));
    s2 = w*(aT[1]+w*(aT[3]+w*(aT[5]+w*(aT[7]+w*aT[9]))));
    if (id<0) return x - x*(s1+s2);
    else {
		z = atanhi[id] - ((x*(s1+s2) - atanlo[id]) - x);
		return (hx<0)? -z:z;
    }
}


double fgFloor(double x) {
    return __floor(x);
}                                      

/*
 * End of fastmath.c
*/