예제 #1
0
def rot_axis_quat(vector1, vector2):
    """ Find the rotation (quaternion) from vector 1 to vector 2"""
    vector1 = vector1.normalized()
    vector2 = vector2.normalized()
    cosTheta = vector1.dot(vector2)
    rotationAxis = Vector((0.0, 0.0, 0.0))
    if (cosTheta < -1 + 0.001):
        v = Vector((0.0, 1.0, 0.0))
        #Get the vector at the right angles to both
        rotationAxis = vector1.cross(v)
        rotationAxis = rotationAxis.normalized()
        q = Quaternion()
        q.w = 0.0
        q.x = rotationAxis.x
        q.y = rotationAxis.y
        q.z = rotationAxis.z
    else:
        rotationAxis = vector1.cross(vector2)
        s = math.sqrt((1.0 + cosTheta) * 2.0)
        invs = 1 / s
        q = Quaternion()
        q.w = s * 0.5
        q.x = rotationAxis.x * invs
        q.y = rotationAxis.y * invs
        q.z = rotationAxis.z * invs
    return q
def quaternion_from_two_vectors(dir, up):
    from io_scene_forsaken import forsaken_utils
    
    vU1 = dir
    vU2 = up
    
    vU1.normalize()
    vU2.normalize()
    
    vAxis = vU1.cross(vU2)

    fAxisMag = forsaken_utils.min(math.sqrt(vAxis.x*vAxis.x+vAxis.y*vAxis.y+vAxis.z*vAxis.z), 1.0)
    fTheta = math.asin(fAxisMag)
    fTheta_C = math.pi - fTheta
    
    if vU1.dot(vU2) < 0.0:
        fTheta = fTheta_C
        fTheta_C = math.pi - fTheta
        
    fEpsilon = 1e-7

    if fTheta < fEpsilon:
        qRot = Quaternion()
        qRot.x = 0.0
        qRot.y = 0.0
        qRot.z = 0.0
        qRot.w = 1.0
        return qRot
        
    if fTheta_C < fEpsilon:
        vCP = vU1.cross(Vector((1.0, 0.0, 0.0)))
        
        fOpposite = vCP.x*vCP.x+vCP.y*vCP.y+vCP.z*vCP.z
        
        if fOpposite >= fEpsilon:
            vAxis = vCP
        else:
            vAxis = Vector((0.0, 1.0, 0.0))
            
    vAxis.normalize()
    
    qRot = Quaternion()
    s = math.sin(fTheta * 0.5)
    c = math.cos(fTheta * 0.5)
    
    qRot.x = vAxis.x * s
    qRot.y = vAxis.y * s
    qRot.z = vAxis.z * s
    qRot.w = c
    
    qRot.normalize()
    return qRot
예제 #3
0
def get_quat(w=1.0, x=0.0, y=0.0, z=0.0):
    quat = Quaternion((0, 0, 0, 0))
    quat.w = w
    quat.x = x
    quat.y = y
    quat.z = z
    return quat
def decode(data, channel, scale):
    scaleFactor = float(1.0)

    if data.bit_count == 8:
        scaleFactor = 1 / float(16)

    result = [None] * channel.num_time_codes
    result[0] = data.initial_value

    for i, delta_block in enumerate(data.delta_blocks):
        deltaScale = scale * scaleFactor * DELTA_TABLE[delta_block.block_index]
        deltas = get_deltas(delta_block.delta_bytes, data.bit_count)

        for j, delta in enumerate(deltas):
            idx = int(i / channel.vector_len) * 16 + j + 1
            if idx >= channel.num_time_codes:
                break

            if channel.type == 6:
                # access quat as xyzw instead of wxyz
                index = (delta_block.vector_index + 1) % 4
                value = result[idx - 1][index] + deltaScale * delta
                if result[idx] is None:
                    result[idx] = Quaternion()
                    result[idx].w = result[idx - 1].w
                    result[idx].x = result[idx - 1].x
                    result[idx].y = result[idx - 1].y
                    result[idx].z = result[idx - 1].z
                result[idx][index] = value

            else:
                result[idx] = result[idx - 1] + deltaScale * delta

    return result
