Fast way to derive an angle from XY?

[Mr RB]

Hi, does anyone know a fast easy way to derive an angle; 0-359 degrees from signed XY values?

Examples;
X 10, Y 10 = 45'
X 0, Y 7 = 0'
X-40, Y 20 = 334'

I can use 32bit math multiply and divides etc as I have already used these in the project so the ROM for these functions is already used, but ideally it should not take too many cycles and not use too much ROM to do the angle calc.

The good news is it only needs the angle as an integer 0 - 359 and an error of 2' or 3' is acceptable, so it doesn't need trig math.

The XY values are signed 16bit values, although if needed I can scale these down to signed 8bit values if it gives enough saving in speed or ROM.

If anyone has a clever angle kludge that gets me within 2 degrees of the right answer please speak up!

 

[dknguyen]

It's always going to take up either more memory or more cycles. Saying that you want to optimize all factors doesn't tell us much because ideally everything would work itself out.

2 degrees is coarse though. Since it's so coarse a 45 entry look-up table would work just fine (2 degree increments for a single quadrant which spans 90 degrees). Just normalize your X/Y ratios and take the quadrant into account. And of course you have cordic which is overkill.

 

[MikeMl]

Maybe you can do something with the equations presented here.:

 

[Mosaic]

Archimedian Spiral

An Archimedian Spiral eqn might be of use here.

 

[skyhawk]

First, based on the signs and relative sizes of x and y, use logic to reduce the problem to finding an angle between 0 and 45.

If tan θ = r = x/y where 0 <= x <= y then θ = (60 - 15*r)*r with an error of less than 1/2 degree.

Finally convert back to the original problem by adding or subtracting θ from the appropriate multiple of 90.

 

[Mosaic]

I think between 0-45 is insufficient when considering quadrants, 0 - 90 deg is required.

I did a series in Excel from 2 to 88 deg, TAN(Angle) = y/x (ratio) when multiplied by 256 progressed from 16 bit integer 9 thru 7331 exponentially.
I'd suggest then multiplying the coordinate Y value by 256 then dividing by the coord. X value to obtain the 16 bit lookup integer for the data table mapping the 'degree vs 256(y/x)' in 2 degree steps.
2 degree steps from 2' to 88' is about 44 table entries. Thus any ([y*256]/x) value less than 9 is trending to 0' and more than 7331 is trending to 90', with anything in between being satisfied by the look up table and accuracy being < 1' error (1/2 of 2')

 

[skyhawk]

I think between 0-45 is insufficient when considering quadrants, 0 - 90 deg is required.

45 degrees is sufficient. Reverse x and y, then compute the complementary angle.

 

[Mosaic]

Doing double divisions would slow the process significantly. More than a lookup table I believe. But this depends on the MCU. A 16F would need to avoid the double divisions for sure, prob an 18F as well as it has no DIV inst.
What's useful is that the index on the look up table would effectively be the answer in degrees. Consider 44, 8 bit byte pairs (16 bit values) being searched. The indirect addressing byte would be incremented by 2 after each comparison thus being equal to the angle when the compare is true.

Edit:
Also, to halve the lookup table iterations, once the solved (256*Y)/x exceeds 255, or the high byte is non zero then start the search from the index = 46 (the latter half of the table). Thus 22 (or less) look up operations will solve the 0-90 degree domain. The median will be about 11 look up operations. The MCU cycles consumed by these 11 ops will have to be weighed against the double divsion overhead of the method you (Skyhawk) suggested.

 

[Diver300]

I've done this on an 18F and also on a 24F.

As others suggested here, I worked out which 45 degree sector the angle is in, from the signs of the x and y, and the relative sizes of the absolute values of x and y.

To get the angle accurate to about 1 degree, I used a short lookup table with linear interpolation between the table entries.

Here is the code for the 24F

;angle is now 0 - 0xffff representing tan(angle) in the range 0 - 45 deg
;there is a lookup table for the values, 0x00, 0x40, 0x80, 0xC0 and 0x100
;the value looked up is 4 times the tan in degrees
;the values either side of the current tan are looked up
;the last 6 bits are multiplied by the difference in the lookup table
;we want the atan of the number /256

	mov		angle, w0
	lsr		w0, #14, w0 	;take most significant two bits

	rcall	        arc_tan_table	;Get a1=atan( x>>14)
	
	mov		w1,w2			;store in w2
	inc		w0, w0
	rcall	        arc_tan_table	;get a2=atan((x>>14)+1)

	sub		w1, w2, w1		;now w1 is the difference
	mov		#0b0011111111111111, w0
	and		angle, wreg
	sl		w0, #2, w0		;multiply by 4 so that 0x3fff, which was nearly the next notch, has the same effect
	mul.uu       w0, w1, w0		;w1 is now the less significant part multipled by the atan difference
	add		w1, w2, w0		;add to the lookup value
	lsr		w0, #4, w0		;divide by 16
	mov		w0, angle		;store as angle, 0 - 45 degrees

	rcall	        angle_range_table

	btsc	        w0, #9
	neg		angle			;negate angle if needed

	bclr	        w0, #9
	
	add		angle			;and that should be it!

	mov		#360, w0
	cp		angle
	btsc	        SR, #C
	clr		angle			;clear angle for pedants if it is still 360 or more!

	bra 	        next_bit_of_code





;angle_range is 0 - 7 here
;bit 0 is set if the angle is south
;bit 1 is set if the angle is west
;bit 2 is set if angle is closer to north/south than east/west
;The lookup table returns the angle in degrees, and with a 1 in bit 9 position if the angle needs to be reversed
angle_range_table:
	mov		angle_range, w0
	and		#7, w0
	bra		w0
	retlw		#270+512, w0
	retlw		#270, w0
	retlw		#90, w0
	retlw		#90+512, w0
	retlw		#180, w0
	retlw		#360+512, w0
	retlw		#180+512, w0
	retlw		#0, w0	


