mymath.cpp

liu7788414

#include "mymath.h"
//#include "aeestdlib.h"
#include <stdio.h>
#include "..\system\sysdef.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 SIN[181] = { 0, 142, 285, 427, 570, 714, 856, 999, 1141, 1284, 
		1427, 1569, 1712, 1854, 1995, 2138, 2279, 2421, 2562, 2703, 
		2844, 2985, 3126, 3265, 3406, 3545, 3684, 3824, 3963, 4100, 
		4240, 4377, 4515, 4653, 4789, 4926, 5062, 5198, 5332, 5468, 
		5603, 5737, 5870, 6004, 6137, 6268, 6401, 6532, 6663, 6792, 
		6923, 7053, 7181, 7308, 7436, 7564, 7690, 7816, 7942, 8067, 
		8192, 8314, 8437, 8559, 8681, 8801, 8922, 9042, 9160, 9279, 
		9396, 9514, 9628, 9745, 9859, 9972, 10085, 10199, 10310, 10420, 
		10529, 10639, 10747, 10856, 10962, 11067, 11172, 11277, 11380, 11483, 
		11585, 11685, 11785, 11883, 11981, 12078, 12174, 12269, 12365, 12458, 
		12550, 12641, 12732, 12822, 12910, 12997, 13084, 13169, 13254, 13338, 
		13420, 13502, 13582, 13660, 13739, 13816, 13893, 13968, 14042, 14116, 
		14188, 14258, 14329, 14398, 14465, 14532, 14598, 14662, 14724, 14786, 
		14848, 14907, 14966, 15024, 15081, 15135, 15189, 15243, 15294, 15345, 
		15394, 15443, 15491, 15536, 15581, 15625, 15668, 15708, 15748, 15787, 
		15825, 15861, 15895, 15930, 15962, 15994, 16025, 16054, 16082, 16108, 
		16134, 16157, 16180, 16203, 16223, 16243, 16261, 16277, 16293, 16306, 
		16320, 16333, 16343, 16352, 16361, 16367, 16372, 16377, 16380, 16382, 16384
	};
/*
{0, 87, 174, 261, 348, 436, 523, 610, 697, 784, 871, 958,
1045, 1132, 1218, 1305, 1391, 1478, 1564, 1650, 1736, 1822, 1908, 1993,
2079, 2164, 2249, 2334, 2419, 2503, 2588, 2672, 2756, 2840, 2923, 3007,
3090, 3173, 3255, 3338, 3420, 3502, 3583, 3665, 3746, 3826, 3907, 3987,
4067, 4146, 4226, 4305, 4383, 4461, 4539, 4617, 4694, 4771, 4848, 4924,
5000, 5075, 5150, 5224, 5299, 5372, 5446, 5519, 5591, 5664, 5735, 5807,
5877, 5948, 6018, 6087, 6156, 6225, 6293, 6360, 6427, 6494, 6560, 6626,
6691, 6755, 6819, 6883, 6946, 7009, 7071, 7132, 7193, 7253, 7313, 7372,
7431, 7489, 7547, 7604, 7660, 7716, 7771, 7826, 7880, 7933, 7986, 8038,
8090, 8141, 8191, 8241, 8290, 8338, 8386, 8433, 8480, 8526, 8571, 8616,
8660, 8703, 8746, 8788, 8829, 8870, 8910, 8949, 8987, 9025, 9063, 9099,
9135, 9170, 9205, 9238, 9271, 9304, 9335, 9366, 9396, 9426, 9455, 9483,
9510, 9537, 9563, 9588, 9612, 9636, 9659, 9681, 9702, 9723, 9743, 9762,
9781, 9799, 9816, 9832, 9848, 9862, 9876, 9890, 9902, 9914, 9925, 9935,
9945, 9953, 9961, 9969, 9975, 9981, 9986, 9990, 9993, 9996, 9998, 9999,
10000};
*/
	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)%(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)%(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 TriValue *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),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 = 90*DEGREE_EXT+angle;
	else
		angle = 90*DEGREE_EXT-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 sqrt(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 = sqrt(hv)/12*CRIT_VALUE+sqrt(lv)/12;
	return value;
}

PhType abs(PhType value)
{
    if(value < 0)
      return (-value);
    else
      return 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);
}