Concept 02: Rotating a Vector

▶ Interactive Demo: Rotation & Field-Oriented Drive Visualizer

Spin the robot’s heading and watch the same joystick push turn into completely different wheel commands. Toggle between the driver’s view and the robot’s view of the identical motion.

1. The Real-World Problem: The Robot Is Not Facing Forward

A driver stands behind the alliance wall holding a gamepad. They push the left stick straight away from their body, meaning: go to the far end of the field. That instruction lives in the field frame, and in field coordinates it means “increase X”.

But the robot’s motors have never heard of the field. A swerve drive accepts a forward speed and a strafe speed measured relative to its own chassis — where its own front bumper is pointing. And at this instant the robot happens to be spun 40 degrees counter-clockwise, because it just finished picking up a game piece.

Send the stick values straight through to the motors and the robot drives 40 degrees off course, diagonally into the wall. Every driver who has ever fought a robot that “goes the wrong way when it’s turned around” has met this problem. The fix is called field-oriented drive, and it is one operation: rotate the command vector from the field frame into the robot frame.

To build that operation we need to answer a smaller question first: when a vector is rotated by an angle θ, what are its new coordinates?

2. Building the Math: What Rotation Does to Coordinates

Step 1: The trigonometry we actually need

Start with a plain right triangle. Pick one of its non-right angles and call it θ. The three sides get names relative to that choice: the side touching θ that is not the hypotenuse is the adjacent side, the side across from θ is the opposite side, and the long side facing the right angle is the hypotenuse.

The ratios between those sides depend only on θ, never on how big you draw the triangle — scale the triangle up and both sides grow by the same factor, leaving the ratio untouched. Three of those ratios get names, remembered by the mnemonic SOH-CAH-TOA:

Now shrink the hypotenuse to exactly 1. Dividing by 1 changes nothing, so the two ratios collapse into something much easier to hold in your head: the adjacent side simply is cos θ, and the opposite side simply is sin θ. Drawn on a circle of radius 1 centered at the origin — the unit circle — a point at angle θ sits at coordinates (cos θ, sin θ).

That last sentence is the entire foundation of what follows. It also extends the definition past the right triangle: a right triangle cannot have a 150-degree angle, but a point on a circle can sit at 150 degrees perfectly well, and its coordinates are still called (cos 150°, sin 150°). The circle is the general definition; the triangle is the special case.

Step 2: Track only the basis vectors

Here is the trick that makes rotation easy. Instead of asking what happens to some arbitrary point, ask what happens to two very specific ones:

• î (read “i-hat”), the unit vector pointing along X, at coordinates (1, 0).
• ĵ (read “j-hat”), the unit vector pointing along Y, at coordinates (0, 1).

These two are the basis vectors, and every other vector is built from them. The vector (3, 2) is literally 3 steps along î plus 2 steps along ĵ.

Now rotate the whole plane counter-clockwise by θ.

Where does î go? It started on the unit circle at angle 0. Rotating by θ leaves it on the unit circle at angle θ. By Step 1, it lands at (cos θ, sin θ).

Where does ĵ go? It started on the unit circle at angle 90°, so it lands at angle θ + 90°, at coordinates (cos(θ + 90°), sin(θ + 90°)). You do not need to memorize the angle-addition identities to simplify this — just look at the picture. A vector 90 degrees counter-clockwise from (cos θ, sin θ) is (−sin θ, cos θ): rotating any vector a quarter turn counter-clockwise swaps its two components and negates the new first one. So ĵ lands at (−sin θ, cos θ).

Step 3: Assemble the general rule

Rotation is linear, which means two things: rotating a scaled vector is the same as scaling the rotated vector, and rotating a sum is the same as summing the rotations. Both are obvious once said aloud — spinning a picture does not change how many times one arrow fits into another, and it does not change how two arrows join tip to tail.

So take any vector v = (x, y), write it as x·î + y·ĵ, and rotate:

Adding those two scaled vectors component by component gives the result:

Two lines of arithmetic, and everything else in this concept is a consequence of them.

Sanity check it. Set θ = 0: cos 0 = 1, sin 0 = 0, so x' = x and y' = y. Rotating by nothing changes nothing. Now set θ = 90°: cos 90° = 0, sin 90° = 1, giving x' = −y and y' = x. Feed in (1, 0) and you get (0, 1) — X really did swing round to Y. The formula behaves.

Math!

The prime mark in x' and y' is read “x-prime” and “y-prime”. It is not a derivative here — it simply means “the new version of”, and it is the standard way to name a transformed copy of something without inventing a fresh letter.

