Fixed-point atan2

First published Febru­ary 6, 2012 then taken down for revi­sion. Updated further in Janu­ary 2017 to correct errors and expand expla­na­tions.

atan2 is a freak­ing useful func­tion: given some y and x1, it computes their “four-quad­rant” arct­an­gent2, which is basi­cally that coordinate’s angle in polar form.

These two beau­ti­ful Wikipedia diagrams explain why it’s so much better than the naïve atan(y/x):

Atan2DiagramAtanDiagram

It also shows the reasons why I suffered a stroke of unsan­ity and whipped up my own atan2 func­tion in Q15 fixed point/1.0.15/1.15/whycan’twehaveonenameforthis format that’s so commonly used by DSP chips:

  1. You see how smooth that thing is? That’s because it was gener­ated in some high-falutin’ float­ing point on a fancy machine with a sweet FPU. Well, I program itty-bitty embed­ded micro­con­trollers with nary a hard­ware inte­ger divider, so I use fixed-point arith­metic for my math. One of the more popu­lar fixed-point formats is Q15, which is taking a 16-bit signed two’s comple­ment inte­ger3 and instead of having it repre­sent [-32768, 32767], have it repre­sent [-1, 1 - 2^{-15}]4. I used it out of conve­nience since I knew a few of the tricks for making Q15 compu­ta­tions go fast.
  2. The range on atan2 output is [-\pi, \pi]. That’s nice and math­emag­i­cat­i­cal, but not so useful an angle repre­sen­ta­tion on a micro­con­troller. See, first, that doesn’t fit inside of a Q15 number. Then even if it did, it’d still be waste­ful to have your angle’s range be book­ended by tran­scen­den­tal numbers. You got only 16 bits! You want every possi­ble 16-bit number to be a valid and useful angle, evenly cover­ing the whole range of possi­ble angu­lar posi­tions still with­out redun­dancy. For exam­ple, there shouldn’t be differ­ent repre­sen­ta­tions for both \frac{2}{3}\pi and -\frac{4}{3}\pi, since they’re the same angu­lar posi­tion. So, let’s map our 16-bit range [-2^{15}, 2^{15} - 1] or [-32768, 32767] to (almost) a full turn of the circle [-\frac{2^{15}}{2^{16}}\text{ turns}, \frac{2^{15} - 1}{2^{16}}\text{ turns}] which is really [-\frac{1}{2}\text{ turns}, \frac{32767}{65536}\text{ turns}].

Fixed point angular representation

A turn here is of course a rota­tion of 2\pi radi­ans, so all this mapping does is assign a scal­ing factor between radi­ans and 16-bit numbers in a unit we’ll call binary radi­ans, or brads. Specif­i­cally, between one turn’s worth of radi­ans and the whole 16-bit space’s worth of brads.

\text{angle in radians} = \frac{2\pi\text{ rad}}{2^{16}\text{ brad}} \times \text{angle in binary}

However, this partic­u­lar factor does more than only preserve preci­sion. It aligns the over­flow bound­ary of 16-bit inte­gers to the wrap­ping that happens to angles each turn. It elegantly exploits the modu­lar arith­metic prop­erty of binary numbers to provide angle wrap­ping and canon­i­cal­iza­tion. Just like adding 1° to 359° gives 0°, adding 1 to 32767 fixed point units gives -32768 fixed point units.

This avoids having to perform modulo during angle compu­ta­tions to keep them “in range,” includ­ing the fairly expen­sive fmod.

Even more conve­niently, signed binary inte­gers can be converted to and from unsigned “for free,” as they are simply differ­ent ways to inter­pret the same bit pattern. Guess what? This works perfectly on angles in brads because it’s anal­o­gous to convert­ing between [-\pi, \pi) and [0, 2\pi). As an exam­ple, let’s say you have the angle \frac{4}{3}\pi, initially repre­sented as 43691 brads:

\begin{aligned} \frac{4}{3}\pi\text{ rad} \times \frac{2^{15}\text{ brad}}{\pi\text{ rad}} &= 43691\text{ brad} \\ &\equiv -21845 \pmod{2^{16}}\text{ brad} \\ &= -21845 \text{ brad} \times \frac{\pi\text{ rad}}{2^{15}\text{ brad}} \\ &\approx 0.66666\pi\text{ rad}\\ &\approx -\frac{2}{3}\pi\text{ rad} \end{aligned}

Free conver­sions of angles to non-nega­tive values and auto­matic rota­tion wrap­ping are incred­i­bly power­ful for digi­tal compu­ta­tion. We can do wicked fast things like limited-preci­sion sin and cos lookup tables:

const int16_t sin_lut[4096]; // a 2^12 table of precomputed sin values
const uint16_t angle; // angle in 16-bit binary radians

// right shift because there are 16x possible angles as table entries
const int16_t sin_of_angle = sin_lut[angle >> 4];

// cos(x) = sin(x + pi / 2), pi / 2 rad = 16384 brad
const int16_t cos_of_angle = sin_lut[(angle + 16384) >> 4];

