Tuesday, April 26, 2011

Angular velocity from two known orientations

Given two quaternions, $q_0$ $q_1$, representing the orientation of an object at two points in time, $t_0$ $t_1$, we want to find the constant angular velocity that accomplishes the given change.  I needed this because I need to simulate the trajectory of a kinematic rigid body through space (the body is kinematic because it follows a trajectory obtained through motion capture).  Since it's kinematic, I wouldn't normally need to actually give it velocities.  I could just place it in the required position and orientation at each point in time.  However, the kinematic object must also interact with dynamic objects in a simulated world (using the Open Dynamics Engine {ODE} to produce the simulation).  In order to produce correct interactions between kinematic objects and dynamic objects, the constraint solver must be informed ahead of time as to where the kinematic object is going.  Otherwise, if there is a joint between kinematic and dynamic objects and the kinematic one moves, the joint is broken and the constraint is violated; strange things happen to the simulation.  Friction doesn't work.  Rigid connections seem springy.  It's no good.

Perhaps this is not a tremendously common task, but a quick search shows that it comes up from time to time.  Unfortunately, at least when I was searching, a quick search didn't provide many answers. 

The most obvious solution is to assume that because velocity is change over time, all we need to do is take $\omega  = (q_1-q_0)/(t_1-t_0)$ (dropping the $w$ component from the equation).  This is the linear approximation.  It sort of works, as long as the changes are very small.  But as the changes increase in magnitude, it gets worse. 

Quaternion rotations exists on the unit hypersphere.  Consequently, you can't compute velocity by drawing a straight line through two points.  You need a line (geodesic) that follows the surface of the sphere.  To find the right angular velocity, I look to spherical linear interpolation or SLERP.  This is the function used in animation to smoothly interpolate from one quaternion rotation to another.  If we're interpolating from $q_0$ to $q_1$ over the interval $t\in[0,1]$, and $\theta=acos(q_0\cdot q_1)$ (the angle between the two points on the hypersphere), then $q(t) = \frac{\sin((1-t)\theta)}{\sin(\theta)}q_0 + \frac{\sin(t\theta)}{\sin(\theta)}q_1$.

If we take the derivative of SLERP with respect to time, we get:
$\frac{dq(t)}{dt} = \frac{\theta\cos(t\theta)}{\sin(\theta)}q_1-\frac{\theta\cos((1-t)\theta)}{\sin(\theta)}q_0$ (thanks to the convenient on-line derivative calculator at number empire).  If we plug in our values of $q_0$ $q_1$ and use $t=0$, then we get: $\frac{\theta}{\sin(\theta)}(q_1-\cos(\theta)q_0)$.  We can expect a lot of very small changes, but $\sin(0)=0$.  Fortunately, the function $sinc(x) = \frac{\sin(x)}{x}$ is actually continuous at $x=0$ with $sinc(0)=1$.  To avoid a numerically unstable divide-by-zero problem, we use a Taylor series approximation if $\theta<1e-4$ .  At that point, we actually only need the first two elements of the Taylor series to approximate at floating point double precision; so we're not losing anything.  Doing this isn't my idea.  I saw it used in ODE.  I'm sure they got it from somewhere too.  The inverse sinc function is also undefined at $\pi$, but with quaternions, we should always have $\theta\in[0,\frac{\pi}{2}]$.  If the angle is outside of that range, we should negate one of the quaternions.  Otherwise, we'll be trying to go the long way around the unit sphere to get from one orientation to the other. 

But this is still not an angular velocity.  To go from $\frac{dq}{dt}$ to $\omega$, we multiply with the inverse of $q_0$, drop the real component, and multiply by two over the amount of time that passes: $\omega = \frac{2}{t_1-t_0}\left(\frac{dq}{dt}q_0^{-1}\right)$ (non-real part).

Once we can correctly state the angular velocity of a motion-captured rigid body, we can make the simulated world respond appropriately to collision events.