Algorithm for Calculating arctan ( atan ) Rectangular to Polar Conversion using

ever

Hopefully this is useful to anyone working with a GPS module who needs to determine the angle between their present position and a desired position. The code is for example and would need more member functions to be useful beyond just understanding that it is possible to get arctan(y/x) (tan^-1(y/x)) without actually having the function by using an algorithm. The algorithm is slow, especially with the granularity set to one degree. Increasing the granularity improves performance but also increases error. Have fun!

import stamp.math.*; // using class Float32Math and class Float32
import stamp.core.*;

public class rectangularToPolar { // converts rectangular cors to polar without arctan
static final int x = 50, y = -100, gran = 1; // x and y rect cors, gran (step granularity in degrees)
static int theta = 0; // angle in degrees (r, theta)
static Float32 radToDeg = new Float32(); // constant 57.2957795 (degrees in a radian)
static Float32 magnitude = new Float32(); // magnitude = r (r, theta)
static Float32 fx = new Float32(); // floatX
static Float32 fy = new Float32(); // floatY
static Float32 fTemp = new Float32(); // float temp var
static Float32 fTheta = new Float32(); // float theta for precision calculations
public static void main() {
radToDeg.set("57.2957795");
magnitude.set(Float32Math.sqrt(fTemp.set(Float32Math.pow(fx.set(x), 2)) // calculate r
.add(Float32Math.pow(fy.set(y), 2))));
if(y == 0 && x > 0) theta = 0; // skip for loop in these conditions
else if(x == 0 && y > 0) theta = 90;
else if(y == 0 && x < 0) theta = 180;
else if(x == 0 && y < 0) theta = 270;
else {
if(x < 0 && y > 0) theta = 90; // optomize based on known polar quadrants
else if(x < 0 && y < 0) theta = 180;
else if(x > 0 && y < 0) theta = 270;
for(; theta < 360; theta+=gran) { // go around the unit circle ...
if(fy.set(y).divide(fx).compare(Float32Math.tan(fTheta.set(theta) // theta lower
.divide(radToDeg))) >= 0
&& fy.set(y).divide(fx).compare(Float32Math.tan(fTheta.set(theta+gran) // theta upper
.divide(radToDeg))) <= 0)
break; // theta was trapped betwene upper and lower, now it is known (approx.)
}
}
System.out.print("polar~: "); // print cors (r, theta) in polar form
System.out.print(magnitude.toString());
System.out.print(", ");
System.out.println(theta);
}
}

Peter Verkaik

Can you post a diagram of which angle exactly
you try to calculate?
The diagram should include the 2 positions, and the unit circle
with its origin. If somehow the origin is involved with the
angle, it must be possible simplify the calculation by
assuming one postion is the origin (0,0).

regards peter

ever

The angle that is calculated is arbitrary. In the code above, x = 50 and y = -100, that would put the the point in the domain of positive x and negative y, the fourth quadrant on a Cartesian plane. In the polar coordinate system all points in a Cartesian plane in the fourth quadrant are angles above 270 degrees.

It is always assumed that you (or the robot) are (0, 0) or the origin on whatever coordinate system you are using. To noramize GPS coordinates so that you or your robot are represented as the origin and the desired point to navigate to is relative to (0, 0), simply use the distance formula. If you are at (x_1, y_1) and the waypoint is (x_2, y_2) the normalized point to use in the algorithm would simply be ((x_2 - x_1), (y_2 - y_1)).

As a functional class used for robot navigation, x_1 and y_1 (present position) would be passed into the function by a GPS module class member function, and x_2 and y_2 would be stored in an array of potentially many GPS waypoints. The robots heading would also be passed by the GPS class. Once the desired heading is calculated the robot can steer in porportion to its cross track error and follow the desired heading to the waypoint, meanwhile recalculating its desired heading and comparing that to its present heading and compensating for cross track error (over and over). most likely two Javelin Stamps would be needed: one to calculate heading and do meta navigational stuff and one to run sensors and steering and throttle, avoid obstacles.

Information about the unit circle can be found at:
http://en.wikipedia.org/wiki/Unit_circle
http://www.humboldt.edu/~dlj1/PreCalculus/Images/UnitCircle.html

Information about the Cartesian plane can be found at:
http://en.wikipedia.org/wiki/Cartesian_coordinate_system
http://www.math.temple.edu/~zachhh/lines.pdf

Information regarding the distance formula can be found at:
http://purplemath.com/modules/distform.htm

The same code for converting from rectangular to polar without arctan written in c++ is below:
Although of course you can use atan(y/x) in c++, this example may add clarity for the c++rs among us. [noparse]:)[/noparse]

#include <iostream>
#include <cstdlib>
#include <cmath>

using namespace std;

int main() {
float x = 0, y = 0, theta = 0;
float radToDeg = 57.2957795;
cin >> x >> y;
cin.get();
if(y == 0 && x > 0) theta = 0;
else if(x == 0 && y > 0) theta = 90;
else if(y == 0 && x < 0) theta = 180;
else if(x == 0 && y < 0) theta = 270;
else {
if(x < 0 && y > 0) theta = 90;
else if(x < 0 && y < 0) theta = 180;
else if(x > 0 && y < 0) theta = 270;
for(; theta < 360; theta++) {
if(tan(theta/radToDeg) <= (y/x) && tan((theta+1)/radToDeg) >= (y/x))
break;
}
}
cout << theta << endl;
cin.get();
return 0;
}