예제 #5
0
def read_quaternion(io_stream):
    quat = Quaternion((0, 0, 0, 0))
    quat.x = read_float(io_stream)
    quat.y = read_float(io_stream)
    quat.z = read_float(io_stream)
    quat.w = read_float(io_stream)
    return quat
예제 #6
0
def QuaternionLookRotation(fro, to, up):
	vector = (fro - to).normalized()
	vector2 = up.normalized().cross(vector).normalized()
	vector3 = vector.cross(vector2)
	m00 = vector2.x
	m01 = vector2.y
	m02 = vector2.z
	m10 = vector3.x
	m11 = vector3.y
	m12 = vector3.z
	m20 = vector.x
	m21 = vector.y
	m22 = vector.z

	num8 = (m00 + m11) + m22
	q = Quaternion()
	if num8 > 0:
		num = sqrt(num8 + 1)
		q.w = num * 0.5
		num = 0.5 / num
		q.x = (m12 - m21) * num
		q.y = (m20 - m02) * num
		q.z = (m01 - m10) * num
	elif m00 >= m11 and m00 >= m22:
		num7 = sqrt(((1 + m00) - m11) - m22)
		num4 = 0.5 / num7
		q.x = 0.5 * num7
		q.y = (m01 + m10) * num4
		q.z = (m02 + m20) * num4
		q.w = (m12 - m21) * num4
	elif m11 > m22:
		num6 = sqrt(((1 + m11) - m00) - m22)
		num3 = 0.5 / num6
		q.x = (m10+ m01) * num3
		q.y = 0.5 * num6
		q.z = (m21 + m12) * num3
		q.w = (m20 - m02) * num3
	else:
		num5 = sqrt(((1 + m22) - m00) - m11)
		num2 = 0.5 / num5
		q.x = (m20 + m02) * num2
		q.y = (m21 + m12) * num2
		q.z = 0.5 * num5
		q.w = (m01 - m10) * num2
	return q
예제 #7
0
def QuaternionLookRotation(fro, to, up):
    vector = (fro - to).normalized()
    vector2 = up.normalized().cross(vector).normalized()
    vector3 = vector.cross(vector2)
    m00 = vector2.x
    m01 = vector2.y
    m02 = vector2.z
    m10 = vector3.x
    m11 = vector3.y
    m12 = vector3.z
    m20 = vector.x
    m21 = vector.y
    m22 = vector.z

    num8 = (m00 + m11) + m22
    q = Quaternion()
    if num8 > 0:
        num = sqrt(num8 + 1)
        q.w = num * 0.5
        num = 0.5 / num
        q.x = (m12 - m21) * num
        q.y = (m20 - m02) * num
        q.z = (m01 - m10) * num
    elif m00 >= m11 and m00 >= m22:
        num7 = sqrt(((1 + m00) - m11) - m22)
        num4 = 0.5 / num7
        q.x = 0.5 * num7
        q.y = (m01 + m10) * num4
        q.z = (m02 + m20) * num4
        q.w = (m12 - m21) * num4
    elif m11 > m22:
        num6 = sqrt(((1 + m11) - m00) - m22)
        num3 = 0.5 / num6
        q.x = (m10 + m01) * num3
        q.y = 0.5 * num6
        q.z = (m21 + m12) * num3
        q.w = (m20 - m02) * num3
    else:
        num5 = sqrt(((1 + m22) - m00) - m11)
        num2 = 0.5 / num5
        q.x = (m20 + m02) * num2
        q.y = (m21 + m12) * num2
        q.z = 0.5 * num5
        q.w = (m01 - m10) * num2
    return q
예제 #8
0
def lerpq(value, bounds):
    ret = Quaternion()
    mult = Vector(bounds.mult)
    add = Vector(bounds.add)
    ret.x = add.x + value.x * mult.x
    ret.y = add.y + value.y * mult.y
    ret.z = add.z + value.z * mult.z
    ret.w = add.w + value.w * mult.w
    return ret
