def _create_bones(self, bone, parent):
        abone = self._armature.edit_bones.new(bone.name)
        abone.tail = Vector([0, 1, 0])

        if parent:
            abone.parent = parent

        rot_part = Quaternion(bone.rotation).inverted().to_matrix()
        pos_part = Vector(bone.position)
        transform = Matrix.Translation(bone.position) * rot_part.to_4x4()

        if parent:
            transform = parent.matrix * transform
            rot_part = transform.to_3x3()
            pos_part = transform.to_translation()

        # abone.transform(transform)
        abone.transform(rot_part)
        abone.translate(pos_part)

        nrm_mtx = transform.to_3x3()
        nrm_mtx.invert()
        nrm_mtx.transpose()

        for vi in range(0, bone.vertex_count):
            vt_ind = vi + bone.vertex_index
            self._file.vertices[vt_ind] = list(transform * Vector(self._file.vertices[vt_ind]))
            self._file.normals[vt_ind] = list(nrm_mtx * Vector(self._file.normals[vt_ind]))

        for child in bone.children:
            self._create_bones(child, abone)

def rotateCam(rift):
    
    cont = G.getCurrentController()
    owner = cont.owner
    
    scene = G.getCurrentScene()
 
    rift.poll()
        
    rotation = Quaternion((rift.rotation[0], 
        rift.rotation[1], 
        rift.rotation[2], 
        rift.rotation[3]))

    eu = rotation.to_euler()
    
    #ativecam
    fix = Euler((-1.57, 0, 3*1.57), 'XYZ')
    rot = Euler((-eu.z, eu.y, -eu.x), 'XYZ')
    
    #owner
    #fix = Euler((0, 2*-1.57, -1.57), 'XYZ')
    #rot = Euler((-eu.x, eu.z, eu.y), 'XYZ')
    
    rot.rotate(fix)
     
    #cam = scene.active_camera
    cam = scene.cameras["Camera"]
    cam.localOrientation = rot

    def test_to_expmap(self):
        q = Quaternion((0, 0, 1), math.radians(90))

        e = q.to_exponential_map()
        self.assertAlmostEqual(e.x, 0)
        self.assertAlmostEqual(e.y, 0)
        self.assertAlmostEqual(e.z, math.radians(90), 6)

def loadMhpFile(context, filepath):
    ob = context.object
    rig = ob.parent
    scn = context.scene
    if rig and rig.type == 'ARMATURE':
        (pname, ext) = os.path.splitext(filepath)
        mhppath = pname + ".mhp"
        
        fp = open(mhppath, "rU")
        for line in fp:
            words = line.split()
            if len(words) < 5:
                continue
            elif words[1] == "quat":
                q = Quaternion((float(words[2]), float(words[3]), float(words[4]), float(words[5])))
                mat = q.to_matrix().to_4x4()
                pb = rig.pose.bones[words[0]]
                pb.matrix_basis = mat
            elif words[1] == "gquat":
                q = Quaternion((float(words[2]), float(words[3]), float(words[4]), float(words[5])))
                mat = q.to_matrix().to_4x4()
                maty = mat[1].copy()
                matz = mat[2].copy()
                mat[1] = -matz
                mat[2] = maty
                pb = rig.pose.bones[words[0]]
                pb.matrix_basis = pb.bone.matrix_local.inverted() * mat
        fp.close()
        print("Mhp file %s loaded" % mhppath)

    def test_expmap_axis_normalization(self):
        q = Quaternion((1, 1, 0), 2)
        e = q.to_exponential_map()

        self.assertAlmostEqual(e.x, 2 * math.sqrt(0.5), 6)
        self.assertAlmostEqual(e.y, 2 * math.sqrt(0.5), 6)
        self.assertAlmostEqual(e.z, 0)

