Subject:
|
Integer-based arctangent2 function for anyone interested
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Thu, 31 Jan 2002 18:48:30 GMT
|
Viewed:
|
2107 times
|
| |
|
|
After scouring the internet for several days and even utilizing Jack
Crenshaw's wonderful book "Math Toolkit for Real-time Programming", I was
unable to find a good, general iatan2(y,x) function. Note that this is the
atan function that takes as input separate y and x coordinates, not a
floating point y/x value. These could also be sin(angle) and cos(angle).
So, out of desperation, I hobbled together a cludged (yes, it is) function
that works by taking integer x and y values, forcing them into a value
between 0 and 100, utilizing a Pade approximation, and returning integer
degrees from 0 to 360. If anyone has a better integer-based arctangent2
function, I'd be happy to know about it (and so would my robot :). If
anyone finds a problem with this function, let me know. There is more than
likely a possibility that it will break for large values of y and x, but I
have not attempted to verify this.
// iatan2() : arctangent using sin@,cos@ or y,x values, respectively
// returns angle in degrees 0 to 360
unsigned int iatan2(long y, long x)
{
unsigned long ax,ay,c;
int comp,oct;
// handle special cases
if(y == 0)
{
if(x >= 0)
return(0);
return(180);
}
if(x == 0)
{
if(y > 0)
return(90);
return(270);
}
// get absolute x and y values
ax = (unsigned long)abs(x);
ay = (unsigned long)abs(y);
// determine octant (counterclockwise 0:0-45 1:46-90 2:91-135 3:136-180 etc.)
if(y > 0)
{
if(x > 0)
{
if(ax >= ay) oct = 0;
else oct = 45;
}
else
{
if(ay >= ax) oct = 90;
else oct = 135;
}
}
else
{
if(x < 0)
{
if(ax >= ay) oct = 180;
else oct = 225;
}
else
{
if(ay >= ax) oct = 270;
else oct = 315;
}
}
// only the odd octants need to be inverted
comp = oct % 90;
// convert y/x into a value in the range 0..100
if(ay > ax)
c = (ax*100)/ay;
else
c = (ay*100)/ax;
// Pade approximation: a = (15*x+4x*x*x)/(15+9x*x) where 0<=a<=(pi/4) radians
// and 0<=x<=1.
// In order to compensate for input range adjustment to 0<=c<=100 and to
// return degrees, the equation was multiplied by 180/PI and then c divided
// by 100 in each term. 100^2 was multiplied to remove subfractions and each
// term divided by 7 to reduce integer size, resulting in the adjusted
equation:
// a ~= (38571*c + c*c*c)/(67143 + 4*c*c)
ax = c*(38571+(c*c));
ay = 67143+(4*c*c);
ax /= ay;
// handle inversion: atan(a) = (PI/4) - atan(1/a)
if(comp)
ax = 45 - ax;
// handle octants
ax += (unsigned long)oct;
return((unsigned int)ax);
}
Robert Templeton
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