// we can also round to the nearest entry
const int16_t sin_of_rounded_angle = sin_lut[(angle + (1 << 3)) >> 4];

// or linearly interpolate between them
const int16_t lower = sin_lut[angle >> 4];
const int16_t upper = sin_lut[(angle >> 4) + 1];
const int16_t angle_fraction = (angle & 0xF) << (15 - 4);
const int16_t better_sin = lerp(lower, upper, angle_fraction);

Note that the contents of the int16_t numbers above are not bradi­ans, but “normal” Q15 fixed point numbers repre­sent­ing the range [-1, 1 - 2^{-15}].

Implementing atan2

Given that \text{atan2}(x, y) = \text{arctan}\left(\frac{y}{x}\right) when x, y are in quad­rants I and IV, I can imple­ment atan2 using arct­an­gent near zero, where it’s smooth and easily approx­i­mated. Inputs outside of that domain can be manip­u­lated into it, provided that the output is adjusted for those manip­u­la­tions.

“Derive” arctangent approximation

To imple­ment atan2, I whipped out Chap­ter 18 from Stream­lin­ing Digi­tal Signal Process­ing, which had several approx­i­ma­tions for atan, includ­ing the follow­ing formula:

\begin{aligned} \text{arctan}^\star(x) &= \frac{\pi}{4}x + 0.273 x (1 - |x|) \\                        &\approx \text{arctan}(x)\text{, } x \in [-1, 1] \end{aligned}

That’s a local poly­no­mial5 approx­i­ma­tion good to a worst-case error of 0.22° for -1 \le x \le 1.

Convert units

Now this is single-operand arct­an­gent, so its para­me­ter x is really the slope \frac{y}{x} as under­stood by atan2. Hence, the limited domain of the approx­i­ma­tion func­tion is equiv­a­lent to octants I and VIII in terms of atan26. The range is [-\frac{\pi}{4}, \frac{\pi}{4}] over that domain. We want to convert to turns, since that’s bit easier to work with:

\begin{aligned} \text{arctan}^\star_\tau(x) &= \frac{1}{2\pi}\text{ arctan}^\star(x) \\                             &= \frac{1}{2\pi}\left(\frac{\pi}{4}x + 0.273 x (1 - |x|)\right) \\                             &= \frac{1}{8}x + \frac{0.273}{2\pi} x (1 - |x|) \\                             &= x\left(\frac{1}{8} + \frac{0.273}{2\pi} - \frac{0.273}{2\pi}|x|\right) \end{aligned}

Now the range is \left[-\frac{1}{8}, \frac{1}{8}\right] in units of turns (or τ). The last form requires only two multi­pli­ca­tions and one addi­tion as opposed to the three multi­pli­ca­tions and two addi­tions required by the orig­i­nal form.

One more thing: because the final func­tion takes in operands with a range of

y, x \in [-1, 1)_{Q15}

in Q15 format and I want its output angle be one turn

\theta \in \left[-\frac{1}{2}, \frac{1}{2}\right)_\tau

in 16-bit brads, I actu­ally need to scale the approx­i­ma­tion back up to deal with the 1.15→0.16 fixed point conver­sion:

\begin{aligned} \text{arctan}^\dagger(x) &= 2\text{ arctan}^\star_\tau(x) \\                          &= x\left(\frac{1}{4} + \frac{0.273}{\pi} - \frac{0.273}{\pi}|x|\right) \end{aligned}

Exploit function symmetries

To handle inputs not in octant I and VIII, I chose to manip­u­late the coor­di­nates and exploit the symme­tries of atan2. For exam­ple, if our input is in octants II and III, then we rotate it by -90°:

\begin{aligned} \begin{bmatrix} x' \\ y' \\ \end{bmatrix} &= \begin{bmatrix} \cos -90\degree & -\sin -90\degree \\ \sin -90\degree & \cos -90\degree \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ \end{bmatrix} \\ &= \begin{bmatrix} 0 & 1 \\ -1 & 0 \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ \end{bmatrix} \\ &= \begin{bmatrix} y \\ -x \\ \end{bmatrix} \end{aligned}

Now x′ and y′ are in octants I and VIII—as required—and we can pass them to our approx­i­mate arct­an­gent:

\text{arctan}^\dagger\left(\frac{y'}{x'}\right)

The result then needs to be rotated 90° to put it back into octants II and III. This is easy since the result is already in angu­lar units: just add 90°. Alto­gether, it looks like this:

\begin{aligned} \text{atan2}(y, x) &= \text{arctan}^\dagger\left(\frac{y'}{x'}\right) + 90\degree \\ &= \text{arctan}^\dagger\left(\frac{-x}{y}\right) + 90\degree \\ &= 90\degree - \text{arctan}^\dagger\left(\frac{x}{y}\right) \end{aligned}

I took it a step further here by using arctangent’s prop­erty as an odd func­tion: \text{arctan}(-x) = -\text{arctan}(x). In terms of imple­men­ta­tion, this means:

  1. Invert the divi­sion by swap­ping the two argu­ments7.
  2. Flip the output sign of \text{arctan}^\dagger.
  3. Add 90° or 0.25 turns or 16384 brads to the result.