arc_tan_table:
	cp		w0, #5
	btss	        SR, #C
	bra		w0
	retlw	       #12, w1
	retlw	       #237, w1
	retlw	       #437, w1
	retlw	       #602, w1	;these are 16 times the arctan of the numbers
	retlw	       #732, w1

 

[Mosaic]

Umm..Diver...where do u normalise X & Y

 

[skyhawk]

Doing double divisions would slow the process significantly.

Only one division!

First find which of 8 cases you have based on the signs of x and y and whether x is larger or smaller than y. Take the absolute values of x and y and swap x and y if necessary.

Then compute tan θ = r = x/y where 0 <= x <= y. That is the only division required!

Compute arctan r using simple polynomial - two multiplications, one addision.

Find final answer based on which of 8 cases.

 

[Mosaic]

Gotcha!...I'll be using your method now 8))

 

[Mr RB]

Thanks everyone for all the replies! I didn't expect such a good response.
DKNguyen- a 45 entry lookup table sounds like one good solution.
MikeML- Thanks for the link to that page of circle/pixel math!
Diver300- That's a good idea with the short lookup table and linear interpolation between entries. That was the first system I tried yesterday.

I actually came up with a fast enough solution yesterday shortly after I posted as I was stuck in the middle of coding and needed to get it done, but I still much appreciate everyone's help and my solution is still not perfect maybe the best solution would be a combination of ideas from this thread...

Here's the system I ended up using;

**broken link removed**

One advantage is that it auto-scales for any XY values and almost instantly gives a result that is always 0-45 degrees (but has 0-4 degree error). I then fix the error by doing 4 simple byte comparisons which is quick and reliable. On seeing the replies here it shares many similarities with other people's systems so it can;t be too far from the right track.

   // Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com
   // Converts any XY values including 0 to a degree value that should be
   // within +/- 1 degree of the accurate value without needing
   // large slow trig functions like ArcTan() or ArcCos().
   // NOTE! X value must be greater than zero!
   // This is the simplest version, for one octant (half a quadrant)
   // so X must be >= Y, although any values of X and Y are usable
   // provided they are under 1456 so the 16bit multiply does not overflow.
   // the result will be 0-45 degrees range

   unsigned char tempdegree;
   unsigned char comp;
   unsigned int degree;     // this will hold the result
   signed int x;            // these hold the XY vector at the start
   signed int y;            //

   // 1. Calc the scaled "degrees"
   degree = (y * 45) / x; // note! X must be >= Y, result will be 0-45

   // 2. Compensate for the 4 degree error curve
   comp = 0;
   tempdegree = degree;     // use an unsigned char for speed!
   if(tempdegree > 22)      // if top half of range
   {
      if(tempdegree <= 44) comp++;
      if(tempdegree <= 41) comp++;
      if(tempdegree <= 37) comp++;
      if(tempdegree <= 32) comp++;  // max is 4 degrees compensated
   }
   else    // else is lower half of range
   {
      if(tempdegree >= 2) comp++;
      if(tempdegree >= 6) comp++;
      if(tempdegree >= 10) comp++;
      if(tempdegree >= 15) comp++;  // max is 4 degrees compensated
   }
   degree += comp;   // degree is now accurate to +/- 1 degree!

The full 0-360 degree code and some optimisation suggestions have been put up here; Fast Integer Degree Algorithm

Thanks again!

 

[skyhawk]

Unfortunately I used a nonstandard definitition of θ with

tan θ = r = x/y where 0 <= x<= y.

However, the previous logic follows using the standard definition

tan θ = r = y/x where 0 <= y <= x.

The interesting part here is that I proposed to calculate θ using the polynomial

θ = (60-15r)*r

This is the form I would use for calculation, but it can be rearranged to the form

θ = 45*r + 15*(1-r)*r.

This form illustrates the linear form used by Roman plus a "quadratic correction". The correction reaches a maximum value of 3.75 when r = .5. If 15 is replaced by 16, then we obtain

θ = 45*r + 16*(1-r)*r, which has a maximum correction of 4 and is nearly identical to Roman's result. In computational form this becomes:

θ = (61-16*r)*r.

This may be a particularly efficient calculation (especially in assembly), since multiplication by 16 is just a 4 bit shift to the left.

 

[Mr RB]

That's an interesting way of solving the error Skyhawk! That's made me think a bit...

Hmm, it still requires calculating the ratio first and avoiding floating point, maybe something like this?

ratio = (y*256) / x;
degrees = ((61 - (16*ratio)) * ratio) / 256; // or; degrees = ((61 - (ratio<<4)) * ratio) >> 8;

obviously using *256 and /256 to avoid handling the float...

It's going to be uneccessarily slow when only 1 or 2 degree resolution is required but it looks better than ArcTan() for apps with more resolution.

You have given me an idea, what about expressing angle in binary so 0-360' becomes 0-256, this allows use of a single byte for the "heading" which is plenty of resolution for some smaller apps that may not need actual "degrees".

In that case it can use your idea of a binary multiply;

angle = (y * 32) / x; // or; angle = (y << 5) / x;
// error will be minus 0-3 units now simplifying a staged error test;
if(angle > 4 || angle <28) angle++; // maybe can use bitwise tests?? will save a lot of speed
if(angle > 8 || angle <24) angle++;

Just some thoughts. By the way the algorithm I used above in post #13 is finished and working in the application now, and working very nicely too.