def loadTPose(rig, filename):
    if filename:
        filepath = os.path.join(os.path.dirname(__file__), filename)
        filepath = os.path.normpath(filepath)
        print("Loading %s" % filepath)
        struct = loadJson(filepath)
        rig.McpTPoseFile = filename
    else:
        return False

    unit = Matrix()
    for pb in rig.pose.bones:
        pb.matrix_basis = unit

    for name,value in struct:
        bname = getBoneName(rig, name)
        try:
            pb = rig.pose.bones[bname]
        except KeyError:
            continue
        quat = Quaternion(value)
        pb.matrix_basis = quat.to_matrix().to_4x4()
        setBoneTPose(pb, quat)

    rig.McpTPoseLoaded = True
    rig.McpRestTPose = False
    return True

    def execute(self, context):
        scn = context.scene
        if 'localgrid_menu_items_strings' in scn and \
           'localgrid_menu_items_float' in scn:
            strings_dict = scn['localgrid_menu_items_strings']
            float_dict = scn['localgrid_menu_items_float']
            for i in range(len(strings_dict.keys())):
                strings = strings_dict[str(i)]
                icon, name = strings.split(',', 1)
                ls = float_dict[str(i)]
                orig = Vector([ls[0], ls[1], ls[2]])
                quat = Quaternion([ls[3], ls[4], ls[5], ls[6]])


                item = scn.local_grid.items.add()
                item.item_name = name
                item.icon = icon
                item.orig = orig
                item.quat = quat.inverted()
        if 'localgrid_menu_items_strings' in scn:
            del(scn['localgrid_menu_items_strings'])
        if 'localgrid_menu_items_float' in scn:
            del(scn['localgrid_menu_items_float'])
        if hasattr(scn, 'localgrid_menu_items_strings'):
            del(scn.localgrid_menu_items_strings)
        if hasattr(scn, 'localgrid_menu_items_float'):
            del(scn.localgrid_menu_items_float)
        return {'FINISHED'}

 def execute(self, context):
     selected = bpy.context.selected_objects
     obj = selected[-1]
     surf = bpy.context.scene.objects['surface']
     
     loc = bpy.context.scene.cursor_location
     
     bvh = BVHTree.FromObject(surf, bpy.context.scene)
     
     loc = surf.matrix_world.inverted() * loc
     (loc, normal, index, dist) = bvh.find_nearest(loc)
     if self.use_smooth:
         normal = smooth_normal(surf, loc, index)
     loc = surf.matrix_world * loc
     
     bpy.ops.object.duplicate()
     new_obj = bpy.context.selected_objects[-1]
     
     (unused, surf_rot, unused) = surf.matrix_world.decompose()
     (unused, obj_rot, scale) = obj.matrix_world.decompose()
     
     normal = surf_rot * normal
     vec = obj_rot * Vector((0.0, 0.0, 1.0))
     
     q = vec.rotation_difference(normal)
     q = Quaternion().slerp(q, self.align_with_normal)
     mat_scale = Matrix()
     for i in range(3): mat_scale[i][i] = scale[i]
     new_obj.matrix_world = (Matrix.Translation(loc) *
         q.to_matrix().to_4x4() * obj_rot.to_matrix().to_4x4() *
         mat_scale)
     
     bpy.context.scene.objects.active = new_obj
     
     return {'FINISHED'}

