Conversión de euler a matriz y matriz a euler
Estoy tratando de convertir una rotación 3D descrita en términos de ángulos de euler en una matriz y luego de vuelta, usando .NET / C #. Mis convenciones son:
sistema zurdo (x derecha, y arriba, z adelante)orden de rotaciones: rumbo alrededor de y, tono alrededor de x, banco alrededor de zlas rotaciones son positivas usando la regla de la mano izquierda (el pulgar señala hacia + infinito)Mi prueba es:
Euler a la matriz (He eliminado la parte de traducción x, y, z para simplificar)
Matrix3D matrix = new Matrix3D() {
M11 = cosH * cosB - sinH * sinP * sinB,
M12 = - sinB * cosP,
M13 = sinH * cosB + cosH * sinP * sinB,
M21 = cosH * sinB + sinH * sinP * cosB,
M22 = cosB * cosP,
M23 = sinB * sinH - cosH * sinP * cosB,
M31 = - sinH * cosP,
M32 = - sinP,
M33 = cosH * cosP,
};
Matriz de Euler
const double RD_TO_DEG = 180 / Math.PI;
double h, p, b; // angles in degrees
// extract pitch
double sinP = -matrix.M23;
if (sinP >= 1) {
p = 90; } // pole
else if (sinP <= -1) {
p = -90; } // pole
else {
p = Math.Asin(sinP) * RD_TO_DEG; }
// extract heading and bank
if (sinP < -0.9999 || sinP > 0.9999) { // account for small angle errors
h = Math.Atan2(-matrix.M31, matrix.M11) * RD_TO_DEG;
b = 0; }
else {
h = Math.Atan2(matrix.M13, matrix.M33) * RD_TO_DEG;
b = Math.Atan2(matrix.M21, matrix.M22) * RD_TO_DEG; }
Debe estar equivocado Si tomo 3 ángulos, los convierto en una matriz y vuelvo a convertir la matriz en ángulos, el resultado es diferente a los valores iniciales.
He navegado por varios sitios con diferentes fórmulas, comenzando con euclideanspace.com, pero ahora estoy completamente perdido y no puedo encontrar los cálculos correctos. Aprecio un poco de ayuda. ¿Hay un matemático a bordo?