예제 #9
0
def qΔ由v一轴f二弧生成qLIB(v轴,f弧度):
    q=Quaternion();
    f半弧 = f弧度 *0.5;
    fSin半弧 = sin(f半弧);
    q.x = v轴.x * fSin半弧;
    q.y = v轴.y * fSin半弧;
    q.z = v轴.z * fSin半弧;
    q.w = cos(f半弧);
    return q;
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
예제 #11
0
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
예제 #12
0
def rotation_to(a, b):
    """Calculates shortest Quaternion from Vector a to Vector b"""
    # a - up vector
    # b - direction to point to

    # http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
    # https://github.com/toji/gl-matrix/blob/f0583ef53e94bc7e78b78c8a24f09ed5e2f7a20c/src/gl-matrix/quat.js#L54

    a = a.normalized()
    b = b.normalized()
    q = Quaternion()

    tmpvec3 = Vector()
    xUnitVec3 = Vector((1, 0, 0))
    yUnitVec3 = Vector((0, 1, 0))

    dot = a.dot(b)
    if (dot < -0.999999):
        # tmpvec3 = cross(xUnitVec3, a)
        tmpvec3 = xUnitVec3.cross(a)
        if (tmpvec3.length < 0.000001):
            tmpvec3 = yUnitVec3.cross(a)
        tmpvec3.normalize()
        # q = Quaternion(tmpvec3, Math.PI)
        q = Quaternion(tmpvec3, math.pi)
    elif (dot > 0.999999):
        q.x = 0
        q.y = 0
        q.z = 0
        q.w = 1
    else:
        tmpvec3 = a.cross(b)
        q.x = tmpvec3[0]
        q.y = tmpvec3[1]
        q.z = tmpvec3[2]
        q.w = 1 + dot
        q.normalize()
    return q
예제 #13
0
def decode(data, channel, scale):
    scaleFactor = float(1.0)

    if data.bitCount == 8:
        scaleFactor = 1 / float(16)

    result = [None] * channel.numTimeCodes
    result[0] = data.initialValue

    for i, deltaBlock in enumerate(data.deltaBlocks):
        blockScale = delta_table[deltaBlock.blockIndex]
        deltaScale = blockScale * scale * scaleFactor

        vectorIndex = deltaBlock.vectorIndex
        deltas = get_deltas(deltaBlock, data.bitCount)

        for j, delta in enumerate(deltas):
            idx = int(i / channel.vectorLen) * 16 + j + 1
            if idx >= channel.numTimeCodes:
                break

            if channel.type == 6:
                # access quat as xyzw instead of wxyz
                index = (vectorIndex + 1) % 4
                value = result[idx - 1][index] + deltaScale * delta
                if (result[idx] == None):
                    result[idx] = Quaternion()
                    result[idx].w = result[idx - 1].w
                    result[idx].x = result[idx - 1].x
                    result[idx].y = result[idx - 1].y
                    result[idx].z = result[idx - 1].z
                result[idx][index] = value

            else:
                result[idx] = result[idx - 1] + deltaScale * delta

    return result
예제 #14
0
def _arc_segment(v_1, v_2):
    ELorigin = bpy.context.scene.objects['ELorigin']
    ELground = bpy.context.scene.objects['ELground']

    v = v_2 - v_1
    d = v.length

    ELorigin.location = Vector((0, 0, 0))
    ELground.location = Vector((0, 0, -d))

    v_L = ELground.location - ELorigin.location

    q = Quaternion()
    c = Vector.cross(v_L, v)
    q.x = c.x
    q.y = c.y
    q.z = c.z
    q.w = sqrt((v_L.length ** 2) * (v.length ** 2)) + \
     Vector.dot(v_L, v)
    q.normalize()
    euler = q.to_euler()

    bpy.ops.object.runfslg_operator()

    laALL = bpy.context.scene.objects['laALL']
    laALL.name = 'lARC'
    laALL.rotation_euler = euler
    laALL.location = v_1

    bpy.context.active_object.select = False
    laALL.select = True
    bpy.ops.object.transform_apply(location=True, rotation=True, scale=True)
    laALL.select = False
    bpy.context.active_object.select = True

    return laALL
