## Friday, April 29, 2011

### Right-handed rotation matrix for roll/pitch/yaw

Specifically, an three-dimensional orthogonal matrix corresponding to a counter-clockwise/right-handed rotation where roll is the rotation about the x-axis, pitch is the rotation about the y-axis, and yaw is the rotation about the z-axis.

You wouldn't think this would be hard to find online, but go figure.

Thanks to Phaedrus for his explanations and...well, doing most of the work.

Note: this matrix effectively yaws first, then pitches, then rolls.  For the opposite order (and the one I ended up using), look below.

 [ cos(pitch)*cos(yaw) -sin(yaw)*cos(pitch) sin(pitch) ] cos(yaw)*sin(pitch)*sin(roll)     + sin(yaw)*cos(roll) -sin(yaw)*sin(pitch)*sin(roll)     + cos(yaw)*cos(roll) -cos(pitch)*sin(roll) -cos(yaw)*sin(pitch)*cos(roll)     + sin(yaw)*sin(roll) sin(yaw)*sin(pitch)*cos(roll)     + cos(yaw)*sin(roll) cos(pitch)*cos(roll)

And to put it another way:

//Top row
rotation = cos(pitch)*cos(yaw);
rotation = -sin(yaw)*cos(pitch);
rotation = sin(pitch);

//Middle row
rotation = cos(yaw)*sin(pitch)*sin(roll) + sin(yaw)*cos(roll);
rotation = -sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll);
rotation = -cos(pitch)*sin(roll);

//Bottom row
rotation = -cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll);
rotation = sin(yaw)*sin(pitch)*cos(roll) + cos(yaw)*sin(roll);
rotation = cos(pitch)*cos(roll);

Note: this matrix effectively rolls first, then pitches, then yaws.  This turned out to be the one I used for my system.

 [ cos(pitch)*cos(yaw) -sin(yaw)*cos(roll)     + cos(yaw)*sin(pitch)*sin(roll) sin(roll)*sin(yaw)     + cos(yaw)*sin(pitch)*cos(roll) ] cos(pitch)*sin(yaw) cos(yaw)*cos(roll)     + sin(pitch)*sin(yaw)*sin(roll) -sin(roll)*cos(yaw)     + sin(pitch)*sin(yaw)*cos(roll) -sin(pitch) cos(pitch)*sin(roll) cos(pitch)*cos(roll)

//Top row
rotation = cos(pitch)*cos(yaw);
rotation = -sin(yaw)*cos(roll) + cos(yaw)*sin(pitch)*sin(roll);
rotation = sin(roll)*sin(yaw) + cos(yaw)*sin(pitch)*cos(roll);

//Middle row
rotation = cos(pitch)*sin(yaw);
rotation = cos(yaw)*cos(roll) + sin(pitch)*sin(yaw)*sin(roll);
rotation = -sin(roll)*cos(yaw) + sin(pitch)*sin(yaw)*cos(roll);

//Bottom row
rotation = -sin(pitch);
rotation = cos(pitch)*sin(roll);
rotation = cos(pitch)*cos(roll);