Obteniendo los ángulos de Euler

Solución para salvar el problema del Gimbal Lock al obtener los ángulos de Euler a partir de la matriz que se usa en ArToolkit.

Para obtener los ángulos de euler a partir de la matriz de rotación he programado el siguiente código:

int getAngulos(double rot[3][3],  double *rfXAng, double *rfYAng, double *rfZAng) {

// Matriz de Rotacion

// +-           -+   +-                                      -+

// | r00 r01 r02 |   |  cy*cz  cz*sx*sy-cx*sz  cx*cz*sy+sx*sz |

// | r10 r11 r12 | = |  cy*sz  cx*cz+sx*sy*sz -cz*sx+cx*sy*sz |

// | r20 r21 r22 |   | -sy     cy*sx           cx*cy          |

// +-           -+   +-                                      -+

// gimbalLock == 0 , no se produce gimbal Lock

// gimbalLock == 1 , se ha producido gimbal Lock por y angulo = pi/2

// gimbalLock == 2 , se ha producido gimbal Lock por y angulo = -pi/2

int gimbalLock = 0;

if (rot[2][0] < 0.995) {

if (rot[2][0] > -0.995) {

// y_angulo = asin(-r20)

// z_angulo = atan2(r10,r00)

// x_angulo = atan2(r21,r22)

*rfYAng = asin(-rot[2][0]);

*rfZAng = atan2(rot[1][0],rot[0][0]);

*rfXAng = atan2(rot[2][1],rot[2][2]);

}

else {

// y_angulo = +pi/2

// x_angulo + z_angulo = atan2(r01,r02)

// NOTA.  La solución no es unica.  Tomamos x_angulo = 0.

*rfYAng = PI/2;

*rfZAng = -atan2(rot[0][1],rot[0][2]);

*rfXAng = 0.0;

gimbalLock = 1;

}

}

else {

// y_angulo = -pi/2

// x_angulo + z_angulo = atan2(-r01,-r02)

// NOTA.  La solución no es unica.  Tomamos x_angulo = 0.

*rfYAng = -PI/2;

*rfZAng = atan2(-rot[0][1],-rot[0][2]);

*rfXAng = 0.0;

gimbalLock = 2;

}

printf(“Radianes: %3.1f %3.1f %3.1f \n”,*rfXAng,*rfYAng,*rfZAng);

printf(“Centigrados: %3.1f %3.1f %3.1f \n”,(*rfXAng*180)/PI, (*rfYAng*180)/PI, (*rfZAng*180)/PI);

return gimbalLock;

}
Esta función nos indica los ángulos de Euler y además devuelve un valor 1 u 2 cuando se produce Gimbal Lock por angulo con respecto al eje “Y” de 90 o -90 grados respectivamente, de este modo estaré advertido de la situación en un futuro.

En breve voy a empezar a construir mis propios archivos bvh, espero no tener demasiados problemas.

Advertisement