Regards,
Weston

PS: the Floa32Math class functions sometimes return invalid data, so using a granularity of one is not recommended. A granularity of 5 or 10 is less accurate but improves performance and reduces chances of bad data being returned from the Float32Math tan() function.

Post Edited (ever) : 9/11/2007 4:22:34 AM GMT

Peter Verkaik

From just two points I cannot construct an angle. There
must be a 3rd point involved. So again, which angle are you
trying to calculate? How is the angle constructed?

regards peter

ever

Of course an angle can be created with two points. (0, 0) and (5, 5) creates a 45 degree angle, (0, 0) and (-5, 5) creates a 135 degree angle, and so on and so forth. The angle is created by converting from rectangular to polar. Really P_1 or the origin is assumed. It is just one point, the gps waypoint we are concerned with. I can elaborate further if this is still not clear.

Peter Verkaik

If you assume the horizontal axis is the start reference, then the 3rd
point assumed is (1,0), giving the angle (x,y)->(0,0)->(1,0)
What if your point (X,Y) is at the origin? You cannot calculate
an angle then.
IMHO the calculation becomes easier if you just
use
dy = y2-y1
dx = x2-x1

angle = atan2(dy,dx)

Here is the atan2() from my Trig class

· /**
·· * Calculate arctan(y/x)
·· *
·· * @param y Vertical signed value
·· * @param x Horizontal signed value
·· * @return Angle in 0.1 degree units (-1799 to +1800)
·· */
· public static int atan2(int y, int x) {
··· int q = (x<0) ? 1 : 0; //calculate quadrant
··· q ^= (y<0) ? 3 : 0;
··· y = UnsignedIntMath.abs(y);
··· x = UnsignedIntMath.abs(x);
··· if (y == x) {
····· q = q*900 + 450; //this returns 450 in case y=x=0
····· return (q>1800) ? q-3600 : q;
··· }
··· if (x == 0) return (q==3) ? -900 : 900;
··· boolean inverse = (y > x);
··· int f = (inverse) ? UnsignedIntMath.ufrac(x,y) : UnsignedIntMath.ufrac(y,x);
··· int a = UnsignedIntMath.umulf(9204,f); // 9204*(F/65536)
//··· int n = f>>>1;
//··· int d = (short)32768 + UnsignedIntMath.umulf(a,f);
//··· int r = UnsignedIntMath.umulf(1800,UnsignedIntMath.ufrac(n,d)); // 1800*(n/d)
//··· r = UnsignedIntMath.umulf(r,20861); // 1800*(n/d)*(1/pi)
//··· int n = f;
··· int d = (short)32768 + UnsignedIntMath.umulf(a,f);
··· int n = UnsignedIntMath.umulf((short)32768,f); // f*(1/pi)
··· int r = UnsignedIntMath.umulf(573,UnsignedIntMath.ufrac(n,d)); // 900*(n/d)
··· if (inverse) r = 900-r;
··· switch (q) {
····· case 1: r = 1800-r;
············· break;
····· case 2: r = 1800+r;
············· break;
····· case 3: r = 3600-r;
··· }
··· return (r>1800) ? r-3600 : r;
· }

No calculation for square root required. Only multiplication of integer with fraction.

explanation:

· /*
··· arctan approximation
··················· (y/x)··········· 1800
··· atn =

---

* ----················ 0.1 deg units, for |y/x|<1
········· 1 + 0.280872*(y/x)*(y/x)··· pi
····················· (y/x)·············· 1800
······· =

---

* ----··········· 0.1 deg units, for |y/x|<1
········· 1 + (18407/65536)*(y/x)*(y/x)··· pi
··· |y/x|<1, so y/x = F/65536
························ (F/65536)··············· 1800
··· atn =

---

* ----··· 0.1 degree units, for 0<=F<= 65535
········· 1 + (18407/65536)*(F/65536)*(F/65536)··· pi
························· F·················· 1800
······· =

---

* ----······· 0.1 degree units, for 0<=F<= 65535
········· 65536 + 18407*(F/65536)*(F/65536)··· pi
························ F/2················ 1800
······· =

---

* ----········ 0.1 degree units, for 0<=F<= 65535
········· 32768 + 9204*(F/65536)*(F/65536)··· pi········· 1/pi = 20861/65536
··· for |y/x|>1 use atn(y/x) = 900 - atn(x/y)
· */

regards peter

Post Edited (Peter Verkaik) : 9/11/2007 5:24:53 AM GMT

ever

If (x, y) is at the origin, then the robot has achieved its goal of navigating to the waypoint! Nice program by the way, Ill try it out. It is probably more efficient, no iteration. I use brute force, you use elegance. On such a resource constrained device I cannot afford brute force.

Nice Work,
Weston