mymath.cpp

一个3D的保龄球的源代码

#include "mymath.h"
#include "aeestdlib.h"

#include <stdio.h>

//------------------------------------------------------------------------//
// Misc. Constants
//------------------------------------------------------------------------//
#ifdef USE_FLOAT
	const	TriValue pi	= 3.14159265;
	const	PhType g = 9.8f;

TriValue DegreesToRadians(PhType deg)
{
	return deg * pi / 180.0f;
}

TriValue RadiansToDegrees(PhType rad)
{	
	return rad * 180.0f / pi;
}

#else
	const	TriValue pi	= 51472;//3.1416*(2^14)
	const	PhType g = 1254;//9.8*(2^7)


	const TriValue TAN[181] = {	0, 142, 285, 427, 571, 714, 858, 1001, 1145, 1289, 
		1431, 1576, 1721, 1866, 2010, 2156, 2301, 2447, 2593, 2741, 
		2888, 3035, 3183, 3332, 3481, 3630, 3781, 3932, 4084, 4236, 
		4389, 4543, 4697, 4852, 5008, 5164, 5323, 5480, 5641, 5801, 
		5962, 6124, 6288, 6453, 6619, 6786, 6953, 7123, 7294, 7466, 
		7639, 7813, 7990, 8167, 8347, 8527, 8711, 8894, 9081, 9268, 
		9458, 9650, 9843, 10040, 10236, 10436, 10639, 10842, 11051, 11259, 
		11472, 11685, 11902, 12122, 12345, 12571, 12799, 13031, 13266, 13505, 
		13746, 13991, 14240, 14494, 14752, 15012, 15278, 15546, 15820, 16098, 
		16384, 16672, 16965, 17263, 17568, 17879, 18196, 18517, 18846, 19182, 
		19524, 19873, 20230, 20596, 20969, 21351, 21741, 22141, 22549, 22968, 
		23397, 23838, 24289, 24752, 25228, 25716, 26219, 26735, 27266, 27813, 
		28377, 28957, 29556, 30174, 30813, 31472, 32155, 32859, 33592, 34349, 
		35135, 35949, 36798, 37679, 38597, 39554, 40550, 41592, 42680, 43820, 
		45013, 46266, 47582, 48965, 50423, 51961, 53588, 55310, 57137, 59077, 
		61145, 63352, 65711, 68242, 70965, 73903, 77080, 80528, 84287, 88399, 
		92916, 97905, 103443, 109626, 116577, 124447, 133436, 143799, 155882, 170152, 
		187269, 208178, 234301, 267875, 312624, 375254, 469175, 625678, 938637, 1877419, 0x7fffffff
	};
/*
{0, 87, 174, 261, 349, 436, 524, 611, 699, 787,
874, 962, 1051, 1139, 1227, 1316, 1405, 1494, 1583, 1673,
1763, 1853, 1943, 2034, 2125, 2216, 2308, 2400, 2493, 2586,
2679, 2773, 2867, 2962, 3057, 3152, 3249, 3345, 3443, 3541,
3639, 3738, 3838, 3939, 4040, 4142, 4244, 4348, 4452, 4557,
4663, 4769, 4877, 4985, 5095, 5205, 5317, 5429, 5543, 5657,
5773, 5890, 6008, 6128, 6248, 6370, 6494, 6618, 6745, 6872,
7002, 7132, 7265, 7399, 7535, 7673, 7812, 7954, 8097, 8243,
8390, 8540, 8692, 8847, 9004, 9163, 9325, 9489, 9656, 9826,
10000, 10176, 10355, 10537, 10723, 10913, 11106, 11302, 11503, 11708,
11917, 12130, 12348, 12571, 12799, 13032, 13270, 13514, 13763, 14019,
14281, 14550, 14825, 15108, 15398, 15696, 16003, 16318, 16642, 16976,
17320, 17674, 18040, 18417, 18807, 19209, 19626, 20056, 20503, 20965,
21445, 21942, 22460, 22998, 23558, 24142, 24750, 25386, 26050, 26746,
27474, 28239, 29042, 29886, 30776, 31715, 32708, 33759, 34874, 36058,
37320, 38667, 40107, 41652, 43314, 45107, 47046, 49151, 51445, 53955,
56712, 59757, 63137, 66911, 71153, 75957, 81443, 87768, 95143, 103853,
114300, 127062, 143006, 163498, 190811, 229037, 286362, 381884, 572899, 1145886,
0x7fffffff};
*/

