PDA

View Full Version : Matrices math



Alessandro_dup1
10-13-2011, 02:23 PM
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;
}

biller23
10-13-2011, 03:04 PM
I do not tested your code but the angles are in radians. Do not forget to convert them back.

BTW: why have you decided to allocate memory in the heap during matrix multiplication?

A.Conz
10-14-2011, 01:24 AM
euler angles suffer from singularities. that's why you are getting wrong results. use quaternions instead ;)