Notice what the four coefficients in those two equations are. Reading down the columns, (cos θ, sin θ) is exactly where î landed, and (−sin θ, cos θ) is exactly where ĵ landed. That is not a coincidence, and it generalizes: these four numbers are usually packed into a single object called a matrix, and the fact that its columns are the images of the basis vectors turns out to be true of every matrix, not just this one. The linear algebra module derives what that object is and how to multiply by it. For now the two scalar equations are all you need, and computing them by hand is worth doing — it makes the abstraction land properly when it arrives.

Step 4: Rotation preserves length

A rotation should not stretch anything. Let us confirm the formula agrees, by computing the new squared length:

Expand both squares:

The two middle terms are identical apart from sign, so they cancel. Grouping what remains:

And cos²θ + sin²θ = 1 — which is just the Pythagorean theorem from Concept 01 applied to a triangle with hypotenuse 1. So:

Length out equals length in, for every angle and every vector. A transformation that preserves all distances is called rigid, and rigid is exactly what you want when the thing being transformed is a physical robot.

Step 5: Undoing a rotation is free

To rotate back, rotate by −θ. Two facts about the unit circle make this cheap: cos(−θ) = cos θ (a point and its mirror image below the X-axis share the same x-coordinate) while sin(−θ) = −sin θ (their y-coordinates are opposite). Substituting those into our two equations:

Compare the coefficients with the forward rotation from Step 3. They are the same four numbers — cos θ, sin θ, −sin θ, cos θ — merely rearranged. Undoing a rotation costs you nothing beyond flipping one sign. There is no division, no square root, and nothing that can lose precision.

That is worth appreciating, because “undo this transformation” is usually an expensive and numerically delicate operation. For rotations it is almost free, and the reason will become sharp in the linear algebra module: the rearrangement you just spotted has a name, and rotations belong to a special family of transformations for which reversing is only ever a relabeling.

Step 6: Rotations compose by adding angles

Rotate by β, then by α. Physically the result must be a single rotation by α + β — there is no other outcome a turn followed by a turn could have.

Now do it algebraically. Take î = (1, 0), rotate it by β to get (cos β, sin β), then feed that result back through the Step 3 equations with angle α:

But we already know where a single rotation by α + β sends î: to (cos(α + β), sin(α + β)). Both routes must land on the same point, so the expressions must be equal. You have just derived the angle-addition identities from geometry, rather than memorizing them:

One caution: in 2D we get lucky, because α + β = β + α, so the order you apply two rotations in cannot matter. That luck runs out in three dimensions. Turn a book 90 degrees to the right and then 90 degrees forward, then reset and do it in the opposite order — the book ends up in two visibly different orientations. Concept 07 returns to this when 3D rotations and quaternions arrive, and it is the root of gimbal lock.

Step 7: Rotating about a point that is not the origin

The formula spins everything around (0, 0). To pivot about some other center c — say a swerve module’s location, or the robot’s center of mass — use the standard three-step sandwich: subtract c to move the pivot to the origin, rotate, then add c back.

Getting the order wrong here is one of the most common geometry bugs in robot code, and it fails in a distinctive way: the object rotates correctly but also swings around the field on a large arc, because you rotated its position vector along with its shape.

A note on sign conventions

Everything above assumes θ grows counter-clockwise and that Y points up. That is the WPILib convention and the mathematical norm. Screen graphics traditionally put Y pointing down, which silently flips the direction of every rotation. When a visualizer spins the wrong way, this is almost always why — and it is why the companion demo negates its angle before handing it to the canvas.

3. Solving It in Code (Java & WPILib)

First-Principles Java

// Rotate a vector counter-clockwise by theta, about the origin.
static double[] rotate(double x, double y, double thetaRadians) {
    double c = Math.cos(thetaRadians);
    double s = Math.sin(thetaRadians);
    return new double[] { x * c - y * s,      // x' = x cos - y sin
                          x * s + y * c };    // y' = x sin + y cos
}

// Field-oriented drive: the driver's field-frame command, expressed in robot terms.
double fieldVx = 3.0;    // driver pushes 3.0 m/s toward the far end of the field
double fieldVy = 0.0;
double headingRadians = Math.toRadians(40.0);   // gyro says the robot is turned 40 deg

// Rotate by NEGATIVE heading: we are going from the field frame into the robot frame.
double[] robotFrame = rotate(fieldVx, fieldVy, -headingRadians);

System.out.printf("forward %.2f m/s, strafe %.2f m/s%n", robotFrame[0], robotFrame[1]);
// forward 2.30 m/s, strafe -1.93 m/s

The minus sign on the heading is the part worth pausing on. The robot is rotated +40° relative to the field, so the field is rotated −40° relative to the robot. Transforming a vector into a frame uses the opposite sign from rotating a vector within a frame. Concept 03 makes this precise; for now, the sanity check is that the robot must steer to its own right (a negative strafe) to compensate for having been turned to the left.

Production WPILib Equivalent

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;

Rotation2d heading = Rotation2d.fromDegrees(40.0);