void myPrintError(char *s)
{
	DBGPRINTF(s);
}


int searchIndex(AngDegree degree)
{
  	int p;
	AngDegree d = 5;
  	if(degree>=0&&degree<=90*DEGREE_EXT)
  	{
  		p = degree/d + ( (degree%d)>d/2 ? 1 : 0 );
  	}
  	else
  		p = -1;
  	return p;
}

/*TriValue sin(AngDegree degree)
{
  	int p = searchIndex(degree);
  	if(p<0)
  		return -1;
  	return SIN[p];
}*/

TriValue sin360(AngDegree degree)
{	
	AngDegree tmp = abs(degree)%(3600);
	tmp=tmp*FP_HALF/3600;
	int val=Sinus(tmp);
	return degree<0?-val:val;

/*AngDegree tmp = abs(degree)%(360*DEGREE_EXT);
    TriValue value=0;
    if(tmp>=0&&tmp<=90*DEGREE_EXT)
      value = sin(tmp);
    else if (tmp>90*DEGREE_EXT&&tmp<=180*DEGREE_EXT)
      value = sin(180*DEGREE_EXT-tmp);
    else if (tmp>180*DEGREE_EXT&&tmp<=270*DEGREE_EXT)
      value = -sin(tmp-180*DEGREE_EXT);
    else if (tmp>270*DEGREE_EXT&&tmp<360*DEGREE_EXT)
      value = -sin(360*DEGREE_EXT-tmp);
	if(degree<0)
		value = -value;
    return value;*/
}

/*TriValue cos(AngDegree degree)
{
  	int p = searchIndex(90*DEGREE_EXT-degree);
  	if(p<0)
  		return -1;
  	return SIN[p];
}*/

TriValue cos360(AngDegree degree)
{
	AngDegree tmp = abs(degree)%(3600);
	tmp=tmp*FP_HALF/3600;
	return Cosinus(tmp);


/*	AngDegree tmp = abs(degree)%(360*DEGREE_EXT);
    TriValue value=0;
    if(tmp>=0&&tmp<=90*DEGREE_EXT)
      value = cos(tmp);
    else if (tmp>90*DEGREE_EXT&&tmp<=180*DEGREE_EXT)
      value = -cos(180*DEGREE_EXT-tmp);
    else if (tmp>180*DEGREE_EXT&&tmp<=270*DEGREE_EXT)
      value = -cos(tmp-180*DEGREE_EXT);
    else if (tmp>270*DEGREE_EXT&&tmp<360*DEGREE_EXT)
      value = cos(360*DEGREE_EXT-tmp);
    return value;*/
}

TriValue tan(AngDegree degree)
{
  	int p = searchIndex(degree);
  	if(p<0)
  		return -1;
  	return TAN[p];
}

TriValue tan360(AngDegree degree)
{
	AngDegree tmp = abs(degree)%(360*DEGREE_EXT);
    TriValue value=0;
    if(tmp>=0&&tmp<=90*DEGREE_EXT)
      value = tan(tmp);
    else if (tmp>90*DEGREE_EXT&&tmp<=180*DEGREE_EXT)
      value = -tan(180*DEGREE_EXT-tmp);
    else if (tmp>180*DEGREE_EXT&&tmp<270*DEGREE_EXT)
      value = tan(tmp-180*DEGREE_EXT);
    else if (tmp>=270*DEGREE_EXT&&tmp<360*DEGREE_EXT)
      value = -tan(360*DEGREE_EXT-tmp);
	if(degree<0)
		value = -value;
    return value;
}

