math xna quaternions

math - Extrayendo Guiñada de un Quaternion



xna quaternions (4)

Habiendo dado un Quaternion q, puedes calcular el balanceo, cabeceo y guiñada de esta manera:

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); var pitch = asin(-2.0*(q.x*q.z - q.w*q.y)); var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

Esto debería ajustarse a la rotación intiática de tait-bryan de orden xyz. Para otras órdenes de rotación, rotaciones extrínsecas y debidas-euler deben usarse otras conversiones.

Tengo un quaternion de rotación y quiero extraer el ángulo de rotación sobre el eje Up (la guiñada). Estoy usando XNA y, por lo que puedo ver, no hay una función incorporada para esto. ¿Cuál es la mejor manera de hacer esto?

Gracias por cualquier ayuda, Venatu

La representación del cuaternión de la rotación es una variación del eje y el ángulo. Entonces, si rotan r radianes alrededor del eje x , y , z , entonces su cuaternión q es:

q[0] = cos(r/2); q[1] = sin(r/2)*x; q[2] = sin(r/2)*y; q[3] = sin(r/2)*z;

Si desea crear un cuaternión que solo gire alrededor del eje y , ponga a cero los ejes x y z y luego vuelva a normalizar el cuaternión:

q[1] = 0; q[3] = 0; double mag = sqrt(q[0]*q[0] + q[2]*q[2]); q[0] /= mag; q[2] /= mag;

Si quieres el ángulo resultante:

double ang = 2*acos(q[0]);

Esto supone que la representación del cuaternión se almacena: w, x, y, z. Si ambos q [0] y q [2] son ​​cero, o cerca de él, el cuaternión resultante debería ser {1,0,0,0}.

Nota : He verificado el código a continuación contra las ecuaciones de Wikipedia más la documentación de Pixhawk y es correcto.

Si está trabajando con drones / aviación, a continuación se muestra el código (tomado directamente de DJI SDK ). Aquí q0, q1, q2, q3 corresponde a las componentes w, x, y, z del cuaternión, respectivamente. También tenga en cuenta que yaw, pitch, roll puede referirse como título, actitud y banco, respectivamente, en algunos estudios.

float roll = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2)); float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1)); float yaw = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

Si necesita calcular los 3, puede evitar volver a calcular términos comunes mediante las siguientes funciones:

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152 EulerianAngle Flight::toEulerianAngle(QuaternionData data) { EulerianAngle ans; double q2sqr = data.q2 * data.q2; double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0; double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3); double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2); double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1); double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0; t2 = t2 > 1.0 ? 1.0 : t2; t2 = t2 < -1.0 ? -1.0 : t2; ans.pitch = asin(t2); ans.roll = atan2(t3, t4); ans.yaw = atan2(t1, t0); return ans; } QuaternionData Flight::toQuaternion(EulerianAngle data) { QuaternionData ans; double t0 = cos(data.yaw * 0.5); double t1 = sin(data.yaw * 0.5); double t2 = cos(data.roll * 0.5); double t3 = sin(data.roll * 0.5); double t4 = cos(data.pitch * 0.5); double t5 = sin(data.pitch * 0.5); ans.q0 = t2 * t4 * t0 + t3 * t5 * t1; ans.q1 = t3 * t4 * t0 - t2 * t5 * t1; ans.q2 = t2 * t5 * t0 + t3 * t4 * t1; ans.q3 = t2 * t4 * t1 - t3 * t5 * t0; return ans; }

Nota sobre la biblioteca Eigen

Si está utilizando la biblioteca Eigen, tiene otra forma de hacer esta conversión, sin embargo, esto puede no ser tan optimizado como el código directo anterior:

Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0); yaw = euler[0]; pitch = euler[1]; roll = euler[2];