Quaternion dfference + time -> angular velocity (giroscopio en la biblioteca de física)

Estoy usando la biblioteca Bullet Physic para progtwigr alguna function, donde tengo una diferencia entre la orientación del giroscopio dada en cuaternión y la orientación de mi object, y el time entre cada cuadro en milisegundos. Todo lo que quiero es establecer la orientación desde mi giroscopio a la orientación de mi object en el espacio 3D. Pero todo lo que puedo hacer es establecer la velocidad angular de mi object. Tengo diferencia de orientación y time, y a partir de eso calculo el vector de velocidad angular [Wx, Wy, Wz] a partir de esa fórmula: W (t) = 2 * dq (t) / dt * conj (q (t))

Mi código es:

btQuaternion diffQuater = gyroQuater - boxQuater; btQuaternion conjBoxQuater = gyroQuater.inverse(); btQuaternion velQuater = ((diffQuater * 2.0f) / d_time) * conjBoxQuater; 

Y todo funciona bien, hasta que obtengo:

1> rotando alnetworkingedor del eje Y, ángulo de aproximadamente 60 grados, luego tengo estos valores en 2 cuadros críticos:

 x: -0.013220 y: -0.038050 z: -0.021979 w: -0.074250 - diffQuater x: 0.120094 y: 0.818967 z: 0.156797 w: -0.538782 - gyroQuater x: 0.133313 y: 0.857016 z: 0.178776 w: -0.464531 - boxQuater x: 0.207781 y: 0.290452 z: 0.245594 - diffQuater -> euler angles x: 3.153619 y: -66.947929 z: 175.936615 - gyroQuater -> euler angles x: 4.290697 y: -57.553043 z: 173.320053 - boxQuater -> euler angles x: 0.138128 y: 2.823307 z: 1.025552 w: 0.131360 - velQuater d_time: 0.058000 x: 0.211020 y: 1.595124 z: 0.303650 w: -1.143846 - diffQuater x: 0.089518 y: 0.771939 z: 0.144527 w: -0.612543 - gyroQuater x: -0.121502 y: -0.823185 z: -0.159123 w: 0.531303 - boxQuater x: nan y: nan z: nan - diffQuater -> euler angles x: 2.985240 y: -76.304405 z: -170.555054 - gyroQuater -> euler angles x: 3.269681 y: -65.977966 z: 175.639420 - boxQuater -> euler angles x: -0.730262 y: -2.882153 z: -1.294721 w: 63.325996 - velQuater d_time: 0.063000 

2> rotando alnetworkingedor del eje X, ángulo de aproximadamente 120 grados, luego tengo estos valores en 2 cuadros críticos:

 x: -0.013045 y: -0.004186 z: -0.005667 w: -0.022482 - diffQuater x: -0.848030 y: -0.187985 z: 0.114400 w: 0.482099 - gyroQuater x: -0.834985 y: -0.183799 z: 0.120067 w: 0.504580 - boxQuater x: 0.036336 y: 0.002312 z: 0.020859 - diffQuater -> euler angles x: -113.129463 y: 0.731925 z: 25.415056 - gyroQuater -> euler angles x: -110.232368 y: 0.860897 z: 25.350458 - boxQuater -> euler angles x: -0.865820 y: -0.456086 z: 0.034084 w: 0.013184 - velQuater d_time: 0.055000 x: -1.721662 y: -0.387898 z: 0.229844 w: 0.910235 - diffQuater x: -0.874310 y: -0.200132 z: 0.115142 w: 0.426933 - gyroQuater x: 0.847352 y: 0.187766 z: -0.114703 w: -0.483302 - boxQuater x: -144.402298 y: 4.891629 z: 71.309158 - diffQuater -> euler angles x: -119.515343 y: 1.745076 z: 26.646086 - gyroQuater -> euler angles x: -112.974533 y: 0.738675 z: 25.411509 - boxQuater -> euler angles x: 2.086195 y: 0.676526 z: -0.424351 w: 70.104248 - velQuater d_time: 0.057000 

