Thursday, September 22, 2011

Revisiting angular velocity from two quaternions

It recently occurred to me that my previous method was needlessly complex.

Given two quaternions: $q_0$ and $q_1$ separated by $t$ time, we can easily find the angular velocity by observing that there exists some quaternion that rotates $q_0$ into $q_1$ thus: $q_1 = r*q_0$.  We can find the angular velocity $\omega$ directly from $r$:
$$r = q_1 * q_0^{-1} = \left[\cos\left(\frac{\theta}{2}\right) \: \sin\left(\frac{\theta}{2}\right)[x \: y \: z]\right]$$
Where $[x \: y \: z]$ is the axis of rotation and $\theta$ is the amount.  So, after first finding $r$ through simple quaternion multiplication, we find $$\theta = 2\cos^{-1}\left(r_w\right)$$
Using <math.h> this gives us $\theta \in [0,2\pi]$. Consequently, if $\theta>\pi$, we subtract $2\pi$ so that $\theta \in (-\pi,\pi]$ to avoid going the long way around (unless, somehow, we know that you should go the long way based on the previous velocity).  If $\theta \ne 0$, then we have $$\omega = \frac{\theta}{t}\frac{\left[r_x \; r_y \; r_z \right]}{\sqrt{r_x^2 + r_y^2 + r_z^2}}$$
This method seems to be much simpler than my previous approach; however, it may be less numerically stable when the angular velocity is very small.