// Rotation2d caches cos and sin at construction, so repeated use costs no trig calls.
Translation2d fieldCommand = new Translation2d(3.0, 0.0);
Translation2d robotCommand = fieldCommand.rotateBy(heading.unaryMinus());

// Rotations compose by adding angles - exactly Step 6.
Rotation2d combined = heading.plus(Rotation2d.fromDegrees(15.0));   // 55 degrees

// In practice the whole field-oriented conversion is one library call:
ChassisSpeeds speeds =
    ChassisSpeeds.fromFieldRelativeSpeeds(3.0, 0.0, 0.0, heading);

Rotation2d stores cos θ and sin θ rather than θ itself. That is a direct consequence of Step 3: the sine and cosine are the only things the rotation formula ever asks for, so computing them once at construction removes trigonometry from the 50 Hz control loop entirely.

4. Bridge to Machine Learning & Modern Autonomy

The two equations you just derived are, unmodified, the mechanism behind Rotary Position Embeddings (RoPE) — the position-encoding scheme used in Llama, Mistral, Qwen and most other current large language models.

A transformer processes all tokens in parallel, so on its own it cannot tell “the robot hit the wall” from “the wall hit the robot”. It needs position information injected. RoPE does this by taking each token’s embedding vector, chopping its hundreds of dimensions into consecutive pairs, and treating every pair as a little 2D vector — then rotating pair number k of the token at position m by the angle m · θₖ. That is precisely R(θ) from Step 3, applied a few hundred times per token.

Two properties derived above are exactly why this works rather than wrecking the model. Length preservation (Step 4) means the rotation cannot inflate or shrink any embedding, so attention scores stay numerically well behaved no matter how long the context grows. Composition by angle addition (Step 6) is the deeper one: when attention compares a token at position m with a token at position n, the two rotations combine into a single rotation by (m − n)·θₖ. The absolute positions cancel and only the relative distance survives. A model trained on short documents can therefore generalize to longer ones, because it only ever learned about gaps between tokens, never about absolute slots.

The length-preserving property from Step 4 shows up again in how networks are initialized. Transformations that neither amplify nor attenuate the signal passing through them are a standard choice for initializing deep and recurrent networks, precisely because a transformation that stretches even slightly, applied across fifty layers, compounds into gradients that explode or vanish. Rotations are the archetype of a transformation that does neither, and the linear algebra module gives this family its proper name.

5. Checkpoints & Exploration Prompts

Checkpoint 1

Rotate the vector (4, 0) counter-clockwise by 90 degrees using the formula, then again by another 90 degrees. Confirm the length is unchanged at each stage.

Solution: At θ = 90°, cos θ = 0 and sin θ = 1.

1. First rotation: x' = 4·0 − 0·1 = 0, y' = 4·1 + 0·0 = 4. The result is (0, 4), length √(0 + 16) = 4. ✓
2. Second rotation, applied to (0, 4): x' = 0·0 − 4·1 = −4, y' = 0·1 + 4·0 = 0. The result is (−4, 0), length 4. ✓ Two 90-degree turns produced a 180-degree turn, which negated both components — agreeing with Step 6, since R(90°)·R(90°) = R(180°).

Checkpoint 2

A teammate writes field-oriented drive as rotate(fieldVx, fieldVy, +heading) instead of −heading. The robot is turned 90 degrees counter-clockwise and the driver pushes straight down-field, commanding (3.0, 0.0). What does the robot actually do, and what should it have done?

Solution:

• Correct, with −90°: x' = 3·cos(−90°) − 0·sin(−90°) = 0, y' = 3·sin(−90°) + 0 = −3. The command is “strafe 3 m/s to my right”, which for a robot turned a quarter turn to the left does send it down-field. ✓
• The bug, with +90°: x' = 0, y' = +3. The robot strafes to its left at 3 m/s — a 180-degree error, driving it directly away from the intended direction. The sign error is invisible when the heading is 0 and worst when the robot is sideways, which is why it typically survives testing and fails in a match.

Deep Dive 1

Step 4 proved rotation preserves length. Investigate what else it preserves: take two vectors, rotate both by the same θ, and work out whether the angle between them changes. Then take a triangle with known area, rotate all three of its corners, and compute the new area using the shoelace formula from Geometry Concept 05. Does rotation change area? Does it change the sign of the shoelace sum — that is, can a rotation turn a shape into its mirror image? Argue why not from the geometry alone. The linear algebra module formalises what you will discover under the name “determinant”.

Deep Dive 2

Step 6 noted that 2D rotations commute but 3D rotations do not. Test this physically: take a book, rotate it 90 degrees about the vertical axis and then 90 degrees about the horizontal axis, and note the final orientation. Reset and perform the two rotations in the opposite order. Then research why this non-commutativity leads to gimbal lock in Euler-angle systems, and why quaternions are the standard fix in robot IMUs and 3D graphics.