def calc_pose_mats(iqmodel, iqpose, bone_axis):
	loc_pose_mat = [None] * len(iqmodel.bones)
	abs_pose_mat = [None] * len(iqmodel.bones)
	recalc = False

	# convert pose to local matrix and compute absolute matrix
	for n in range(len(iqmodel.bones)):
		iqbone = iqmodel.bones[n]

		pose_pos = iqpose[n].translate
		pose_rot = iqpose[n].rotate
		pose_scale = iqpose[n].scale

		local_pos = Vector(pose_pos)
		local_rot = Quaternion((pose_rot[3], pose_rot[0], pose_rot[1], pose_rot[2]))
		local_scale = Vector(pose_scale)

		mat_pos = Matrix.Translation(local_pos)
		mat_rot = local_rot.to_matrix().to_4x4()
		mat_scale = Matrix.Scale(local_scale.x, 3).to_4x4()
		loc_pose_mat[n] = mat_pos * mat_rot * mat_scale

		if iqbone.parent >= 0:
			abs_pose_mat[n] = abs_pose_mat[iqbone.parent] * loc_pose_mat[n]
		else:
			abs_pose_mat[n] = loc_pose_mat[n]

	# Remove negative scaling from bones.
	# Due to numerical instabilities in blender's matrix <-> head/tail/roll math
	# this isn't always stable when the bones are in the X axis. If the bones
	# end up rotated 90 degrees from what they should be, that's the reason.
	for n in range(len(iqmodel.bones)):
		if abs_pose_mat[n].is_negative:
			if not hasattr(iqmodel, 'abs_bind_mat'):
				print("warning: removing negative scale in bone", iqmodel.bones[n].name)
			abs_pose_mat[n] = abs_pose_mat[n] * Matrix.Scale(-1, 4)
			recalc = True

	# flip bone axis (and recompute local matrix if needed)
	if bone_axis == 'X':
		axis_flip = Matrix.Rotation(math.radians(-90), 4, 'Z')
		abs_pose_mat = [m * axis_flip for m in abs_pose_mat]
		recalc = True
	if bone_axis == 'Z':
		axis_flip = Matrix.Rotation(math.radians(-90), 4, 'X')
		abs_pose_mat = [m * axis_flip for m in abs_pose_mat]
		recalc = True

	if recalc:
		inv_pose_mat = [m.inverted() for m in abs_pose_mat]
		for n in range(len(iqmodel.bones)):
			iqbone = iqmodel.bones[n]
			if iqbone.parent >= 0:
				loc_pose_mat[n] = inv_pose_mat[iqbone.parent] * abs_pose_mat[n]
			else:
				loc_pose_mat[n] = abs_pose_mat[n]

	return loc_pose_mat, abs_pose_mat

def quats_to_matrix(qx, qy, qz, qw, tx, ty, tz): # pylint: disable=invalid-name
    """
    Converts the quaternions and the translation into a 4-dimensional matrix
    """
    # this is straight up math, nothing to "graps" or "understand". Var names are practical
    # pylint: disable=invalid-name
    mat = Quaternion((qx, qy, qz, qw)).to_matrix().to_4x4()
    mat.translation = Vector((tx, ty, tz))
    return mat

    def test_from_expmap(self):
        e = Vector((1, 1, 0))
        q = Quaternion(e)
        axis, angle = q.to_axis_angle()

        self.assertAlmostEqual(angle, math.sqrt(2), 6)
        self.assertAlmostEqual(axis.x, math.sqrt(0.5), 6)
        self.assertAlmostEqual(axis.y, math.sqrt(0.5), 6)
        self.assertAlmostEqual(axis.z, 0)

def change_to_scs_quaternion_coordinates(rot):
    """Transposes quaternion rotation from Blender to SCS game engine.

    :param rot: Blender quaternion (or four floats)
    :type rot: Quaternion | list | tuple
    :return: Transposed quaternion rotation
    :rtype: Quaternion
    """
    quat = Quaternion((rot[0], rot[1], rot[2], rot[3]))
    return (scs_to_blend_matrix().inverted() * quat.to_matrix().to_4x4() * scs_to_blend_matrix()).to_quaternion()