int searchInUpArray(TriValue value,  const long *array, int len)
{
  	int first = 0;
  	int last = len-1;
  	int p = (first+last)/2;
  	if(value>=array[first]&&value<=array[last])
  	{
  		while(last-first>1)
  		{
  			if(value>array[p])
    			{
    				first = p;
    				p = (first+last)/2;
    				continue;
    			}
    			if(value < array[p])
    			{
    				last = p;
    				p = (first+last)/2;
    				continue;
    			}
    			if(value == array[p])
    			{
    				first = p;
    				break;
    			}
  		}
  		if(value-array[first]>=array[last]-value)
    			p = last;
    		else
    			p = first;
  	}
  	else
  		p = -1;
  	return p;
}

AngDegree asin(TriValue value)
{ 
   	AngDegree ang;
	int p = searchInUpArray(abs(value),(const long *)TSIN,513);
	if(p<0)
		return 0xffffffff;
	
	ang = (900*p)>>9;
	if(value<0)
		ang = -ang;
    return ang;

	/*AngDegree ang;
	int p = searchInUpArray(abs(value),SIN,181);
	if(p<0)
		return 0xffffffff;
	ang = 5*p;
	if(value<0)
		ang = -ang;
    return ang;*/
}

AngDegree acos(TriValue value)
{
    AngDegree angle = asin(abs(value));
    if(angle==0xffffffff)
		return angle;
    if(value<0)
		angle = 900+angle;
	else
		angle = 900-angle;
    return angle;
}

AngDegree atan(TriValue value)
{
	AngDegree ang;
    int p = searchInUpArray(abs(value),TAN,181);
	ang = 5*p;
    if(p==179 && abs(value)>TAN[179]*100)
    {
    	ang+=5;
    }
	if(value<0)
		ang = -ang;
    return ang;
}

AngDegree atan2(PhType y, PhType x)
{
	TriValue value;
	if(x==0)
	{
		if(y>0)
			return 90*DEGREE_EXT;
		if(y<0)
			return -90*DEGREE_EXT;
		if(y==0)
			return 0;
	}
	value = abs((TriValue)(y<<TRI_EXT)/x);
	AngDegree ang=atan(value);
	if(x<0&&y>=0)
		ang = 1800-ang;
	if(x<0&&y<0)
		ang += 1800;
	if(x>0&&y<0)
		ang = 3600-ang;
	return ang;
}

PhType mysqrt(PhType value)
{
	PhType  a, b;

    if ( value == 0 )
      return 0;

    a = value / 2;
    do
    {
      if(a==0)
	  {
		  a = 1;
		  break;
	  }
      b = ( value / a - a ) / 2;
      a = a + b;
    } while ( ( b > 1 ) || ( b < -1 ) );
	a = a<<(PHT_EXT/2);
	a += (PHT_EXT%2==1 ? a/2 : 0);
    return a;
}

void calSquare(PhType x, PhType *lv, PhType *hv)
{
	PhType n,mod,n2,mod2,tempn,tempmod,temp;
	
	x = abs(x);
	*lv = 0;
	*hv = 0;
	if(x<CRIT_VALUE)//2^15
	{
		(*lv) += x*x;
		return;
	}
	n = x/CRIT_VALUE;
	mod = x%CRIT_VALUE;
	n2 = n*n;
	mod2 = mod*mod;
	*hv = n2;
	*lv = mod2;

	tempn = mod*n/CRIT_VALUE;
	tempmod = mod*n%CRIT_VALUE;

	(*hv) += 2*tempn;

	temp = 2*tempmod*CRIT_VALUE;
	(*hv) += temp/0x40000000;
	(*lv) += temp%0x40000000;
	(*hv) += (*lv)/0x40000000;
	(*lv) %= 0x40000000;
}

