Fixed-point atan2

First pub­lished Feb­ru­ary 6, 2012 then tak­en down for revi­sion. Updat­ed fur­ther in Jan­u­ary 2017 to cor­rect errors and expand expla­na­tions.

atan2 is a freak­ing use­ful func­tion: given some y and x1, it com­putes their “four-quad­rant” arc­t­an­gent2, which is basi­cal­ly that coordinate’s angle in polar form.

The­se two beau­ti­ful Wikipedia dia­grams explain why it’s so much bet­ter than the naïve atan(y/x):

Atan2DiagramAtanDiagram

It also shows the rea­sons why I suf­fered a stroke of unsan­i­ty and whipped up my own atan2 func­tion in Q15 fixed point/1.0.15/1.15/whycan’twehaveonenameforthis for­mat that’s so com­mon­ly used by DSP chips:

  1. You see how smooth that thing is? That’s because it was gen­er­at­ed in some high-falutin’ float­ing point on a fan­cy machine with a sweet FPU. Well, I pro­gram itty-bit­ty 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 pop­u­lar fixed-point for­mats is Q15, which is tak­ing a 16-bit signed two’s com­ple­ment inte­ger3 and instead of hav­ing it rep­re­sent [-32768, 32767], have it rep­re­sent [-1, 1 - 2^{-15}]4. I used it out of con­ve­nience since I knew a few of the tricks for mak­ing Q15 com­pu­ta­tions go fast.
  2. The range on atan2 out­put is [-\pi, \pi]. That’s nice and math­emag­i­cat­i­cal, but not so use­ful an angle rep­re­sen­ta­tion on a micro­con­troller. See, first, that doesn’t fit inside of a Q15 num­ber. Then even if it did, it’d still be waste­ful to have your angle’s range be book­end­ed by tran­scen­den­tal num­bers. You got only 16 bits! You want every pos­si­ble 16-bit num­ber to be a valid and use­ful angle, even­ly cov­er­ing the whole range of pos­si­ble angu­lar posi­tions still with­out redun­dan­cy. For exam­ple, there shouldn’t be dif­fer­ent rep­re­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 cir­cle [-\frac{2^{15}}{2^{16}}\text{ turns}, \frac{2^{15} - 1}{2^{16}}\text{ turns}] which is real­ly [-\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 map­ping does is assign a scal­ing fac­tor between radi­ans and 16-bit num­bers in a unit we’ll call bina­ry radi­ans, or brads. Specif­i­cal­ly, 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}

How­ev­er, this par­tic­u­lar fac­tor does more than only pre­serve pre­ci­sion. It aligns the over­flow bound­ary of 16-bit inte­gers to the wrap­ping that hap­pens to angles each turn. It ele­gant­ly exploits the mod­u­lar arith­metic prop­er­ty of bina­ry num­bers 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 hav­ing to per­form mod­u­lo dur­ing angle com­pu­ta­tions to keep them “in range,” includ­ing the fair­ly expen­sive fmod.

Even more con­ve­nient­ly, signed bina­ry inte­gers can be con­vert­ed to and from unsigned “for free,” as they are sim­ply dif­fer­ent ways to inter­pret the same bit pat­tern. Guess what? This works per­fect­ly on angles in brads because it’s anal­o­gous to con­vert­ing between [-\pi, \pi) and [0, 2\pi). As an exam­ple, let’s say you have the angle \frac{4}{3}\pi, ini­tial­ly rep­re­sent­ed 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 con­ver­sions of angles to non-neg­a­tive val­ues and auto­mat­ic rota­tion wrap­ping are incred­i­bly pow­er­ful for dig­i­tal com­pu­ta­tion. We can do wicked fast things like lim­it­ed-pre­ci­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 con­tents of the int16_t num­bers above are not bra­di­ans, but “nor­mal” Q15 fixed point num­bers rep­re­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 arc­t­an­gent near zero, where it’s smooth and eas­i­ly approx­i­mat­ed. Inputs out­side of that domain can be manip­u­lat­ed into it, pro­vid­ed that the out­put is adjust­ed for those manip­u­la­tions.