def angle_between_nor(nor_orig, nor_result):
    angle = math.acos(nor_orig.dot(nor_result))
    axis = nor_orig.cross(nor_result).normalized()

    q = Quaternion()
    q.x = axis.x * math.sin(angle / 2)
    q.y = axis.y * math.sin(angle / 2)
    q.z = axis.z * math.sin(angle / 2)
    q.w = math.cos(angle / 2)

    return q

 def set_LRS(self, context, obj, LRS, rotation_mode='QUATERNION'):
     L, R, S = LRS
     L_mode, R_mode, S_mode, to_m, persp = self.calc_matrix(context, obj)
     
     mL = (L is not None) and (L_mode != 'BASIS')
     mR = (R is not None) and (R_mode != 'BASIS')
     mS = (S is not None) and (S_mode != 'BASIS')
     
     if mL or mR or mS:
         in_m = matrix_inverted_safe(to_m) * BlUtil.Object.matrix_world(obj)
         
         if not mL:
             in_L = in_m.to_translation()
         else:
             L = Vector(L) # make sure it's a Vector
             if L_mode in ('CAMERA', 'VIEW'):
                 L = Vector((L.x / persp.x, L.y / persp.y, -L.z)).lerp(
                     Vector((L.x * L.z / persp.x, L.y * L.z / persp.y, -L.z)), persp.z)
             in_L = L
             L = None
         
         if not mR:
             in_R = in_m.to_quaternion()
             if not R: rotation_mode = obj.rotation_mode
         else:
             if rotation_mode == 'QUATERNION':
                 in_R = Quaternion(R)
             elif rotation_mode == 'AXIS_ANGLE':
                 in_R = Quaternion(R[1:], R[0])
             else:
                 if (len(R) == 4): R = R[1:]
                 in_R = Euler(R).to_quaternion()
             R = None
         
         if not mS:
             in_S = in_m.to_scale()
         else:
             in_S = Vector(S)
             S = None
         
         x, y, z = in_R.normalized().to_matrix().col
         in_m = matrix_compose(x*in_S.x, y*in_S.y, z*in_S.z, in_L)
         BlUtil.Object.matrix_world_set(obj, to_m * in_m)
         
         if (not mL) and (not L): L = Vector(obj.location)
         if (not mR) and (not R): R = BlUtil.Object.rotation_convert(obj.rotation_mode, obj.rotation_quaternion,
             obj.rotation_axis_angle, obj.rotation_euler, rotation_mode)
         if (not mS) and (not S): S = Vector(obj.scale)
     
     if L: obj.location = Vector(L)
     if R: BlUtil.Object.rotation_apply(obj, R, rotation_mode)
     if S: obj.scale = Vector(S)

	def compute_pose(self, world_poses, local_rotations, joint_offsets, index):
		"""
		Compute a pose for an index. Returns the resulting mat4
		"""

		parent_quat = Quaternion([1, 0, 0, 0])
		parent_pose = Matrix()

		if index > 0:
			parent_quat = local_rotations[index - 1]
			parent_pose = world_poses[index - 1]

		local_pose = (parent_quat.inverted() * local_rotations[index]).to_matrix()
		local_pose.resize_4x4()
		return parent_pose * joint_offsets[index] * local_pose

def setTPose(context):
    rig = context.object
    scn = context.scene
    if not rig.McpHasTPose:
        print(("%s has no defined T-pose" % rig))

    quat = Quaternion((1,0,0,0))
    mat = quat.to_matrix().to_4x4()
    for pb in rig.pose.bones:
        try:
            qw = pb["McpRestW"]
        except KeyError:
            continue
        pb.matrix_basis = mat
    print("Set T-pose")

def clearTPose(context):
    rig = context.object
    scn = context.scene
    if not rig.McpHasTPose:
        print(("%s has no defined T-pose" % rig))

    for pb in rig.pose.bones:
        try:
            qw = pb["McpRestW"]
            qx = pb["McpRestX"]
            qy = pb["McpRestY"]
            qz = pb["McpRestZ"]
        except KeyError:
            continue
        quat = Quaternion((qw,qx,qy,qz))
        pb.matrix_basis = quat.to_matrix().to_4x4()
    print("Cleared T-pose")