PhType calLength(PhType *x, int n)
{
	PhType lv=0, hv = 0, value=0;
	PhType templ=0,temph=0;
	int i;
	for(i=0;i<n;i++)
	{
		calSquare(x[i],&templ,&temph);
		hv += temph;
		lv += templ;
		hv += lv/0x40000000;
		lv %= 0x40000000;
	}
	value = mysqrt(hv)/12*CRIT_VALUE+mysqrt(lv)/12;
	return value;
}

inline PhType abs(PhType value)
{
    return Abs(value);
}

#endif


//------------------------------------------------------------------------//
// Vector Class and vector functions
//------------------------------------------------------------------------//

Vector::Vector(void)
{
	x = 0;
	y = 0;
	z = 0;
}

Vector::Vector(PhType xi, PhType yi, PhType zi)
{
	x = xi;
	y = yi;
	z = zi;
}

PhType Vector::Magnitude(void)
{
#ifdef USE_FLOAT
	return sqrt(x*x + y*y + z*z);
#else
	PhType m[3];
	m[0]=x; m[1]=y; m[2]=z;
	return calLength(m,3);
#endif
	
}

void  Vector::Normalize(void)
{
	PhType m = Magnitude();
	
	if (m==0)
		m=(1<<PHT_EXT);
	
	x = x<<PHT_EXT;
	x /= m;
	y = y<<PHT_EXT;
	y /= m;
	z = z<<PHT_EXT;
	z /= m;
}

void  Vector::Reverse(void)
{
	x = -x;
	y = -y;
	z = -z;
}

Vector& Vector::operator+=(Vector u)
{
	x += u.x;
	y += u.y;
	z += u.z;
	return *this;
}

Vector& Vector::operator-=(Vector u)
{
	x -= u.x;
	y -= u.y;
	z -= u.z;
	return *this;
}

Vector& Vector::operator*=(PhType s)
{
#ifdef USE_FLOAT
	x *= s;
	y *= s;
	z *= s;
#else
#ifdef DEBUG_OVERFLOW
	if(abs(x)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(x)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE){
		x = x>>PHT_EXT;
		x *= s;
	}
	else{
		x *= s;
		x = x>>PHT_EXT;
	}
#ifdef DEBUG_OVERFLOW
	if(abs(y)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(y)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE){
		y = y>>PHT_EXT;
		y *= s;
	}
	else{
		y *= s;
		y = y>>PHT_EXT;
	}
#ifdef DEBUG_OVERFLOW	
	if(abs(z)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(z)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE){
		z = z>>PHT_EXT;
		z *= s;
	}
	else{
		z *= s;
		z = z>>PHT_EXT;
	}
#endif	
	return *this;
}

Vector& Vector::operator/=(PhType s)
{
	x = x<<PHT_EXT;
	x /= s;
	y = y<<PHT_EXT;
	y /= s;
	z = z<<PHT_EXT;
	z /= s;
	return *this;
}

Vector Vector::operator-(void)
{
	return Vector(-x, -y, -z);
}


Vector operator+(Vector u, Vector v)
{
	return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
}

Vector operator-(Vector u, Vector v)
{
	return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
}

// Vector cross product (u cross v)
Vector operator^(Vector u, Vector v)
{
#ifdef USE_FLOAT
	return Vector(	u.y*v.z - u.z*v.y,
					-u.x*v.z + u.z*v.x,
					u.x*v.y - u.y*v.x);
#else
	PhType tX=0,tY=0,tZ=0;
#ifdef DEBUG_OVERFLOW
	if(abs(u.y)>=DANGER_VALUE&&abs(v.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(u.y>=CRIT_VALUE)&&abs(v.z)>=CRIT_VALUE)
		tX += (u.y>>PHT_EXT)*v.z;
	else
		tX += u.y*v.z>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(u.z)>=DANGER_VALUE&&abs(v.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(u.z>=CRIT_VALUE)&&abs(v.y)>=CRIT_VALUE)
		tX -= (u.z>>PHT_EXT)*v.y;
	else
		tX -= u.z*v.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW