__author__ = 'thoth'

#
# demonstrate the "instability" of matrix decomposition into quaternions.
#

"""
http://blender.stackexchange.com/questions/28438/how-do-i-ensure-a-sequence-of-quaternions-from-matrix-decompose-is-continuous

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.

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).

"""

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)


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

    def stabilize(self, q):
        if self.old is None:
            rval = q
        else:
            # compute the distance between old and q
            d1 = (self.old-q).magnitude

            # compute the distance between old and -q
            d2 = (self.old+q).magnitude

            if (d1<d2):
                # q is closer to old
                rval = q
            else:
                # -q is closer to old
                rval = -q

        self.old = rval
        return rval

mission(bpy.context.active_object)