c++ - Obtener tono y rollo de la matriz sin singularidades

math matrix

Question

Estoy trabajando en el simulador de movimiento con 2 DOF (pitch & roll). Estoy leyendo la matriz de transformación del juego y necesito obtener los ángulos y enviarlos al hardware para conducir los motores. Como los ángulos de Euler tienen singularidades, realmente no puedo usarlos. Se comporta así:

cuando debería tener esto:

Preparé un ejemplo en línea para mostrar mejor el problema:

// Get euler angles from model matrix
var mat = model.matrix;
mat.transpose();

var e = new THREE.Euler();
e.setFromRotationMatrix(mat, ''XZY'');
var v = e.toVector3();

var pitch = -v.z;
var roll  = -v.x;

http://jsfiddle.net/qajro0ny/3/

Por lo que yo entiendo, hay dos problemas aquí.

1. No hay eje de guiñada en el simulador.
2. Incluso si hubiera eje de guiñada, los motores simplemente no se comportan como gráficos de computadora, es decir, necesitan tiempo para llegar a la posición de destino.

He leído sobre gimbal lock e incluso implementé el filtro euler, pero no funcionó como se esperaba. La mayoría de los consejos sobre bloqueo de cardán eran para usar cuaterniones, pero no puedo conducir un motor físico con cuaternión (¿o puedo?).

El orden del eje realmente no importa aquí, porque cambiarlo solo moverá la singularidad de un eje a otro.

Necesito manejar esto de otra manera.

Intenté multiplicar vectores de ejes por matriz y luego usar producto de puntos y cruces para obtener ángulos, pero eso también falló. Creo que también debería haber una reproyección de los ejes para hacerlo bien, pero no pude resolverlo. Pero algo me dice que esta es la forma correcta de hacer esto. Era algo como esto: http://jsfiddle.net/qajro0ny/53/

Luego se me ocurrió una idea diferente. Sé la posición anterior, así que tal vez haga lo siguiente:

1. Convertir matriz en cuaternión
2. Calcule la diferencia entre el cuaternión actual y el anterior
3. Convierta el cuaternión resultante en ángulos euler
4. Agregue esos ángulos a las variables estáticas de tono, balanceo y guiñada.

Así que lo intenté y ... ¡funcionó! Sin singularidades en ninguna de las direcciones, rotación perfecta de 360 ​​grados en cabeceo, balanceo y guiñada. ¡La solución perfecta! Excepto ... no lo es. Los marcos no se sincronizaron, así que después de un tiempo los ángulos estaban muy lejos de lo que deberían ser. He estado pensando en algún tipo de mecanismo de sincronización, pero pensé que esta no era la manera correcta.

Se veía así: http://jsfiddle.net/qajro0ny/52/

Y la misma lógica, pero directamente con matrices: http://jsfiddle.net/qajro0ny/54/

He buscado en la web alto y bajo, he leído docenas de documentos y otras preguntas / publicaciones y simplemente no puedo creer que nada funcione realmente para mi caso.

Puede que no entienda o no me pierda algo, así que aquí está todo lo que encontré y probé:

Enlaces: http://pastebin.com/3G0dYLvu

Código: http://pastebin.com/PiZKwE2t (lo he reunido todo, así que está desordenado)

Debo extrañar algo, o estoy mirando esto desde el ángulo equivocado.

Answer 1

Agregué mis dos centavos en http://jsfiddle.net/qajro0ny/58/ , básicamente aplicando los trabajos de http://blogs.msdn.com/b/mikepelton/archive/2004/10/29/249501.aspx que me encontré con unos años atrás. Básicamente se reduce a

var a = THREE.Math.clamp(-mat.elements[6], -1, 1);
var xPitch = Math.asin(a);
var yYaw = 0;
var zRoll = 0;
if (Math.cos(xPitch) > 0.0001)
{
  zRoll = -Math.atan2(mat.elements[4], mat.elements[5]);
  yYaw = -Math.atan2(mat.elements[2], mat.elements[10]);
}
else
{
  zRoll = -Math.atan2(-mat.elements[1], mat.elements[0]);
}

Me di cuenta de que la asignación supuesta de guiñada, cabeceo, balanceo a ejes (y, x, z) tal como se usa en el sistema de coordenadas para zurdos DirectX difiere de la que está utilizando OpenGL / WebGL, por lo que tal vez sea necesario resolverlo finalmente .

Espero que esto ayude.

Answer 2

Me imagino que en realidad podrías conducir motores físicos con cuaterniones. Recuerde que el cuaternión representa un eje de rotación (x, y, z) y un ángulo. Supongo que tienes tres motores (para cada eje), cuya velocidad de rotación puedes escalar. Ahora, al escalar la velocidad de rotación de estos tres motores, puede establecer un eje de rotación específico y, midiendo cuidadosamente el tiempo, puede establecer un ángulo de rotación específico. Debería ser más fácil que convertir a deltas de ángulos de Euler.

Answer 3

Si sabe que la matriz contiene solo las dos rotaciones (en el orden dado T = RZ * RX ), entonces puede hacer algo como lo siguiente:

El eje x local no se ve afectado por la segunda rotación. Entonces puedes calcular el primer ángulo solo con el eje x local. Luego puede eliminar esta rotación de la matriz y calcular el ángulo restante desde cualquiera de los otros dos ejes:

function calculateAngles() {
    var mat = model.matrix;

    //calculates the angle from the local x-axis
    var pitch = Math.atan2(mat.elements[1], mat.elements[0]);

    //this matrix is used to remove the first rotation
    var invPitch = new THREE.Matrix4();
    invPitch.makeRotationZ(-pitch);

    //this matrix will only contain the roll rotation
    //  rollRot = RZ^-1 * T
    //          = RZ^-1 * RZ * RX
    //          = RX
    var rollRot = new THREE.Matrix4();
    rollRot.multiplyMatrices(invPitch, mat);

    //calculate the remaining angle from the local y-axis
    var roll  = Math.atan2(rollRot.elements[6], rollRot.elements[5]);

    updateSimAngles(pitch, roll);
}

Por supuesto, esto solo funciona si la matriz dada coincide con los requisitos. No debe contener una tercera rotación. De lo contrario, probablemente necesites encontrar una solución no lineal de mínimos cuadrados.

Answer 4

Tu idea sobre el uso de los deltas de la rotación suena realmente prometedora.

Sin embargo, no estoy muy seguro de lo que quiere decir con los "marcos no se sincronizaron". Me imagino que tus cálculos con cuaterniones podrían no ser 100% exactos (¿utilizas puntos flotantes?), Lo que da como resultado pequeños errores que se acumulan y finalmente dan como resultado movimientos asincrónicos.

El punto es que se supone que debes usar unidades-cuaterniones para la representación de rotaciones. Puedes hacer eso en un modelo teórico, pero si representas quaterniones por 4 números de coma flotante, tus cuaterniones no serán cuaterniones de unidades en la mayoría de los casos, sino que solo estarán muy cerca (su norma es 1+e para algunos pequeños). posiblemente negativo - valor e ). Esto está bien, ya que no notará estas pequeñas diferencias, sin embargo, si está lanzando toneladas de operaciones en sus cuaterniones (lo cual está haciendo rooteando constantemente su modelo y calculando los deltas), estos pequeños errores se acumularán. Por lo tanto, necesita renormalizar sus cuaterniones constantemente para mantenerlos lo más cerca posible de los cuaterniones de la unidad, de modo que sus cálculos (especialmente la conversión a ángulos de Euler) se mantengan (casi) exactos.