Starting from 3 angles, I’m trying to create a global rotation matrix and get the angles back again.
First of all, I calculate a global rotation matrix multiplying the 3 angles rotation matrices
float xRot=45*DEG_TO_RAD, yRot=30*DEG_TO_RAD; zRot=20*DEG_TO_RAD;
// build rotateX matrix
matX= new float[16];
matX[5] = cosf(xRot);
matX[6] = sinf(xRot);
matX[9] = -matX[6];
matX[10] = matX[5];
// build rotateY matrix
matY= new float[16];
matY[0] = cosf(yRot);
matY[2] = sinf(yRot);
matY[8] = -matY[2];
matY[10] = matY[0];
// build rotateZ matrix
matZ= new float[16];
matZ[0] = cosf(zRot);
matZ[1] = sinf(zRot);
matZ[4] = -matZ[1];
matZ[5] = matZ[0];
matXY=new float[16];
matXYZ=new float[16];
matXY= matrixMultiply(matX, matY);
matXYZ=matrixMultiply(matXY, matZ); //calculate final rotation matrix matXYZ
And then I try to get the angles back:
float angle_x,angle_y,angle_z,tr_x,tr_y;
angle_y = -asin( matXYZ[2]); /* Calculate Y-axis angle */
float C = cos( angle_y );
if ( fabs( C ) > 0.005 ) /* Gimball lock? */
{
tr_x = matXYZ[10] / C; /* No, so get X-axis angle */
tr_y = -matXYZ[6] / C;
angle_x = atan2( tr_y, tr_x ) ;
tr_x = matXYZ[0] / C; /* Get Z-axis angle */
tr_y = -matXYZ[1] / C;
angle_z = atan2( tr_y, tr_x ) ;
}
else /* Gimball lock has occurred */
{
angle_x = 0; /* Set X-axis angle to zero */
tr_x = matXYZ[5]; /* And calculate Z-axis angle */
tr_y = matXYZ[4];
angle_z = atan2( tr_y, tr_x ) ;
}
I’d expect angle_x, angle_y, angle_z to be equal to: float xRot=45, yRot=30; zRot=20;
And instead they are not. What am I doing wrong?
Here also the matrix multuplication function:
float* matrixMultiply(float* m1, float* m2)
{
float *finalMat;
finalMat=new float[16];
matrixIdentity(finalMat);
// Fisrt Column
finalMat[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
finalMat[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
finalMat[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
finalMat[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];
// Second Column
finalMat[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
finalMat[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
finalMat[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
finalMat[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];
// Third Column
finalMat[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
finalMat[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
finalMat[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
finalMat[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];
// Fourth Column
finalMat[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
finalMat[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
finalMat[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
finalMat[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
return finalMat;
}