예제 #15
0
def _arc_segment(v_1, v_2):
	ELorigin = bpy.context.scene.objects['ELorigin']
	ELground = bpy.context.scene.objects['ELground']

	v = v_2 - v_1
	d = v.length

	ELorigin.location = Vector((0, 0, 0))
	ELground.location = Vector((0, 0, -d))

	v_L = ELground.location - ELorigin.location

	q = Quaternion()
	c = Vector.cross(v_L, v)
	q.x = c.x
	q.y = c.y
	q.z = c.z
	q.w = sqrt((v_L.length ** 2) * (v.length ** 2)) + \
		Vector.dot(v_L, v)
	q.normalize()
	euler = q.to_euler()

	bpy.ops.object.runfslg_operator()

	laALL = bpy.context.scene.objects['laALL']
	laALL.name = 'lARC'
	laALL.rotation_euler = euler
	laALL.location = v_1

	bpy.context.active_object.select = False
	laALL.select = True
	bpy.ops.object.transform_apply(location=True, rotation=True, scale=True)
	laALL.select = False
	bpy.context.active_object.select = True	

	return laALL
def orientacion(escena, objeto):
    """
    Parameters
    ----------
    escena : bpy.scene
        Escena de nuestro entorno.
    objeto : bpy.context.object
        Objeto de nuestra escena.

    Returns
    -------
    q : Quaternion
        Cuaternion de rotación y lateral.

    """
    e = bpy.context.object.AnimSettings.eje_inclinacion
    e_l = bpy.context.object.AnimSettings.eje_lateral

    if e == "ejeX+":
        e = Vector((1, 0, 0))
    elif e == "ejeX-":
        e = Vector((-1, 0, 0))
    elif e == "ejeY+":
        e = Vector((0, 1, 0))
    elif e == "ejeY-":
        e = Vector((0, -1, 0))
    elif e == "ejeZ+":
        e = Vector((0, 0, 1))
    elif e == "ejeZ-":
        e = Vector((0, 0, -1))

    if e_l == "ejeX+":
        e_l = Vector((1, 0, 0))
    elif e_l == "ejeX-":
        e_l = Vector((-1, 0, 0))
    elif e_l == "ejeY+":
        e_l = Vector((0, 1, 0))
    elif e_l == "ejeY-":
        e_l = Vector((0, -1, 0))
    elif e_l == "ejeZ+":
        e_l = Vector((0, 0, 1))
    elif e_l == "ejeZ-":
        e_l = Vector((0, 0, -1))

    # Alinear el objeto con la tangente
    f_actual = escena.frame_current
    f_anterior = f_actual - 1

    pos_act = Vector(objeto.location)

    escena.frame_set(f_anterior)
    pos_ant = Vector(objeto.location)

    t = (pos_act - pos_ant).normalized()

    vec = Vector.cross(e, t).normalized()

    c1 = clamp(e.dot(t), -1, 1)

    theta = acos(c1)

    s = cos(theta / 2.0)
    w = sin(theta / 2.0) * vec

    q1 = Quaternion()
    q1.w = s
    q1.x = w[0]
    q1.y = w[1]
    q1.z = w[2]

    # Inclinar el coche lateralmente
    e_l_rotate = Vector(e_l)
    e_l_rotate.rotate(q1)

    z = Vector((0, 0, 1))
    l = t.cross(z)
    l.normalize()

    c2 = clamp(e_l_rotate.dot(l), -1, 1)

    theta_l = acos(c2)

    if e_l_rotate[2] < 0:
        theta_l = -theta_l

    s = cos(theta_l / 2.0)
    w = sin(theta_l / 2.0) * t

    q2 = Quaternion()
    q2.w = s
    q2.x = w[0]
    q2.y = w[1]
    q2.z = w[2]

    if bpy.context.object.AnimSettings.inclinacion_bool == True:
        inclinacion = bpy.context.object.AnimSettings.inclinacion
        theta_i = math.pi * inclinacion / 180
        s = cos(theta_i / 2.0)
        w = sin(theta_i / 2.0) * e

        q3 = Quaternion()
        q3.w = s
        q3.x = w[0]
        q3.y = w[1]
        q3.z = w[2]

        q = q2 @ q1 @ q3
    else:
        q = q2 @ q1

    return q
