Quaternion dfference + time --> angular velocity (gyroscope in physics library)
- by AndrewK
I am using Bullet Physic library to program some function, where I have difference between orientation from gyroscope given in quaternion and orientation of my object, and time between each frame in milisecond. All I want is set the orientation from my gyroscope to orientation of my object in 3D space. But all I can do is set angular velocity to my object. I have orientation difference and time, and from that I calculate vector of angular velocity [Wx,Wy,Wz] from that formula: W(t) = 2 * dq(t)/dt * conj(q(t))
My code is:
btQuaternion diffQuater = gyroQuater - boxQuater;
btQuaternion conjBoxQuater = gyroQuater.inverse();
btQuaternion velQuater = ((diffQuater * 2.0f) / d_time) * conjBoxQuater;
And everything works well, till I get:
1 rotating around Y axis, angle about 60 degrees, then I have these values in 2 critical frames:
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 rotating around X axis, angle about 120 degrees, then I have these values in 2 critical frames:
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 rotating around Z axis, angle about 120 degrees, then I have these values in 2 critical frames:
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
The problem is the most visible in diffQuater - euler angles vector. Can someone tell me why it is like that? and how to solve that problem?
All suggestions are welcome.