# Quaternion dfference + time --> angular velocity (gyroscope in physics library)

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.

-

Ahh! Seeing the values, now I see what's going on.

The big subtlety with quaternions is that every rotation has two different representations by quaternions; if q is the quaternion representation of a given rotation, then -q represents the same rotation. For whatever reason, as you transition across the thresholds you've indicated, your gyro quaternion is 'flipping' — it's still representing the same rotation but it's using the other quaternion to do so. You can see that while it's now radically different from boxQuater, it's very very close to the negative of boxQuater; blindly taking the difference between the two, then, is resulting in a huge quaternion rather than the very small difference you should see.

The fix, fortunately, is straightforward; since boxQuater and gyroQuater are both (approximately) unit quaternions and since both are supposed to be very near each other, their dot product should be very close to 1. Try computing the dot product of the two before executing your delta and angular-velocity code. If you find the dot product to be close to -1 (in fact, if it's negative at all), then simply negate gyroQuater — replace it by -gyroQuater — before you compute your value for diffQuater.

-
Now it works perfectly :) Thank you very much!!!! – AndrewK Jun 20 '12 at 23:54

Thank you guys for the last help, but I moved to PhysX instead of Bullet, and now I have to calculate Torque instead of AngularVelocity. Unfortunately that is more complicated than last one for me :(

I've found some help in here:

but not exactly. I have 2 Quaternions, not points(?), and now angularVelocity to rotate between each physic update, and things like: rigidbody.inertiaTensorRotation rigidbody.inertiaTensor;

Is there some way to calculate torque which I need to add to my object?

I have tried something like (from above link):

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

But it seems not work for me, I mean my object does not stop in right place, but starts to jiggle at the end of the move. Even when I multiply Torque by some fraction and add AngularDrag to hight value to rigidbody.

Can you help me with that?

-
I've set angularDrag to 20, and now it behave well, but is there some way to optimize calculations? – AndrewK Aug 8 '12 at 20:42
I strongly suggest asking a new question for this - it really is an independent question from the one you've already asked, and the answers are likely to be independent. – Steven Stadnicki Aug 9 '12 at 1:07

You are computing the derivative of a quaternion function using `(q(t+Δt) - q(t)) / Δt`. This may lead to accuracy issues because the orientation difference between two quaternions would be better represented by dividing two quaternions.

Unlike usual functions, where the values of the derivative can be summed over time in order to get the real value of the function, unit quaternions are multiplied over time.

So if you have two quaternions for `q(t)` and `q(t+Δt)`, an approximation of the derivative better than `(q(t+Δt) - q(t)) / Δt` is `exp(log(q(t+Δt) / q(t)) / Δt)`.

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

If you do not have exponential and logarithm functions for your quaternion class, you may implement them using the standard definitions from Wikipedia.

Edit: reword to make it clear the initial code was not really incorrect

-
Sam, I finally have that code and it works well: btQuaternion diffQuater = gyroQuater - boxQuater; btQuaternion conjBoxQuater = boxQuater.inverse(); btQuaternion velQuater = ((diffQuater * 2.0f) / d_time) * conjBoxQuater; Do you suggest your idea is better (more accurately/simpler/faster) ? – AndrewK Jun 21 '12 at 0:10
This isn't strictly true. The derivative of a quaternion function doesn't necessarily carry direct physical meaning in terms of a rotation, the way that the derivative of a position function carries direct velocity information, but that doesn't mean that the concept - or the classical approximation of the derivative - is any less valid for all that. Using slerp-style techniques works better for larger values of delta t, but the 'classical' approximation is perfectly valid and the mathematics does work out correctly. – Steven Stadnicki Jun 21 '12 at 0:12
@StevenStadnicki You're totally right. I agree lerp will give reasonable results compared to slerp (though it'd really need normalisation at some point) but depending on where the quaternions come from, a sudden change of sign may be harmful. I'll reword my answer, though. – sam hocevar Jun 21 '12 at 0:40
@AndrewK to make it short, my idea is certainly more accurate, probably not simpler (unless you have `exp` and `log` readily available), and definitely slower. – sam hocevar Jun 21 '12 at 0:41
@SamHocevar Yeah - as I suggest in my own answer, I'm pretty sure it's the change-of-sign issue that's biting him here, but flipping one of the quats is an easy fix for that particular issue. – Steven Stadnicki Jun 21 '12 at 0:45