2> girando alnetworkingedor del eje Z, ángulo de aproximadamente 120 grados, luego tengo estos valores en 2 cuadros críticos:

 x: -0.000736 y: 0.002812 z: -0.004692 w: -0.008181 - diffQuater x: -0.003829 y: 0.012045 z: -0.868035 w: 0.496343 - gyroQuater x: -0.003093 y: 0.009232 z: -0.863343 w: 0.504524 - boxQuater x: -0.000822 y: -0.003032 z: 0.004162 - diffQuater -> euler angles x: -1.415189 y: 0.304210 z: -120.481873 - gyroQuater -> euler angles x: -1.091881 y: 0.227784 z: -119.399445 - boxQuater -> euler angles x: 0.159042 y: 0.169228 z: -0.754599 w: 0.003900 - velQuater d_time: 0.025000 x: -0.007598 y: 0.024074 z: -1.749412 w: 0.968588 - diffQuater x: -0.003769 y: 0.012030 z: -0.881377 w: 0.472245 - gyroQuater x: 0.003829 y: -0.012045 z: 0.868035 w: -0.496343 - boxQuater x: -5.645197 y: 1.148993 z: -146.507187 - diffQuater -> euler angles x: -1.418294 y: 0.270319 z: -123.638245 - gyroQuater -> euler angles x: -1.415183 y: 0.304208 z: -120.481873 - boxQuater -> euler angles x: 0.017498 y: -0.013332 z: 2.040073 w: 148.120056 - velQuater d_time: 0.027000 

El problema es el más visible en diffQuater -> euler angles vector. ¿Puede alguien decirme por qué es así? y cómo resolver ese problema?

Todas las sugerencias son bienvenidas

Ahh! Al ver los valores, ahora veo lo que está pasando.

La gran sutileza con los cuaterniones es que cada rotation tiene dos representaciones diferentes por cuaterniones; si q es la representación del cuaternión de una rotation dada, entonces -q representa la misma rotation. Por alguna razón, a medida que realiza la transición a través de los umbrales que ha indicado, su cuaternión gyro está 'volteando'; todavía representa la misma rotation pero está usando el otro cuaternión para hacerlo. Puedes ver que si bien ahora es radicalmente diferente de boxQuater, está muy cerca del negativo de boxQuater; tomar ciegamente la diferencia entre los dos, está resultando en un gran cuaternión en lugar de la pequeña diferencia que debería ver.

La solución, afortunadamente, es sencilla; ya que boxQuater y gyroQuater son ambos cuaterniones (aproximadamente) de la unidad y como se supone que ambos están muy cerca uno del otro, su producto escalar debería estar muy cerca de 1. Intenta calcular el producto escalar de los dos antes de ejecutar tu delta y código de velocidad angular . Si encuentra que el producto escalar está cerca de -1 (de hecho, si es negativo en absoluto), simplemente niegue gyroQuater – reemplácelo por -gyroQuater – antes de calcular su valor para diffQuater.

Está calculando la derivada de una function de cuaternión utilizando (q(t+Δt) - q(t)) / Δt . Esto puede llevar a problemas de precisión porque la diferencia de orientación entre dos cuaterniones estaría mejor representada dividiendo dos cuaterniones.

A diferencia de las funciones habituales, donde los valores de la derivada pueden sumrse a lo largo del time para get el valor real de la function, los cuaterniones de la unidad se multiplican a lo largo del time.

Entonces, si tienes dos cuaterniones para q(t) y q(t+Δt) , una aproximación de la derivada mejor que (q(t+Δt) - q(t)) / Δt es exp(log(q(t+Δt) / q(t)) / Δt) .

Por lo tanto, tu código debe ser:

 btQuaternion diffQuater = gyroQuater * boxQuater.inverse(); btQuaternion conjBoxQuater = boxQuater.inverse(); btQuaternion velQuater = 2.0f * exp(log(diffQuater) / d_time) * conjBoxQuater; 

Si no tiene funciones exponenciales y de logaritmo para su class de cuaternión, puede implementarlas usando las definiciones estándar de Wikipedia .

Editar : reformular para dejar en claro que el código inicial no era realmente incorrecto

Gracias a todos por la última ayuda, pero me mudé a PhysX en lugar de Bullet, y ahora tengo que calcular Torque en lugar de AngularVelocity. Desafortunadamente, eso es más complicado que el último para mí 🙁

Encontré algo de ayuda aquí:

http://answers.unity3d.com/questions/48836/determining-the-torque-needed-to-rotate-an-object.html

pero no exactamente Tengo 2 Quaternions, no puntos (?), Y ahora angularVelocity para rotar entre cada actualización física, y cosas como: rigidbody.inertiaTensorRotation rigidbody.inertiaTensor;

¿Hay alguna manera de calcular el torque que necesito agregar a mi object?

He intentado algo como (desde el enlace de arriba):

 var q : Quaternion = transform.rotation * rigidbody.inertiaTensorRotation; var T : Vector3 = q * Vector3.Scale(rigidbody.inertiaTensor, (Quaternion.Inverse(q) * angVelocity)); rigidbody.AddTorque(T * 0.01, ForceMode.Impulse); 

Pero parece que no funciona para mí, quiero decir que mi object no se detiene en el lugar correcto, sino que comienza a sacudirse al final del movimiento. Incluso cuando multiplico el Torque por una fracción y agrego AngularDrag a un alto valor para el cuerpo rígido.

¿Me puede ayudar con eso?