For the other octants, simi­lar trans­for­ma­tions can be applied to put them into the valid domain of \text{arctan}^\dagger and results can like­wise be rotated back.

Lastly as a detail, the bound­ary condi­tion x = y needs to be tested for. This is because the result of \frac{y}{x}, 1, can’t be repre­sented in the Q1.0.15 format. Fortu­nately, the result of atan2 for those cases is obvi­ous. That is, with the excep­tion of x = y = 0, which we just have to test for explic­itly and handle as an error.

Book mark

My deriva­tions here diverged from the four-quad­rant formula in the book. I marked these as wrong in 2012, but I’m not so sure they’re differ­ent from my imple­men­ta­tion when account­ing for signed↔unsigned angu­lar output.

Never trust a Canadian

It looks like the octants IV and V equa­tion accounts for the sign change across the signed atan2 discon­ti­nu­ity. However, since binary angle discon­ti­nu­ity is handled by inte­ger over­flow aligned with the output of my atan2, I can handle octants IV and V simply as the 180° rotated version of octants I and VII.

nabs rocks

So anyways, here’s the imple­men­ta­tion. I should note that the code relies on a q15_mul func­tion that perform round­ing of the prod­uct from the 32-bit inter­me­di­ate to 16-bit result for maxi­mum accu­racy. Also, s16_nabs is a func­tion for nega­tive absolute value. Why use nabs? Well, abs is unde­fined for the most nega­tive number, and chances are if you plug the Q15 value for -1 into abs, you’ll get -1 back out. Nega­tive absolute value, on the other hand, is pretty easy to keep fully defined.

To test my approx­i­mate arct­an­gent, I tested all 2^{16}\times2^{16}=4294967296 possi­ble inputs and computed their errors rela­tive to the double-preci­sion atan2 imple­men­ta­tion in the GNU C library. I got a worst-case error of 0.221°, which means I’m not losing much preci­sion or intro­duc­ing quan­ti­za­tion error by using fixed-point, and there’s an root mean square (RMS) error of about 0.0004 turns.

What I’m saying is this: relax, this code was tested by brute force. 😉

s16_nabs, q15_mul, q15_div, and of course fxpt_atan2 are all included in this MIT-licensed source file: fxpt_atan2.c

A DSP library might have faster/better versions of some of those func­tions that may take advan­tage of your microcontroller’s DSP ALU.

Codes!

/**
 * 16-bit fixed point four-quadrant arctangent. Given some Cartesian vector
 * (x, y), find the angle subtended by the vector and the positive x-axis.
 *
 * The value returned is in units of 1/65536ths of one turn. This allows the use
 * of the full 16-bit unsigned range to represent a turn. e.g. 0x0000 is 0
 * radians, 0x8000 is pi radians, and 0xFFFF is (65535 / 32768) * pi radians.
 *
 * Because the magnitude of the input vector does not change the angle it
 * represents, the inputs can be in any signed 16-bit fixed-point format.
 *
 * @param y y-coordinate in signed 16-bit
 * @param x x-coordinate in signed 16-bit
 * @return angle in (val / 32768) * pi radian increments from 0x0000 to 0xFFFF
 */
uint16_t fxpt_atan2(const int16_t y, const int16_t x) {
    if (x == y) { // x/y or y/x would return -1 since 1 isn't representable
        if (y > 0) { // 1/8
            return 8192;
        } else if (y < 0) { // 5/8
            return 40960;
        } else { // x = y = 0
            return 0;
        }
    }
    const int16_t nabs_y = s16_nabs(y), nabs_x = s16_nabs(x);
    if (nabs_x < nabs_y) { // octants 1, 4, 5, 8
        const int16_t y_over_x = q15_div(y, x);
        const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(y_over_x));
        const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, y_over_x);
        if (x < 0) { // octants 1, 8
            return unrotated;
        } else { // octants 4, 5
            return 32768 + unrotated;
        }
    } else { // octants 2, 3, 6, 7
        const int16_t x_over_y = q15_div(x, y);
        const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(x_over_y));
        const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, x_over_y);
        if (y > 0) { // octants 2, 3
            return 16384 - unrotated;
        } else { // octants 6, 7
            return 49152 - unrotated;
        }
    }
}
  1. Or a and b from a complex number, if you swing that way. []
  2. In regu­lar atan, you’d write atan(y/x), which is why atan2‘s argu­ment order is y then x. []
  3. If that’s confus­ing, look them up before contin­u­ing because it’s about to get worse. []
  4. Trust me, +1 really is miss­ing from the range. []
  5. Note that since we’re approx­i­mat­ing an odd func­tion with a quadratic, we need that absolute value in the equa­tion to make the second-order term become even. []
  6. Actu­ally octants IV and V in my non-nega­tive output atan2, but that’s handled by the signed↔unsigned conver­sion and is an imple­men­ta­tion detail. []
  7. Conve­niently, but not coin­ci­den­tally, the swap­ping prevents the quotient from exceed­ing 1. []