Recommended way to compose rotations - c++

Question

I have 2 rotations represented as yaw, pitch, roll (Tait-Brian intrinsic right-handed). What is the recommended way to construct a single rotation that is equivalent to both of them?
EDIT: if I understand correctly from the answers, I must first convert yaw, pitch, roll to either matrix or quaternion, compose them and then transform the result back to yaw, pitch, roll representation.
Also, my first priority is simplicity, then numerical stability and efficiency.
Thanks :)

Answer 1

As a general answer, if you make a rotation matrix for each of the two rotations, you can then make a single matrix which is the product of the two (order is important!) to represent the effect of applying both rotations.
It is possible to conceive of instances where "gimbal lock" could make this numerically unstable for certain angles (typically involving angles very close to 90 degrees).
It is faster and more stable to use quaternions. You can see a nice treatment at http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm - in summary, every rotation can be represented by a quaternion and multiple rotations are just represented by the product of the quaternions. They tend to have better stability properties.
Formulas for doing this can be found at http://en.m.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
UPDATE using the formulas provided at http://planning.cs.uiuc.edu/node102.html , you can adapt the following code to do a sequence of rotations. While the code is written in (and compiles as ) C++, I am not taking advantage of certain built in C++ types and methods that might make this code more elegant - showing my C roots here. The point is really to show how the rotation equations work, and how you can concatenate multiple rotations.
The two key functions are calcRot which computes the rotation matrix for given yaw, pitch and roll; and mMult which multiplies two matrices together. When you have two successive rotations, the product of their rotation matrices is the "composite" rotation - you do have to watch out for the order in which you do things. The example that I used shows this. First I rotate a vector by two separate rotations; then I compute a single matrix that combines both rotations and get the same result; finally I reverse the order of the rotations, and get a different result. All of which should help you solve your problem.
Make sure that the conventions I used make sense for you.
#include <iostream>
#include <cmath>
#define PI (2.0*acos(0.0))
//#define DEBUG
void calcRot(double ypr[3], double M[3][3]) {
// extrinsic rotations: using the world frame of reference
// ypr: yaw, pitch, roll in radians
double cy, sy, cp, sp, cr, sr;
// compute sin and cos of each just once:
cy = cos(ypr[0]);
sy = sin(ypr[0]);
cp = cos(ypr[1]);
sp = sin(ypr[1]);
cr = cos(ypr[2]);
sr = sin(ypr[2]);
// compute this rotation matrix:
// source: http://planning.cs.uiuc.edu/node102.html
M[0][0] = cy*cp;
M[0][1] = cy*sp*sr - sy*cr;
M[0][2] = cy*sp*cr + sy*sr;
M[1][0] = sy*cp;
M[1][1] = sy*sp*sr + cy*cr;
M[1][2] = sy*sp*sr - cy*sr;
M[2][0] = -sp;
M[2][1] = cp*sr;
M[2][2] = cp*cr;
}
void mMult(double M[3][3], double R[3][3]) {
// multiply M * R, returning result in M
double T[3][3] = {0};
for(int ii = 0; ii < 3; ii++) {
for(int jj = 0; jj < 3; jj++) {
for(int kk = 0; kk < 3; kk++ ) {
T[ii][jj] += M[ii][kk] * R[kk][jj];
}
}
}
// copy the result:
for(int ii = 0; ii < 3; ii++) {
for(int jj = 0; jj < 3; jj++ ) {
M[ii][jj] = T[ii][jj];
}
}
}
void printRotMat(double M[3][3]) {
// print 3x3 matrix - for debug purposes
#ifdef DEBUG
std::cout << "rotation matrix is: " << std::endl;
for(int ii = 0; ii < 3; ii++) {
for(int jj = 0; jj < 3; jj++ ) {
std::cout << M[ii][jj] << " ";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
}
void applyRot(double before[3], double after[3], double M[3][3]) {
// apply rotation matrix M to vector 'before'
// returning result in vector 'after'
double sumBefore = 0, sumAfter = 0;
std::cout << "Result of rotation:" << std::endl;
for(int ii = 0; ii < 3; ii++) {
std::cout << before[ii] << " -> ";
sumBefore += before[ii] * before[ii];
after[ii] = 0;
for( int jj = 0; jj < 3; jj++) {
after[ii] += M[ii][jj]*before[jj];
}
sumAfter += after[ii] * after[ii];
std::cout << after[ii] << std::endl;
}
std::cout << std::endl;
#ifdef DEBUG
std::cout << "length before: " << sqrt(sumBefore) << "; after: " << sqrt(sumAfter) << std::endl;
#endif
}
int main(void) {
double r1[3] = {0, 0, PI/2}; // order: yaw, pitch, roll
double r2[3] = {0, PI/2, 0};
double initPoint[3] = {3,4,5}; // initial point before rotation
double rotPoint[3], rotPoint2[3];
// initialize rotation matrix to I
double R[3][3];
double R2[3][3];
// compute first rotation matrix in-place:
calcRot(r1, R);
printRotMat(R);
applyRot(initPoint, rotPoint, R);
// apply second rotation on top of first:
calcRot(r2, R2);
std::cout << std::endl << "second rotation matrix: " << std::endl;
printRotMat(R2);
// applying second matrix to result of first rotation:
std::cout << std::endl << "applying just the second matrix to result of first: " << std::endl;
applyRot(rotPoint, rotPoint2, R2);
mMult(R2, R);
std::cout << "after multiplication: " << std::endl;
printRotMat(R2);
std::cout << "Applying the combined matrix to the intial vector: " << std::endl;
applyRot(initPoint, rotPoint2, R2);
// now in the opposite order:
double S[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
calcRot(r2, S);
printRotMat(S);
calcRot(r1, R2);
mMult(R2, S);
std::cout << "applying rotation in the opposite order: " << std::endl;
printRotMat(R2);
applyRot(initPoint, rotPoint, R2);
}
Output (with #DEBUG not defined - commented out):
Result of rotation:
3 -> 3
4 -> -5
5 -> 4
second rotation matrix:
applying just the second matrix to result of first:
Result of rotation:
3 -> 4
-5 -> -5
4 -> -3
after multiplication:
Applying the combined matrix to the intial vector:
Result of rotation:
3 -> 4
4 -> -5
5 -> -3
Note that these last two give the same result, showing that you can combine rotation matrices.
applying rotation in the opposite order:
Result of rotation:
3 -> 5
4 -> 3
5 -> 4
Now the result is different - the order is important.

Answer 2

If you are familiar with matrix operations, you may try Rodrigues' rotation formula. If you are familiar with quaternions, you may try the P' = q*P*q' approach.
Quaterion math is a bit more complicated to grasp, but code is simpler and faster.