“Derive” arctangent approximation

To imple­ment atan2, I whipped out Chap­ter 18 from Stream­lin­ing Dig­i­tal Sig­nal Pro­cess­ing, which had sev­er­al approx­i­ma­tions for atan, includ­ing the fol­low­ing for­mu­la:

\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­mi­al5 approx­i­ma­tion good to a worst-case error of 0.22° for -1 \le x \le 1.

Convert units

Now this is sin­gle-operand arc­t­an­gent, so its para­me­ter x is real­ly the slope \frac{y}{x} as under­stood by atan2. Hence, the lim­it­ed 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 con­vert to turns, since that’s bit eas­ier 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 mul­ti­pli­ca­tions and one addi­tion as opposed to the three mul­ti­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 for­mat and I want its out­put angle be one turn

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

in 16-bit brads, I actu­al­ly need to scale the approx­i­ma­tion back up to deal with the 1.15→0.16 fixed point con­ver­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 han­dle inputs not in octant I and VIII, I chose to manip­u­late the coor­di­nates and exploit the sym­me­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 arc­t­an­gent:

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

The result then needs to be rotat­ed 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­geth­er, 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 fur­ther here by using arctangent’s prop­er­ty 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 out­put sign of \text{arctan}^\dagger.
  3. Add 90° or 0.25 turns or 16384 brads to the result.

For the oth­er octants, sim­i­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 rotat­ed back.

Last­ly as a detail, the bound­ary con­di­tion x = y needs to be test­ed for. This is because the result of \frac{y}{x}, 1, can’t be rep­re­sent­ed in the Q1.0.15 for­mat. For­tu­nate­ly, the result of atan2 for those cas­es is obvi­ous. That is, with the excep­tion of x = y = 0, which we just have to test for explic­it­ly and han­dle as an error.

Book mark

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

Never trust a Canadian

It looks like the octants IV and V equa­tion accounts for the sign change across the signed atan2 dis­con­ti­nu­ity. How­ev­er, since bina­ry angle dis­con­ti­nu­ity is han­dled by inte­ger over­flow aligned with the out­put of my atan2, I can han­dle octants IV and V sim­ply as the 180° rotat­ed ver­sion of octants I and VII.

nabs rocks

So any­ways, here’s the imple­men­ta­tion. I should note that the code relies on a q15_mul func­tion that per­form round­ing of the pro­duct from the 32-bit inter­me­di­ate to 16-bit result for max­i­mum accu­ra­cy. Also, s16_nabs is a func­tion for neg­a­tive absolute val­ue. Why use nabs? Well, abs is unde­fined for the most neg­a­tive num­ber, and chances are if you plug the Q15 val­ue for -1 into abs, you’ll get -1 back out. Neg­a­tive absolute val­ue, on the oth­er hand, is pret­ty easy to keep ful­ly defined.

To test my approx­i­mate arc­t­an­gent, I test­ed all 2^{16}\times2^{16}=4294967296 pos­si­ble inputs and com­put­ed their errors rel­a­tive to the dou­ble-pre­ci­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 los­ing much pre­ci­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 say­ing is this: relax, this code was test­ed by brute force. 😉 

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

A DSP library might have faster/better ver­sions 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 com­plex num­ber, if you swing that way. []
  2. In reg­u­lar atan, you’d write atan(y/x), which is why atan2‘s argu­ment order is y then x. []
  3. If that’s con­fus­ing, look them up before con­tin­u­ing because it’s about to get worse. []
  4. Trust me, +1 real­ly is miss­ing from the range. []
  5. Note that since we’re approx­i­mat­ing an odd func­tion with a qua­drat­ic, we need that absolute val­ue in the equa­tion to make the sec­ond-order term become even. []
  6. Actu­al­ly octants IV and V in my non-neg­a­tive out­put atan2, but that’s han­dled by the signed↔unsigned con­ver­sion and is an imple­men­ta­tion detail. []
  7. Con­ve­nient­ly, but not coin­ci­den­tal­ly, the swap­ping pre­vents the quo­tient from exceed­ing 1. []