在fix point 系统上的一些数学方程
#define UP_SCALE(a) ((int)((a) * 1000))#define PI UP_SCALE(3.1415)
#define PI12 (PI/12)
#define PI6 (PI/6)
#define PI2 (PI/2)
#define SQRT3 (UP_SCALE(1.7320))
int sinTable[91] =
{
0, 17, 34, 52, 69,
87, 104, 121, 139, 156,
173, 190, 207, 224, 241,
258, 275, 292, 309, 325,
342, 358, 374, 390, 406,
422, 438, 453, 469, 484,
499, 515, 529, 544, 559,
573, 587, 601, 615, 629,
642, 656, 669, 681, 694,
707, 719, 731, 743, 754,
766, 777, 788, 798, 809,
819, 829, 838, 848, 857,
866, 874, 882, 891, 898,
906, 913, 920, 927, 933,
939, 945, 951, 956, 961,
965, 970, 974, 978, 981,
984, 987, 990, 992, 994,
996, 997, 998, 999, 999,
1000
};
inline int sind(int theta)
{
if(theta < 0)
{ theta -= (theta / UP_SCALE(360) - 1) * UP_SCALE(360);
}
if(theta > UP_SCALE(360))
{ theta -= (theta / UP_SCALE(360)) * UP_SCALE(360);
}
int multiplier = 1;
if(theta > UP_SCALE(180))
{
multiplier = -1;
theta -= UP_SCALE(180);
}
if(theta > UP_SCALE(90))
{
theta = UP_SCALE(180) - theta;
}
int fl = floor(theta);
if(theta == UP_SCALE(90))
{
return UP_SCALE(1) * multiplier;
}
return linear_interpolation(fl, sinTable[fl / UP_SCALE(1)], fl + UP_SCALE(1), sinTable[fl / UP_SCALE(1) + 1], theta) * multiplier;
}
inline int cosd(int theta)
{
return sind(UP_SCALE(90) - theta);
}
inline int sin(int rad)
{
return sind(fix_div(fix_multi(rad, UP_SCALE(180)), PI));
}
inline int cos(int rad)
{
return cosd(fix_div(fix_multi(rad, UP_SCALE(180)), PI));
}
inline int linear_interpolation(int x1, int y1, int x2, int y2, int x)
{
return (x - x1) * ((y2 - y1) / (x2 - x1)) + y1;
}
inline int arctg(int x)
{
int sta = 0;
int sp = 0;
int x2;
int a;
if(x < 0)
{
x = -x;
sta |= 0x01;
}
if(x > 1)
{
x = fix_div(UP_SCALE(1), x);
sta |= 0x02;
}
while(x > PI12)
{
sp++;
a = x + SQRT3;
a = fix_div(UP_SCALE(1), a);
x = fix_multi(x, SQRT3);
x -= UP_SCALE(1);
x = fix_multi(x, a);
}
x2 = fix_multi(x, x);
a = x2 + UP_SCALE(1.4087812);
a = fix_div(UP_SCALE(0.55913709), a);
a += UP_SCALE(0.60310579);
a -= fix_multi(UP_SCALE(0.05160454), x2);
a = fix_multi(a, x);
while(sp > 0)
{
a += PI6;
sp--;
}
if(sta & 0x02)
{
a = PI2 - a;
}
if(sta & 0x01)
{
a = -a;
}
return a;
}
inline int vectorToAngle(int x, int y)
{
int angle = 0;
if(!y)
{ if(x > 0)
{angle = PI2;}
else{angle = -PI2;}
}
else
{angle = arctg(fix_div(x, y));}
if(y < 0)
{
angle += PI;
}
return -angle;
}
inline int floor(int x)
{
if(x >= 0)
{
return (x / UP_SCALE(1)) * UP_SCALE(1);
}
else
{
return (x / UP_SCALE(1) - 1) * UP_SCALE(1);
}
}
inline int ceil(int x)
{
if(x % UP_SCALE(1) == 0)
{
return x;
}
return floor(x) + UP_SCALE(1);
}
inline int round(int x)
{
int multiplier = 1;
if(x < 0)
{
multiplier = -1;
x = -x;
}
if(x % UP_SCALE(1) < UP_SCALE(0.5))
{
return floor(x) * multiplier;
}
return ceil(x) * multiplier;
}
inline int fix_multi(int p1, int p2)
{
return p1 * p2 / UP_SCALE(1);
}
inline int fix_div(int p1, int p2)
{
return p1 * UP_SCALE(1) / p2;
}
inline int abs(int x)
{
return x >= 0 ? x : -x;
}
inline int sign(int x)
{
if(x > 0)
{
return UP_SCALE(1);
}
if(x < 0)
{
return UP_SCALE(-1);
}
return 0;
}
inline int dotProduct2D(int op1x, int op1y, int op2x, int op2y)
{
return fix_multi(op1x, op2x) + fix_multi(op1y, op2y);
}
inline unsigned int rnd()
{
static unsigned int
x=123456789, y=362436069,
z=521288629, w=88675123;
unsigned int t;
t = x ^ (x << 11);
x = y; y = z; z = w;
w = (w ^ (w >> 19)) ^ (t ^ (t >> 8));
return w;
}