def bake_path_offsets(context, cu_path, ob, action, specials):
    """ bake path offsets into an action """
    channels = get_bone_channels(action)
    channels = topmost_level(channels, ob, specials)
    limits = (int(action.frame_range[0]), 2  + int(action.frame_range[1]))
    values = evaluate_curves(channels, limits)
    zero_offset = get_path_offset(context, cu_path, ob, 0).copy()
    for bone, groups in channels.items():

        for data_path, curves in groups.items():
            data = [(cu.data_path, cu.array_index, cu.group.name) for cu in curves]
            while curves:
                cu = curves.pop(-1)
                action.fcurves.remove(cu)
            for datum in data:
                cu = action.fcurves.new(datum[0], datum[1], datum[2])
                curves.append(cu)
    for frame in range(limits[0], limits[1]):
        context.scene.frame_set(frame)
        current_offset = ob.matrix_world
        print(ob.name, current_offset.to_translation() , zero_offset.to_translation())
        for bone, groups in channels.items():
            for transforms in 'location', 'rotation_quaternion':
                if 'location' in groups:
                    old_loc = values[bone]['location'][frame - limits[0]]
                else:
                    old_loc = Vector((0,0,0))
                if 'rotation_quaternion' in groups:
                    old_rot = Quaternion(values[bone]['rotation_quaternion'][frame - limits[0]])
                else:
                    old_rot = Quaternion((1, 0, 0, 0))
            old_trans = Matrix.Translation(old_loc).to_4x4() * old_rot.to_matrix().to_4x4()
            rest_mat = ob.data.bones[bone].matrix_local
            old_trans_world = current_offset * rest_mat * old_trans
            new_trans =\
                rest_mat.inverted() * zero_offset.inverted() * old_trans_world
            new_loc, new_rot, sca = new_trans.decompose()
            for group, curves in groups.items():
                for array_index, curve in enumerate(curves):
                    if curve.data_path.endswith('location'):
                        insert_keyframe_curve(
                            curve, frame, new_loc[array_index], 'LINEAR')
                    else:
                        insert_keyframe_curve(
                            curve, frame, new_rot[array_index], 'LINEAR')

def createTPose(context):
    rig = context.object
    scn = context.scene
    if rig.McpHasTPose:
        setTPose(context)
        return

    filepath = os.path.join(os.path.dirname(__file__), "t_pose.json")
    struct = loadJson(filepath)

    for name,value in struct:
        pb = rig.pose.bones[name]
        quat = Quaternion(value)
        pb.matrix_basis = quat.to_matrix().to_4x4()
        rest = quat.inverted()
        pb["McpRestW"] = rest.w
        pb["McpRestX"] = rest.x
        pb["McpRestY"] = rest.y
        pb["McpRestZ"] = rest.z

    children = []
    for ob in scn.objects:
        if ob.type != 'MESH':
            continue
        for mod in ob.modifiers:
            if (mod.type == 'ARMATURE' and
                mod.object == rig):
                children.append((ob, mod.name))
                scn.objects.active = ob
                bpy.ops.object.modifier_apply(apply_as='SHAPE', modifier=mod.name)
                ob.data.shape_keys.key_blocks[mod.name].value = 1

    scn.objects.active = rig
    bpy.ops.pose.armature_apply()
    for ob,name in children:
        scn.objects.active = ob
        mod = ob.modifiers.new(name, 'ARMATURE')
        mod.object = rig
        mod.use_vertex_groups = True
        bpy.ops.object.modifier_move_up(modifier=name)
        setShapeKey(ob, name, 1.0)

    scn.objects.active = rig
    rig.McpHasTPose = True
    print("Created T-pose")

 def applied_influence(matrix, group_matrix=None):
     q = matrix.to_3x3().to_quaternion()
     loc = matrix.to_translation()
     if self.mode == 'A_TO_B':
         q0 = Quaternion([1, 0, 0, 0])
         loc0 = Vector()
     else:
         q0 = group_matrix.to_quaternion()
         if self.transform_mode == 'TRANSLATE':
             q = q0.copy()
         if self.transform_mode in {'ALL', 'TRANSLATE'}:
             loc0 = group_matrix.to_translation()
         else:
             loc0 = Vector()
     q = q * self.influence + q0 * (1.0 - self.influence)
     loc = loc * self.influence + loc0 * (1.0 - self.influence)
     mat = q.to_matrix().to_4x4()
     mat.col[3][:3] = loc
     return mat