How do I ensure a sequence of quaternions from matrix.decompose() is continuous?

Question

It seems that blender's Matrix decompose() function can exhibit some numerical "instability". What I mean is that orientation matrices that are relatively close can have significantly different elements in their quaternion matrix that makes interpolated keyframing unusable.

The following simple python script will calculate a sequence of matrices representing rotations around the Z axis in a smooth manner. It then decomposes the matrix into a quaternion, and inserts that as a keyframe. When you view the resulting fcurves in blender you can see that the Z value jumps from -1 to 1 about halfway through the animation.

import bpy
from math import *
from mathutils import *

def matrix_for_time(t):
    theta = 2 * pi * t
    mat = Matrix([[cos(theta), sin(theta), 0],
                  [-sin(theta), cos(theta), 0],
                  [0, 0, 1]]).to_4x4()
    # for illustration we use rotation about Z axis,
    # but in the arbitrary case, the orientation could be for a pine cone bouncing down a hill.
    return mat


def mission(obj):
    res = 36
    qs = QuaternionStabilizer()
    for z in range(res):
        mat = matrix_for_time(z/res)
        (loc,quat,scale) = mat.decompose()

        obj.rotation_quaternion = qs.stabilize(quat)
        for ai in range(len(quat)):
            obj.keyframe_insert(frame=z*5, data_path="rotation_quaternion", index=ai)


mission(bpy.context.active_object)

" How to make a true linear quaternion rotation? " has some screenshots that imply quaternions aren't necessarily discontinuous.

Is there a technique for blender's python API to get a sequence of quaternions that can be keyframed and interpolated without discontinuity? For bonus points: link to an article that explains this mathematical oddity and why decomposition works this way.

This technique should be usable with arbitrary orientation matrices, because they are calculated from something a little more complex than this simple Z rotation (in my specific case I'm flying along a bezier curve).

Answer 1

After doing a little more research I think I have found a technique that will work. Examining the matrices that are computed from the quaternion led me to conclude that q and -q both represent the same orientation, so I compare both q and -q to the previous quaternion orientation and pick whichever one is closest in 4-space. The following udpated code sample illustrates the technique:

import bpy
from math import *
from mathutils import *

def mission(obj):
    res = 36
    qs = QuaternionStabilizer()
    for z in range(res):
        theta = 2*pi *z / res
        mat = Matrix( [ [ cos(theta), sin(theta), 0],
                        [ -sin(theta), cos(theta), 0],
                        [ 0,0,1]]).to_4x4()
        (loc,quat,scale) = mat.decompose()

        obj.rotation_quaternion = qs.stabilize(quat)
        for ai in range(len(quat)):
            obj.keyframe_insert(frame=z*5, data_path="rotation_quaternion", index=ai)


class QuaternionStabilizer:
    def __init__(self):
        self.old=None

    def stabilize(self, q):
        if self.old is None:
            rval = q
        else:
            d1 = (self.old-q).magnitude
            d2 = (self.old+q).magnitude
            if (d1<d2):
                rval = q
            else:
                rval = -q
        self.old = rval
        return rval

mission(bpy.context.active_object)

And now my fcurves don't look discontinuous:

This will not give good results if your orientations are flailing about wildly, but that's a case of Garbage In Garbage Out.

Answer 2

I fiddled around with the Quaternion(axis, angle) constructor and Quaternion.slerp but both gave non-continous results as well. (Quaternion.slerp worked using a third value inbetween though)

But you might use the formula given on this wikipedia page:

cos(θ / 2) + sin(θ / 2) * (u_x * i + u_y * j + u_z * k)

where θ is the rotation angle and u = (u_x, u_y, u_z) the unit vector for the rotation axis.

import bpy
import math
from mathutils import Quaternion

obj = bpy.context.active_object
action = obj.animation_data.action

def from_axis_angle(axis, angle):
    half_angle = angle / 2.0
    scalar = math.cos(half_angle)
    factor = math.sin(half_angle)

    return Quaternion((
        scalar,
        axis[0] * factor,
        axis[1] * factor,
        axis[2] * factor
    ))

def set_rot_kf(frame, quat, action):
    for fcu in action.fcurves:
        if fcu.data_path == "rotation_quaternion":
            index = fcu.array_index
            fcu.keyframe_points.insert(frame, quat[index])

axis = (0.0, 0.0, 1.0)
angle = 2.0 * math.pi

frame_start = 0
frame_end = 60
dt = frame_end - frame_start
frac = 1 / dt

for frame in range(dt + 1): 
    quat = from_axis_angle(axis, frame * frac * angle)
    set_rot_kf(frame_start + frame, quat, action)

    #or
    #quat *= dq 
    #where
    #dq = from_axis_angle(axis, frac * angle)

The result looked like this, where you can see the graphs of cos(θ / 2) and sin(θ / 2) from 0 to 2π: