[Back to Home Page]

www.RomanBlack.com

XY vector to integer degree fast algorithm
This generates a degree (0-360) from any XY vector - 15th Jan 2011


What is it?

I needed a fast simple mathematical algorithm to run on a PIC and generate a result in "degrees" as an integer 0-360 degrees, from an input which is an XY vector (which generally represents a movement). I designed this algorithm out of combining 2 imperfect systems, the combined result is fast and accurate enough for many simple tasks.

The XY vector can have positive or negative values for both X and Y, and one value can be zero (but not both). A big advantage is automatic scaling of the X and Y values that can be in a very large range.


Why is it useful?

Traditional ways of generating degrees from an XY vector are problematic;
1. They require large, slow ArcTan() or ArcCos() functions, or look-up tables
2. They have problems if a value is zero
3. They can't do more than 180 degrees
4. They usually require an additional ratio calc to be done first
5. They are very difficult to implement on a small PIC

This algorithm has some benefits;
1. It is very small and fast compared to trig math!
2. It is even smaller if you just need one quadrant 0-90 degrees or 0-45 degrees
3. It only requires 16bit multiply/divide in most cases (no 32bit math)
4. It has a simple integer output; 0-360 degrees
5. It works with any X and Y values, +/- and one value can be zero
6. It automatically avoids a divide by zero error, as long as either X or Y is non-zero

Limitations of this algorithm;
1. Accuracy! Its accuracy is +/- 1 degree worst case, but usually better
2. Its output is 0-360 degrees, not 0-359 (that is easily fixed if needed)
3. X and Y must be between -1456 and 1456 (for 16bit version)

A big benefit of this algorithm is its flexibility, being able to use any X and Y values. If you use a 32bit multiply and divide which is a little slower than the 16bit multiply and divide it is possible to use very large X and Y values. This makes it ideal for robot navigation or CNC machine coordinates where you just give it the XY values from the last move and it returns the direction as a 0-360 degree heading.

The algorithm is also ideal for processing the output of an X Y accelerometer, and providing a tilt angle in degrees. This can be done even with a very small cheap PIC, a 10F or 12F series.





The algorithm consists of 3 separate processes;

1. A very fast way to generate a 0 - 90 degree single quadrant result, that has up to 4 degrees error
This process requires a 16bit*8bit multiply, followed by a 16bit/16bit divide. This is fast and code efficient even on a small PIC.

2. A very fast and reliable way of reducing the 4 degree max error to less than +/-1 degree.
This process only requires simple comparisons of a 1byte value (0-90 degrees) and some simple additions.

3. (if needed) A very fast way of converting 0-90 degree value to full 0-360 degrees.
This only requires a few bit tests and a 16bit addition or subtraction.



Simple version in C code;

This version requires X >= Y and returns a 0-45 degree result. It is the simplest form of the algorithm and only uses the first 2 processes. 

   // 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!



Full 0-360 version in C code;

This version takes any +/- XY vector and returns degrees 0-360 It looks long and messy but is quite compact in ROM and is fast to execute. Speed could be optimised further still although I don't think it is really necessary. 

   // 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! at least one of the X or Y values must be non-zero!
   // This is the full version, for all 4 quadrants and will generate
   // the angle in integer degrees from 0-360.
   // Any values of X and Y are usable including negative values provided
   // they are between -1456 and 1456 so the 16bit multiply does not overflow.

   unsigned char negflag;
   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;            // (and they will be destroyed)
   unsigned int ux;
   unsigned int uy;

   // Save the sign flags then remove signs and get XY as unsigned ints
   negflag = 0;
   if(x < 0)
   {
      negflag += 0x01;    // x flag bit
      x = (0 - x);        // is now +
   }
   ux = x;                // copy to unsigned var before multiply
   if(y < 0)
   {
      negflag += 0x02;    // y flag bit
      y = (0 - y);        // is now +
   }
   uy = y;                // copy to unsigned var before multiply

   // 1. Calc the scaled "degrees"
   if(ux > uy)
   {
      degree = (uy * 45) / ux;   // degree result will be 0-45 range
      negflag += 0x10;    // octant flag bit
   }
   else
   {
      degree = (ux * 45) / uy;   // degree result will be 0-45 range
   }

   // 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!

   // Invert degree if it was X>Y octant, makes 0-45 into 90-45
   if(negflag & 0x10) degree = (90 - degree);

   // 3. Degree is now 0-90 range for this quadrant,
   // need to invert it for whichever quadrant it was in
   if(negflag & 0x02)   // if -Y
   {
      if(negflag & 0x01)   // if -Y -X
            degree = (180 + degree);
      else        // else is -Y +X
            degree = (180 - degree);
   }
   else    // else is +Y
   {
      if(negflag & 0x01)   // if +Y -X
            degree = (360 - degree);
   }

The code above compiled on a PIC12F675 and used only 196 ROM, with an extra 29 and 43 ROM needed for the 16bit divide and multiply routines.



32-bit version in C code;

This version can use very large values for the +/- XY vector! It is identical to the above version, but uses 32bit variables and the compiler will handle the small overhead of doing the 32bit multiply and divide. This is a similar code size to the 16bit version above, but requires the 32bit mult and divide functions that (if not already included) cost about 350 to 400 more ROM to the project.

It is still quite usable on a small PIC as it is about 200 ROM (but also requires 32 multiply and divide functions). Using 32bit it can now accept +/- XY values anywhere in the range -95443717 to 95443717 and still return a simple integer 0-360 degrees. 

   // For 32bit version substitute these 5 variables below instead of the vars shown above;

   unsigned long degree;     // this will hold the result
   signed long x;            // these hold the XY vector at the start
   signed long y;            // (and their sign will be destroyed)
   unsigned long ux;
   unsigned long uy;



Optimisations!

The algorithm can have an imprtant optimisation to reduce ROM by removing the multiply and divide functions. If using the 32bit version this can save about 350 to 400 ROM (on a PIC16) by removing the 32bit multiply and 32bit divide functions.

This will be slower to execute but the nature of the algorithm makes this not as bad as would be expected. This is because the multiply is guaranteed to be 45 additions, and the divide is guaranteed to be between 0 and 45 subtractions for any possible values of X and Y. See code below. 

   // To remove the 32bit multiply and divide functions and save lots
   // of ROM replace this mult/divide line;

      degree = (uy * 45) / ux;   // degree result will be 0-45 range

   // With this successive addition and subtraction code;
   // (Which takes a maximum of; 45 additions, 0-45 subtractions)

      unsigned char i;

      // do uy*45 by doing 45 additions
      degree = 0;
      i = 45;
      while(i)
      {
         degree += uy;
         i--;
      }

      // do degree/ux by doing from 0 to 45 subtractions as needed
      i=0;
      while(degree >= ux)
      {
         degree -= ux;
         i++;          // count number of divisions
      }
      degree = i;      // load result back into degree


This algorithm and code examples on this page are open-source, use them as you like but please mention www.RomanBlack.com.


- end -

[Back to Home Page]