예제 #17
0
def read_bones(self, context, filepath, root):

    skeleton_node = root.find("Skeleton")
    if (skeleton_node == None):
        return None, None

    bones = []
    bones_tag = []
    flags_list = []
    # LimitRotation and Unk0 have their special meanings, can be deduced if needed when exporting
    flags_restricted = set(["LimitRotation", "Unk0"])
    drawable_name = root.find("Name").text.split(".")[0]
    bones_node = skeleton_node.find("Bones")
    armature = context.object
    bpy.ops.object.mode_set(mode='EDIT')

    for bones_item in bones_node:
        name_item = bones_item.find("Name")
        tag_item = bones_item.find("Tag")
        parentindex_item = bones_item.find("ParentIndex")
        flags_item = bones_item.find("Flags")
        translation_item = bones_item.find("Translation")
        rotation_item = bones_item.find("Rotation")
        scale_item = bones_item.find("Scale")

        quaternion = Quaternion()
        quaternion.w = float(rotation_item.attrib["w"])
        quaternion.x = float(rotation_item.attrib["x"])
        quaternion.y = float(rotation_item.attrib["y"])
        quaternion.z = float(rotation_item.attrib["z"])
        mat_rot = quaternion.to_matrix().to_4x4()

        trans = Vector()
        trans.x = float(translation_item.attrib["x"])
        trans.y = float(translation_item.attrib["y"])
        trans.z = float(translation_item.attrib["z"])
        mat_loc = Matrix.Translation(trans)

        scale = Vector()
        scale.x = float(scale_item.attrib["x"])
        scale.y = float(scale_item.attrib["y"])
        scale.z = float(scale_item.attrib["z"])
        mat_sca = Matrix.Scale(1, 4, scale)

        edit_bone = armature.data.edit_bones.new(name_item.text)
        # edit_bone.bone_id = int(bone_tag.attrib["value"])
        if parentindex_item.attrib["value"] != "-1":
            edit_bone.parent = armature.data.edit_bones[int(
                parentindex_item.attrib["value"])]

        # https://github.com/LendoK/Blender_GTA_V_model_importer/blob/master/importer.py
        edit_bone.head = (0, 0, 0)
        edit_bone.tail = (0, 0.05, 0)
        edit_bone.matrix = mat_loc @ mat_rot @ mat_sca
        if edit_bone.parent != None:
            edit_bone.matrix = edit_bone.parent.matrix @ edit_bone.matrix

        if (flags_item != None and flags_item.text != None):
            flags = flags_item.text.strip().split(", ")

        flags_list.append(flags)

        # build a bones lookup table
        bones.append(name_item.text)
        bones_tag.append(int(tag_item.get('value')))

    bpy.ops.object.mode_set(mode='POSE')

    for i in range(len(bones)):
        armature.pose.bones[i].bone.bone_properties.tag = bones_tag[i]
        for _flag in flags_list[i]:
            if (_flag in flags_restricted):
                continue

            flag = armature.pose.bones[i].bone.bone_properties.flags.add()
            flag.name = _flag

    bpy.ops.object.mode_set(mode='OBJECT')
    return bones, drawable_name
