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) ]
    + sin(yaw)*cos(roll)
    + cos(yaw)*cos(roll)
    + sin(yaw)*sin(roll)
    + cos(yaw)*sin(roll)

 And to put it another way:

//Top row
rotation[0][0] = cos(pitch)*cos(yaw);
rotation[0][1] = -sin(yaw)*cos(pitch);
rotation[0][2] = sin(pitch);

//Middle row
rotation[1][0] = cos(yaw)*sin(pitch)*sin(roll) + sin(yaw)*cos(roll);
rotation[1][1] = -sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll);
rotation[1][2] = -cos(pitch)*sin(roll);

//Bottom row
rotation[2][0] = -cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll);
rotation[2][1] = sin(yaw)*sin(pitch)*cos(roll) + cos(yaw)*sin(roll);
rotation[2][2] = 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)
    + cos(yaw)*sin(pitch)*cos(roll)
cos(pitch)*sin(yaw) cos(yaw)*cos(roll)
    + sin(pitch)*sin(yaw)*sin(roll)
    + sin(pitch)*sin(yaw)*cos(roll)
-sin(pitch) cos(pitch)*sin(roll) cos(pitch)*cos(roll)

//Top row
rotation[0][0] = cos(pitch)*cos(yaw);
rotation[0][1] = -sin(yaw)*cos(roll) + cos(yaw)*sin(pitch)*sin(roll);
rotation[0][2] = sin(roll)*sin(yaw) + cos(yaw)*sin(pitch)*cos(roll);

//Middle row
rotation[1][0] = cos(pitch)*sin(yaw);
rotation[1][1] = cos(yaw)*cos(roll) + sin(pitch)*sin(yaw)*sin(roll);
rotation[1][2] = -sin(roll)*cos(yaw) + sin(pitch)*sin(yaw)*cos(roll);

//Bottom row
rotation[2][0] = -sin(pitch);
rotation[2][1] = cos(pitch)*sin(roll);
rotation[2][2] = cos(pitch)*cos(roll);


David Tarazona said...

OMG, thanks a lot... I've been searching this for a while... Also is great that you have in C syntax, thanks... and you're right, finding it online is quite hard

pirate said...

Is that with Z positive going up or down?

The Writer said...

Z-positive going up.