예제 #18
0
def step(scene):
    global dt
    sub_steps = 1#*scene.jiggle.sub_steps)
    dt = 1.0/(scene.render.fps*sub_steps)

    for o in scene.objects:
        if(o.type == 'ARMATURE'):

            ow = o.matrix_world.copy()
            scale = ow.col[0].length

            children_of_bone = defaultdict(list)
            for bone in o.pose.bones:
                if bone.parent is not None:
                    children_of_bone[bone.parent].append(bone)

            for _ in range( sub_steps):
                bl = []

                for b in o.pose.bones:
                    if(b.parent==None):
                        propB(ow,b,bl,None,children_of_bone)

                bl2 = []
                for wb in bl:
                    b = wb.b
                    # o------ -> ---o---
                    wb.restW = b.bone.matrix_local.copy() * scale
                    wb.Q = wb.Q.normalized()

                    if(b.bone.jiggle.enabled):
                    #(wb.parent.restW.inverted() * wb.restW) #
                        Jb = b.bone.jiggle
                        wb.rest =  b.bone.matrix_local #
                        if(b.parent!=None):
                            wb.rest = b.bone.parent.matrix_local.inverted() @ wb.rest
                        wb.Kc = 0
                        if(Jb.control_bone!=""):
                            if(Jb.control_bone in o.pose.bones):
                                cb = o.pose.bones[Jb.control_bone]
                                clm = cb.matrix
                                if(cb.parent!=None):
                                    clm = cb.parent.matrix.inverted() @ clm
                                wb.cQ = clm.to_quaternion().normalized()
                                wb.Kc = 1- pow(1-Jb.control, 1/scene.jiggle.iterations)


                        wb.rest_base = wb.rest.copy()
                        wb.rest.translation = wb.rest.translation * scale
                        wb.length = b.bone.length*scale
                        wb.irest = wb.rest.inverted()
                        wb.w = 1.0/Jb.mass
                        wb.k = 1- pow(1-Jb.Ks, 1/scene.jiggle.iterations)
                        Jb.V*= 1.0-Jb.Kld
                        Jb.V+= scene.gravity*dt
                        Jb.W*= 1.0-Jb.Kd
                        R = Jb.R.to_matrix()
                        wb.R = R.normalized()
                        wb.P = Jb.P.copy()
                        wb.Cx = wb.sample(0.5)
                        qv = Quaternion()
                        qv.x = Jb.W[0]
                        qv.y = Jb.W[1]
                        qv.z = Jb.W[2]
                        qv.w = 0
                        cv = wb.Cx + Jb.V*dt
                        wb.Q = qadd(wb.Q, [email protected]*dt*0.5).normalized()    #newton's first law
                        wb.P += cv - wb.sample(0.5)#the same

                        bl2.append(wb)


                for i in range(scene.jiggle.iterations):
                    for wb in bl2:
                        b = wb.b
                        if(b.parent==None):
                            continue
                        Jb = b.bone.jiggle
                        Pc =  wb.P
                        target_m = [email protected]
                        Pt = target_m.translation.copy()
                        if(Jb.debug in scene.objects):
                            scene.objects[Jb.debug].location = Pc
                        W = wb.w + wb.parent.w
                        I = (Pc-Pt)/W

                        wb.applyImpulse(Pc,-I)
                        wb.parent.applyImpulse(Pt,I)

                #for i in range(scene.jiggle.iterations):
                    for wb in bl2:
                        b = wb.b
                        if(b.parent==None):
                            continue
                        Jb = b.bone.jiggle
                        quatSpring(wb)
                        if(wb.Kc>0.0):
                            quatSpring(wb,wb.cQ, wb.Kc)
                for wb in bl2:
                    b = wb.b
                    if(b.parent==None):
                        continue
                    Jb = b.bone.jiggle
                    wb.P = [email protected]
                    wb.R = wb.Q.normalized().to_matrix()


                for wb in bl2:
                    b = wb.b
                    Jb = b.bone.jiggle
                    R = Jb.R.to_matrix()
                    Jb.V = (wb.sample(0.5) - wb.Cx)/dt
                    Jb.P = wb.P.copy()
                    wb.Q = wb.Q.normalized()
                    qv = [email protected]() #qadd(wb.Q,-Jb.R)*Jb.R.conjugated()#
                    Jb.W = Vector((qv.x,qv.y,qv.z))*(2/dt)
                    Jb.R = wb.Q

                for wb in bl:
                    wb.R*=scale
                for wb in bl2:
                    b = wb.b
                    pM = ow
                    if(b.parent!=None):
                        pM = wb.parent.M
                    b.matrix_basis = ([email protected]_base).inverted()@wb.M

    scene.jiggle.last_frame+= 1